diff --git a/3rdparty/rbdl/.editorconfig b/3rdparty/rbdl/.editorconfig new file mode 100644 index 0000000..195d343 --- /dev/null +++ b/3rdparty/rbdl/.editorconfig @@ -0,0 +1,10 @@ +root = true + +[*] +end_of_line = lf +indent_style = space +indent_size = 2 +charset = utf-8 + +[Makefile] +indent_style = tab diff --git a/3rdparty/rbdl/.hg_archival.txt b/3rdparty/rbdl/.hg_archival.txt new file mode 100644 index 0000000..f0a6d1a --- /dev/null +++ b/3rdparty/rbdl/.hg_archival.txt @@ -0,0 +1,6 @@ +repo: d536c26f2a238e943cf08a038d1134c782b1e66c +node: ee446c22d4fbfb51b29d2d708472a958e2eb38b6 +branch: dev +latesttag: v2.5.0 +latesttagdistance: 43 +changessincelatesttag: 103 diff --git a/3rdparty/rbdl/.hgignore b/3rdparty/rbdl/.hgignore new file mode 100644 index 0000000..2535234 --- /dev/null +++ b/3rdparty/rbdl/.hgignore @@ -0,0 +1,38 @@ +syntax: glob + +doc/html/* +doc/notes/*.aux +doc/notes/*.pdf + +*Debug/* +*Release/* +*RelWithDebInfo/* +*build/* + +.*.swp +CMakeFiles/* +cmake_install.cmake +CMakeCache.txt +Makefile +tags +*.log + +*.user + +src/rbdl_config.h + +# autogenerated by python/wrappergen.py +python/rbdl.pyx + +examples/simple/CMakeCache.txt +examples/simple/example +examples/simple/librbdl.so + +examples/luamodel/example_luamodel +examples/luamodel/CMakeCache.txt + +*.vcxproj +*.vcxproj.filters +*.suo +*.db +*.sln diff --git a/3rdparty/rbdl/.hgtags b/3rdparty/rbdl/.hgtags new file mode 100644 index 0000000..5e6787a --- /dev/null +++ b/3rdparty/rbdl/.hgtags @@ -0,0 +1,14 @@ +7a77f7a994d813d91742f89563c274d6fd04c061 v1.0.0 +e753e34715b107aad90b8aa7dd985cca2ff01d53 v1.1.0 +e0f64ac8491140fdc9e4499b55d7eadbc093b229 v2.0.0 +7742057b61fb0094c3e437294cd48c596913ad4d v2.0.1 +70d7265f4d36fa5aba907879f939ba658fe58bd3 v2.1.0 +0f2b7a0fd90a5391d9bb5a2399b1a4fe44893018 v2.2.0 +57ec58a73e0948727b96092cb1ea948fdcae4bd0 v2.2.1 +39707193a22c11ea671ce1b981b8dc9b4da406b9 v2.2.2 +2da137bfd1fd7f2e14294aff6e3feeffebfac952 v2.3.1 +8c9139fa7142b8370c0c004abb86ce13d0984aa3 v2.3.2 +7cfc6e93e10135c7fe8e89736160f0322932d440 v2.3.3 +48c0872b39d6fd9d085c1c5c6ff46f7125a17f9d v2.4.0 +9d527cef3bb1dcb8b57b861251fc14ab99a93372 v2.4.1 +37918477608938f4c1e90eedf1a525c69533d843 v2.5.0 diff --git a/3rdparty/rbdl/CMake/FindCython.cmake b/3rdparty/rbdl/CMake/FindCython.cmake new file mode 100644 index 0000000..04aed1f --- /dev/null +++ b/3rdparty/rbdl/CMake/FindCython.cmake @@ -0,0 +1,44 @@ +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_EXECUTABLE +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if( PYTHONINTERP_FOUND ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + HINTS ${_python_path} + ) +else() + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + ) +endif() + + +include( FindPackageHandleStandardArgs ) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE ) + +mark_as_advanced( CYTHON_EXECUTABLE ) diff --git a/3rdparty/rbdl/CMake/FindEigen3.cmake b/3rdparty/rbdl/CMake/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/CMake/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - 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 + +# 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 + 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) diff --git a/3rdparty/rbdl/CMake/FindUnitTest++.cmake b/3rdparty/rbdl/CMake/FindUnitTest++.cmake new file mode 100644 index 0000000..dac8410 --- /dev/null +++ b/3rdparty/rbdl/CMake/FindUnitTest++.cmake @@ -0,0 +1,40 @@ +# - Try to find UnitTest++ +# +# + +SET (UNITTEST++_FOUND FALSE) + +FIND_PATH (UNITTEST++_INCLUDE_DIR UnitTest++.h + /usr/include/unittest++ + /usr/local/include/unittest++ + /opt/local/include/unittest++ + $ENV{UNITTESTXX_PATH}/src + $ENV{UNITTESTXX_INCLUDE_PATH} + ) + +FIND_LIBRARY (UNITTEST++_LIBRARY NAMES UnitTest++ PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + $ENV{UNITTESTXX_PATH} + ENV{UNITTESTXX_LIBRARY_PATH} + ) + +IF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY) + SET (UNITTEST++_FOUND TRUE) +ENDIF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY) + +IF (UNITTEST++_FOUND) + IF (NOT UnitTest++_FIND_QUIETLY) + MESSAGE(STATUS "Found UnitTest++: ${UNITTEST++_LIBRARY}") + ENDIF (NOT UnitTest++_FIND_QUIETLY) +ELSE (UNITTEST++_FOUND) + IF (UnitTest++_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find UnitTest++") + ENDIF (UnitTest++_FIND_REQUIRED) +ENDIF (UNITTEST++_FOUND) + +MARK_AS_ADVANCED ( + UNITTEST++_INCLUDE_DIR + UNITTEST++_LIBRARY + ) diff --git a/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake b/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake new file mode 100644 index 0000000..69e638d --- /dev/null +++ b/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake @@ -0,0 +1,4 @@ +# Note: when executed in the build dir, then CMAKE_CURRENT_SOURCE_DIR is the +# build dir. +file( COPY src test bin DESTINATION "${CMAKE_ARGV3}" + FILES_MATCHING PATTERN "*.py" ) diff --git a/3rdparty/rbdl/CMake/UseCython.cmake b/3rdparty/rbdl/CMake/UseCython.cmake new file mode 100644 index 0000000..7eaa770 --- /dev/null +++ b/3rdparty/rbdl/CMake/UseCython.cmake @@ -0,0 +1,295 @@ +# Define a function to create Cython modules. +# +# For more information on the Cython project, see http://cython.org/. +# "Cython is a language that makes writing C extensions for the Python language +# as easy as Python itself." +# +# This file defines a CMake function to build a Cython Python module. +# To use it, first include this file. +# +# include( UseCython ) +# +# Then call cython_add_module to create a module. +# +# cython_add_module( ... ) +# +# To create a standalone executable, the function +# +# cython_add_standalone_executable( [MAIN_MODULE src1] ... ) +# +# To avoid dependence on Python, set the PYTHON_LIBRARY cache variable to point +# to a static library. If a MAIN_MODULE source is specified, +# the "if __name__ == '__main__':" from that module is used as the C main() method +# for the executable. If MAIN_MODULE, the source with the same basename as +# is assumed to be the MAIN_MODULE. +# +# Where is the name of the resulting Python module and +# ... are source files to be compiled into the module, e.g. *.pyx, +# *.py, *.c, *.cxx, etc. A CMake target is created with name . This can +# be used for target_link_libraries(), etc. +# +# The sample paths set with the CMake include_directories() command will be used +# for include directories to search for *.pxd when running the Cython complire. +# +# Cache variables that effect the behavior include: +# +# CYTHON_ANNOTATE +# CYTHON_NO_DOCSTRINGS +# CYTHON_FLAGS +# +# Source file properties that effect the build process are +# +# CYTHON_IS_CXX +# +# If this is set of a *.pyx file with CMake set_source_files_properties() +# command, the file will be compiled as a C++ file. +# +# See also FindCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Configuration options. +set( CYTHON_ANNOTATE OFF + CACHE BOOL "Create an annotated .html file when compiling *.pyx." ) +set( CYTHON_NO_DOCSTRINGS OFF + CACHE BOOL "Strip docstrings from the compiled module." ) +set( CYTHON_FLAGS "" CACHE STRING + "Extra flags to the cython compiler." ) +mark_as_advanced( CYTHON_ANNOTATE CYTHON_NO_DOCSTRINGS CYTHON_FLAGS ) + +find_package( Cython REQUIRED ) +find_package( PythonLibs REQUIRED ) + +set( CYTHON_CXX_EXTENSION "cxx" ) +set( CYTHON_C_EXTENSION "c" ) + +# Create a *.c or *.cxx file from a *.pyx file. +# Input the generated file basename. The generate file will put into the variable +# placed in the "generated_file" argument. Finally all the *.py and *.pyx files. +function( compile_pyx _name generated_file ) + # Default to assuming all files are C. + set( cxx_arg "" ) + set( extension ${CYTHON_C_EXTENSION} ) + set( pyx_lang "C" ) + set( comment "Compiling Cython C source for ${_name}..." ) + + set( cython_include_directories "" ) + set( pxd_dependencies "" ) + set( c_header_dependencies "" ) + set( pyx_locations "" ) + + foreach( pyx_file ${ARGN} ) + get_filename_component( pyx_file_basename "${pyx_file}" NAME_WE ) + + # Determine if it is a C or C++ file. + get_source_file_property( property_is_cxx ${pyx_file} CYTHON_IS_CXX ) + if( ${property_is_cxx} STREQUAL "TRUE" ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython CXX source for ${_name}..." ) + endif() + + # Get the include directories. + get_source_file_property( pyx_location ${pyx_file} LOCATION ) + get_filename_component( pyx_path ${pyx_location} PATH ) + get_directory_property( cmake_include_directories DIRECTORY ${pyx_path} INCLUDE_DIRECTORIES ) + list( APPEND cython_include_directories ${cmake_include_directories} ) + list( APPEND pyx_locations "${pyx_location}" ) + + # Determine dependencies. + # Add the pxd file will the same name as the given pyx file. + unset( corresponding_pxd_file CACHE ) + find_file( corresponding_pxd_file ${pyx_file_basename}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} + NO_DEFAULT_PATH ) + if( corresponding_pxd_file ) + list( APPEND pxd_dependencies "${corresponding_pxd_file}" ) + endif() + + # pxd files to check for additional dependencies. + set( pxds_to_check "${pyx_file}" "${pxd_dependencies}" ) + set( pxds_checked "" ) + set( number_pxds_to_check 1 ) + while( ${number_pxds_to_check} GREATER 0 ) + foreach( pxd ${pxds_to_check} ) + list( APPEND pxds_checked "${pxd}" ) + list( REMOVE_ITEM pxds_to_check "${pxd}" ) + + # check for C header dependencies + file( STRINGS "${pxd}" extern_from_statements + REGEX "cdef[ ]+extern[ ]+from.*$" ) + foreach( statement ${extern_from_statements} ) + # Had trouble getting the quote in the regex + string( REGEX REPLACE "cdef[ ]+extern[ ]+from[ ]+[\"]([^\"]+)[\"].*" "\\1" header "${statement}" ) + unset( header_location CACHE ) + find_file( header_location ${header} PATHS ${cmake_include_directories} ) + if( header_location ) + list( FIND c_header_dependencies "${header_location}" header_idx ) + if( ${header_idx} LESS 0 ) + list( APPEND c_header_dependencies "${header_location}" ) + endif() + endif() + endforeach() + + # check for pxd dependencies + + # Look for cimport statements. + set( module_dependencies "" ) + file( STRINGS "${pxd}" cimport_statements REGEX cimport ) + foreach( statement ${cimport_statements} ) + if( ${statement} MATCHES from ) + string( REGEX REPLACE "from[ ]+([^ ]+).*" "\\1" module "${statement}" ) + else() + string( REGEX REPLACE "cimport[ ]+([^ ]+).*" "\\1" module "${statement}" ) + endif() + list( APPEND module_dependencies ${module} ) + endforeach() + list( REMOVE_DUPLICATES module_dependencies ) + # Add the module to the files to check, if appropriate. + foreach( module ${module_dependencies} ) + unset( pxd_location CACHE ) + find_file( pxd_location ${module}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} NO_DEFAULT_PATH ) + if( pxd_location ) + list( FIND pxds_checked ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( FIND pxds_to_check ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( APPEND pxds_to_check ${pxd_location} ) + list( APPEND pxd_dependencies ${pxd_location} ) + endif() # if it is not already going to be checked + endif() # if it has not already been checked + endif() # if pxd file can be found + endforeach() # for each module dependency discovered + endforeach() # for each pxd file to check + list( LENGTH pxds_to_check number_pxds_to_check ) + endwhile() + endforeach() # pyx_file + + # Set additional flags. + if( CYTHON_ANNOTATE ) + set( annotate_arg "--annotate" ) + endif() + + if( CYTHON_NO_DOCSTRINGS ) + set( no_docstrings_arg "--no-docstrings" ) + endif() + + if( "${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR + "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo" ) + set( cython_debug_arg "--gdb" ) + endif() + + if( "${PYTHONLIBS_VERSION_STRING}" MATCHES "^2." ) + set( version_arg "-2" ) + elseif( "${PYTHONLIBS_VERSION_STRING}" MATCHES "^3." ) + set( version_arg "-3" ) + else() + set( version_arg ) + endif() + + # Include directory arguments. + list( REMOVE_DUPLICATES cython_include_directories ) + set( include_directory_arg "" ) + foreach( _include_dir ${cython_include_directories} ) + set( include_directory_arg ${include_directory_arg} "-I" "${_include_dir}" ) + endforeach() + + # Determining generated file name. + set( _generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.${extension}" ) + set_source_files_properties( ${_generated_file} PROPERTIES GENERATED TRUE ) + set( ${generated_file} ${_generated_file} PARENT_SCOPE ) + + list( REMOVE_DUPLICATES pxd_dependencies ) + list( REMOVE_DUPLICATES c_header_dependencies ) + + # Add the command to run the compiler. + add_custom_command( OUTPUT ${_generated_file} + COMMAND ${CYTHON_EXECUTABLE} + ARGS ${cxx_arg} ${include_directory_arg} ${version_arg} + ${annotate_arg} ${no_docstrings_arg} ${cython_debug_arg} ${CYTHON_FLAGS} + --output-file ${_generated_file} ${pyx_locations} + DEPENDS ${pyx_locations} ${pxd_dependencies} + IMPLICIT_DEPENDS ${pyx_lang} ${c_header_dependencies} + COMMENT ${comment} + ) + + # Remove their visibility to the user. + set( corresponding_pxd_file "" CACHE INTERNAL "" ) + set( header_location "" CACHE INTERNAL "" ) + set( pxd_location "" CACHE INTERNAL "" ) +endfunction() + +# cython_add_module( src1 src2 ... srcN ) +# Build the Cython Python module. +function( cython_add_module _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + foreach( _file ${ARGN} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + list( APPEND pyx_module_sources ${_file} ) + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + compile_pyx( ${_name} generated_file ${pyx_module_sources} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + python_add_module( ${_name} ${generated_file} ${other_module_sources} ) + if( APPLE ) + set_target_properties( ${_name} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" ) + else() + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ) + endif() +endfunction() + +include( CMakeParseArguments ) +# cython_add_standalone_executable( _name [MAIN_MODULE src3.py] src1 src2 ... srcN ) +# Creates a standalone executable the given sources. +function( cython_add_standalone_executable _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + set( main_module "" ) + cmake_parse_arguments( cython_arguments "" "MAIN_MODULE" "" ${ARGN} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + foreach( _file ${cython_arguments_UNPARSED_ARGUMENTS} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + get_filename_component( _file_we ${_file} NAME_WE ) + if( "${_file_we}" STREQUAL "${_name}" ) + set( main_module "${_file}" ) + elseif( NOT "${_file}" STREQUAL "${cython_arguments_MAIN_MODULE}" ) + set( PYTHON_MODULE_${_file_we}_static_BUILD_SHARED OFF ) + compile_pyx( "${_file_we}_static" generated_file "${_file}" ) + list( APPEND pyx_module_sources "${generated_file}" ) + endif() + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + + if( cython_arguments_MAIN_MODULE ) + set( main_module ${cython_arguments_MAIN_MODULE} ) + endif() + if( NOT main_module ) + message( FATAL_ERROR "main module not found." ) + endif() + get_filename_component( main_module_we "${main_module}" NAME_WE ) + set( CYTHON_FLAGS ${CYTHON_FLAGS} --embed ) + compile_pyx( "${main_module_we}_static" generated_file ${main_module} ) + add_executable( ${_name} ${generated_file} ${pyx_module_sources} ${other_module_sources} ) + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ${pyx_module_libs} ) +endfunction() diff --git a/3rdparty/rbdl/CMakeLists.txt b/3rdparty/rbdl/CMakeLists.txt new file mode 100644 index 0000000..a8e6c4d --- /dev/null +++ b/3rdparty/rbdl/CMakeLists.txt @@ -0,0 +1,214 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +CMAKE_POLICY(SET CMP0048 NEW) + +SET ( RBDL_VERSION_MAJOR 2 ) +SET ( RBDL_VERSION_MINOR 5 ) +SET ( RBDL_VERSION_PATCH 0 ) +SET ( RBDL_VERSION + ${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH} +) +SET ( RBDL_SO_VERSION + ${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH} +) +SET (PROJECT_VERSION ${RBDL_VERSION}) +PROJECT (RBDL VERSION ${RBDL_VERSION}) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_BINARY_DIR}/include +) + +# SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") + +INCLUDE(GNUInstallDirs) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +# Set a default build type to 'Release' if none was specified +IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + MESSAGE(STATUS "Setting build type to 'Release' as none was specified.") + SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + SET_PROPERTY(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +ENDIF() + +# Find and use the system's Eigen3 library +FIND_PACKAGE (Eigen3 3.0.0) + +IF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + MESSAGE (WARNING "Could not find Eigen3 on your system. Install it or use the slower SimpleMath library by enabling RBDL_USE_SIMPLE_MATH.") +ENDIF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + +IF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + INCLUDE_DIRECTORIES (${EIGEN3_INCLUDE_DIR}) +ENDIF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + +# Options +SET (RBDL_BUILD_STATIC_DEFAULT OFF) +IF (MSVC) + SET (RBDL_BUILD_STATIC_DEFAULT ON) +ENDIF (MSVC) + +OPTION (RBDL_BUILD_STATIC "Build statically linked library (otherwise dynamiclly linked)" ${RBDL_BUILD_STATIC_DEFAULT}) +OPTION (RBDL_BUILD_TESTS "Build the test executables" OFF) +OPTION (RBDL_ENABLE_LOGGING "Enable logging (warning: major impact on performance!)" OFF) +OPTION (RBDL_USE_SIMPLE_MATH "Use slow math instead of the fast Eigen3 library (faster compilation)" OFF) +OPTION (RBDL_STORE_VERSION "Enable storing of version information in the library (requires build from valid repository)" OFF) +OPTION (RBDL_BUILD_ADDON_URDFREADER "Build the (experimental) urdf reader" OFF) +OPTION (RBDL_BUILD_ADDON_BENCHMARK "Build the benchmarking tool" OFF) +OPTION (RBDL_BUILD_ADDON_LUAMODEL "Build the lua model reader" OFF) +OPTION (RBDL_BUILD_PYTHON_WRAPPER "Build experimental python wrapper" OFF) +OPTION (RBDL_BUILD_ADDON_GEOMETRY "Build the geometry library" OFF) +OPTION (RBDL_BUILD_ADDON_MUSCLE "Build the muscle library" OFF) + +# Addons +IF (RBDL_BUILD_ADDON_URDFREADER) + ADD_SUBDIRECTORY ( addons/urdfreader ) +ENDIF (RBDL_BUILD_ADDON_URDFREADER) + +IF (RBDL_BUILD_ADDON_BENCHMARK) + ADD_SUBDIRECTORY ( addons/benchmark ) +ENDIF (RBDL_BUILD_ADDON_BENCHMARK) + +IF (RBDL_BUILD_ADDON_LUAMODEL) + ADD_SUBDIRECTORY ( addons/luamodel ) +ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + +IF(RBDL_BUILD_ADDON_MUSCLE) + SET(RBDL_BUILD_ADDON_GEOMETRY ON CACHE BOOL "Build the geometry library" FORCE) + ADD_SUBDIRECTORY ( addons/muscle ) + IF(RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( addons/muscle/tests ) + ENDIF(RBDL_BUILD_TESTS) +ENDIF(RBDL_BUILD_ADDON_MUSCLE) + + +IF(RBDL_BUILD_ADDON_GEOMETRY) + ADD_SUBDIRECTORY ( addons/geometry ) + IF(RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( addons/geometry/tests ) + ENDIF(RBDL_BUILD_TESTS) +ENDIF(RBDL_BUILD_ADDON_GEOMETRY) + + + +IF (RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( tests ) +ENDIF (RBDL_BUILD_TESTS) + +# Source files for RBDL +SET ( RBDL_SOURCES + src/rbdl_version.cc + src/rbdl_mathutils.cc + src/rbdl_utils.cc + src/Constraints.cc + src/Dynamics.cc + src/Logging.cc + src/Joint.cc + src/Model.cc + src/Kinematics.cc + ) + +IF (MSVC AND NOT RBDL_BUILD_STATIC) + MESSAGE (FATAL_ERROR, "Compiling RBDL as a DLL currently not supported. Please enable RBDL_BUILD_STATIC.") +ENDIF (MSVC AND NOT RBDL_BUILD_STATIC) + +# Static / dynamic builds +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} ) + IF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib") + ENDIF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES OUTPUT_NAME "rbdl") + + IF (RBDL_BUILD_ADDON_LUAMODEL) + TARGET_LINK_LIBRARIES ( rbdl-static + rbdl_luamodel-static + ) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + INSTALL (TARGETS rbdl-static + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + INSTALL (TARGETS rbdl + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +IF (RBDL_STORE_VERSION) + # Set versioning information that can be queried during runtime + EXEC_PROGRAM("hg" ${CMAKE_CURRENT_SOURCE_DIR} ARGS "id -i" + OUTPUT_VARIABLE RBDL_BUILD_REVISION) + EXEC_PROGRAM("hg" ${CMAKE_CURRENT_SOURCE_DIR} ARGS "branch" + OUTPUT_VARIABLE RBDL_BUILD_BRANCH) + SET (RBDL_BUILD_TYPE ${CMAKE_BUILD_TYPE}) +ELSE (RBDL_STORE_VERSION) + SET (RBDL_BUILD_REVISION "unknown") + SET (RBDL_BUILD_BRANCH "unknown") + SET (RBDL_BUILD_TYPE "unknown") +ENDIF (RBDL_STORE_VERSION) + +CONFIGURE_FILE ( + "${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/rbdl_config.h.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h" + ) + +# Python wrapper +IF (RBDL_BUILD_PYTHON_WRAPPER) + add_subdirectory ( python ) + +ENDIF (RBDL_BUILD_PYTHON_WRAPPER) + +# Installation +FILE ( GLOB headers + ${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/*.h + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h + ) + +INSTALL ( FILES ${headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl ) + +# Setup of SimpleMath install settings +IF (RBDL_USE_SIMPLE_MATH) + INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/SimpleMath" + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl + ) +ENDIF (RBDL_USE_SIMPLE_MATH) + +# pkg-config +CONFIGURE_FILE ( + ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pc.cmake + ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pc @ONLY + ) +INSTALL ( + FILES ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig + ) + +# Packaging +SET(CPACK_GENERATOR "DEB") +SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Martin Felis ") +SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "the Rigid Body Dynamics Library (RBDL)") +SET(CPACK_PACKAGE_VENDOR "Martin Felis") +SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") +SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +SET(CPACK_PACKAGE_VERSION_MAJOR ${RBDL_VERSION_MAJOR}) +SET(CPACK_PACKAGE_VERSION_MINOR ${RBDL_VERSION_MINOR}) +SET(CPACK_PACKAGE_VERSION_PATCH ${RBDL_VERSION_PATCH}) +SET(CPACK_PACKAGE_INSTALL_DIRECTORY "CPACK_PACKAGE ${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}") +SET(CPACK_PACKAGE_FILE_NAME "rbdl-${CMAKE_LIBRARY_ARCHITECTURE}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") +SET(CPACK_PACKAGE_EXECUTABLES "rbdl_luamodel_util;RBDL Lua Model Utility") + +INCLUDE(CPack) diff --git a/3rdparty/rbdl/CONTRIBUTING.md b/3rdparty/rbdl/CONTRIBUTING.md new file mode 100644 index 0000000..ca9429b --- /dev/null +++ b/3rdparty/rbdl/CONTRIBUTING.md @@ -0,0 +1,209 @@ +# Coding style for RBDL + +This documents gives an overview of the coding style used in RBDL and also +the general goals of RBDL. + +If you are considering contributing to this library please read the whole +document. + +## General Purpose of RBDL + +RBDL implements large parts of the algorithms and methods described in +Featherstone's book Rigid Body Dynamics Algorithms. One of the main goals +of this library is to serve as an efficient implementation for the methods +described in Featherstone's book and to a lesser extent general multibody +dynamics and kinematics computations. A person who is familiar with +Featherstone's book should have an easy time to understand the code and +therefore the variable naming conventions used in the book should when +possible used in the code, e.g.: + + - the joint space inertia matrix is denoted with H + - the coriolis forces are denoted with C + +The algorithmic parts of RBDL's code try to follow the algorithmic or +mathematical notations instead of wrapping algorithms in elegant +programming patterns. + +### Aims and Non-Aims of RBDL + +This is what RBDL aims to be: + +* RBDL aims to be lean +* RBDL aims to be easily integrated into other projects +* RBDL aims to be suitable as a foundation for sophisticated dynamics packages +* RBDL gives you access to its internals and provides only a thin abstraction layer over the actual computation +* RBDL aims to be self-contained and dependant on as few libraries as possible + +And this is what RBDL is ***not*** about: + +* RBDL is ***not*** a fully fledged simulator with collision detection or fancy graphics +* RBDL does not keep you from screwing up things. + +Multibody dynamics is a complicated subject and in this codebase the +preference is mathematical and algorithmic clarity over elegant software +architecture. + +## Licensing + +RBDL is published under the very permissive zlib license that gives you a +lot of freedom in the use of full library or parts of it. The core part +of the library is solely using this license but addons may use different +licenses. + +There is no formal contributor license agreement for this project. Instead +when you submit patches or create a pull request it is assumed that you +have the rights to transfer the corresponding code to the RBDL project and +that you are okay that the code will be published as part of RBDL. + +## Data Storage + +RBDL tries to avoid dynamic allocations and prefers contiguous memory +storage such as in ```std::vector```s over possibly fragmented memory as in +```std::list``` or heap allocated tree structures. + +Where possible we use the Structure-of-Arrays (SOA) approach to store data, +e.g. the velocities v of all bodies is stored in an array (```std::vector```) +of ```SpatialVector```s in the ```Model``` structure. + +## Naming Conventions + +1. Structs and classes use CamelCase, e.g. ```ConstraintSet``` +2. Struct and class members use the lowerCamelCase convention, e.g. + ```Model::dofCount```. + Exceptions are: + 1. The member variable is a mathematical symbol in an + algorithm reference, E.g. ```S``` is commonly used to denote the joint + motion subspace, then we use the algorithm notation. For mathematical + symbols we also allow the underscore ```_``` to denote a subscript. + 2. Specializations of existing variables may be prefixed with an identifier, + followed by a underscore. E.g. ```Model::S``` is the default storage for + joint motion subspaces, however for the specialized 3-DOF joints it uses + the prefix ```multdof3_``` and are therefore stored in + ```Model::multdof3_S```. +3. Only the first letter of an acronym is using a capital letter, e.g. + degree of freedom (DOF) would be used as ```jointDofCount```, or + ```dofCount```. +4. Variables that are not member variables use the ```snake_case``` convention. + +### Examples + + struct Model { + std::vector v; // ok, v is an + std::vector S; // ok, S is commonly used in a reference algorithm + std::vector u; // ok + std::vector multdof3_u; // ok, 3-dof specialization of Model::u + + std::vector mJointIndex; // NOT OK: invalid prefix + unsigned int DOFCount; // NOT OK: only first letter of abbreviation should be in upper case + double error_tol; // NOT OK: use camelCase instead of snake_case + void CalcPositions(); // NOT OK: camelCase for member variables and function must start with lower-case name + + }; + +## Spacing and Line Width + +We use spaces to indent code and use two spaces to indent a block. Do not +use tabs. Namespaces should not be indented. + +Lines should not exceed 80 characters in width. + +Hint: in the root directory of the RBDL repository you find the file +.editorconfig which uses the correct spacing settings automatically for +many editors (see http://editorconfig.org/ for more details). + +## Error Handling + +RBDL will fail loudly and abort if an error occurs. This allows you to spot +errors early on. RBDL does not use exceptions. + +Code must compile without warnings with all compiler warnings enabled. +Please also consider checking code with static code analyzers such as +clang-analyzer (http://clang-analyzer.llvm.org/). + +## Const Correctness + +This code uses const correctness, i.e. parameters that are not expected to +change must be specified as const. Use const references whenever possible. +For some optional variables we use pointers, but when possible use +references. + +## Braces + +Use braces whenever possible. E.g. even there is only a single line of code +after an if-statement wrap it with curly braces. The opening brace starts +in the same line as the ```if``` or the ```else``` statement. + +## Documentation + +Most importantly the code should be readable to someone who is familiar +with multibody dynamics, especially with Featherstone's notation. The +documentation should mainly serve to clarify the API in terms of doxygen +comments. Within the code itself comments may be used to emphasize on ideas +behind it or to clarify sections. But in general it is best to write +readable code in the first place as comments easily become deprecated. + +The doxygen comments should be written in the header files and not in the +```.cc``` files. + +## Testing + +All code contributions must provide unit tests. RBDL uses UnitTest++ +(https://github.com/unittest-cpp/unittest-cpp) as a testing framework. Many +small tests that check single features are preferred over large tests that +test multiple things simultaneously. + +Bugfixes ideally come with a test case that reproduce the bug. + +## Branching and Bookmarks + +RBDL uses Mercurial (https://mercurial-scm.org) as version control system. +The branching concept of mercurial is different than in git. Git's +branching concept is captured in mercurial using bookmarks +(https://www.mercurial-scm.org/wiki/Bookmarks). + +The ```default``` branch is reserved for releases. Bugfixes and +contributions happen in the ```dev``` branch but should have a bookmark +assigned to them. + +Please do not create new branches, i.e. do not run ```hg branch +```. + +### Working on a new feature + +The following steps are advised when working on a new feature for RBDL: + +1. Clone the official repository. +2. Switch to the ```dev``` branch. +3. Create a bookmark that describes that feature preferably in a single + word. +4. Commit all changes to this bookmark in the ```dev``` branch. +5. Publish your work online and notify the RBDL maintainer(s). + +Here are the commands to perform steps 1.-4.: + + # Step 1: clone official repository + hg clone https://bitbucket.org/rbdl/rbdl && cd + + # Step 2: switch to the dev branch + hg update dev + + # Step 3: create a bookmark + hg bookmark newfeature + + # ... + # Make changes + # ... + + # Step 4: commit changes + hg commit + +For step 5 the easiest would be to push your changes to a fork of the +official repository on bitbucket and create a pull request. + +## Debugging + +* Todo: mention logging facility +* Todo: mention SimpleMath as a fast-compiling (but slower runtime) linear +algebra package. + + diff --git a/3rdparty/rbdl/Doxyfile b/3rdparty/rbdl/Doxyfile new file mode 100644 index 0000000..1d2b380 --- /dev/null +++ b/3rdparty/rbdl/Doxyfile @@ -0,0 +1,1837 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project +# +# All text after a hash (#) is considered a comment and will be ignored +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" ") + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "Rigid Body Dynamics Library" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./doc + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = YES + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 2 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = YES + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = YES + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = NO + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = YES + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = YES + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = ./src \ + include/rbdl \ + doc/api_changes.txt \ + doc/example.h \ + doc/python_example.h \ + doc/Mainpage.h \ + doc/images \ + doc/luamodel_example.h \ + addons/luamodel/luamodel.h \ + addons/urdfreader/rbdl_urdfreader.h \ + addons/urdfreader/urdfreader.h \ + addons/geometry/Function.h \ + addons/geometry/SegmentedQuinticBezierToolkit.h \ + addons/geometry/SmoothSegmentedFunction.h \ + addons/muscle/Millard2016TorqueMuscle.h \ + addons/muscle/MuscleFunctionFactory.h \ + addons/muscle/TorqueMuscleFunctionFactory.h \ + ./include/rbdl + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.d \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.idl \ + *.odl \ + *.cs \ + *.php \ + *.php3 \ + *.inc \ + *.m \ + *.mm \ + *.dox \ + *.py \ + *.f90 \ + *.f \ + *.vhd \ + *.vhdl + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = ./include/rbdl/SimpleMath + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = doc \ + examples/luamodel \ + examples/simple \ + examples/python + + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = doc/images \ + doc \ + doc/logo \ + + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = YES + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = YES + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 3 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = YES + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 248 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 12 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4wide + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = amsmath xr amsfonts + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. This is useful +# if you want to understand what is going on. On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = FreeSans + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = NO + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES + diff --git a/3rdparty/rbdl/LICENSE b/3rdparty/rbdl/LICENSE new file mode 100644 index 0000000..e1fcd42 --- /dev/null +++ b/3rdparty/rbdl/LICENSE @@ -0,0 +1,23 @@ +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2016 Martin Felis + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/README.md b/3rdparty/rbdl/README.md new file mode 100644 index 0000000..6289abe --- /dev/null +++ b/3rdparty/rbdl/README.md @@ -0,0 +1,171 @@ +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2016 Martin Felis + +Introduction +============ + +RBDL is a highly efficient C++ library that contains some essential rigid +body dynamics algorithms such as the Articulated Body Algorithm (ABA) for +forward dynamics, Recursive Newton-Euler Algorithm (RNEA) for inverse +dynamics and the Composite Rigid Body Algorithm (CRBA) for the efficient +computation of the joint space inertia matrix. It further contains code for +Jacobians, forward and inverse kinematics, and handling of external +constraints such as contacts and collisions. + +The code is developed by Martin Felis +at the research group [Optimization in Robotics and Biomechanics +(ORB)](http://orb.iwr.uni-heidelberg.de) of the [Interdisciplinary Center +for Scientific Computing (IWR)](http://www.iwr.uni-heidelberg.de) at +[Heidelberg University](http://www.uni-heidelberg.de). The code tightly +follows the notation used in Roy Featherstone''s book "Rigid Body Dynamics +Algorithm". + +Recent Changes +============== + * 28 April 2016: Nev version 2.5.0: + * Added an experimental Cython based Python wrapper of RBDL. The API is + very close to the C++ API. For a brief glimpse of the API see the file + python/test_wrapper.py. + * Matthew Millard added CustomJoints which allow to create different joint + types completely by user code. They are implemented as proxy joints for + which their behaviour is specified using virtual functions. + * Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + the joint space inertia matrix with a vector in O(n) time. + * Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + the floating base joint. + * Loading of floating base URDF models must now be specified as a third + parameter to URDFReadFromFile() and URDFReadFromString() + * Added the URDF code from Bullet3 which gets used when ROS is not found. + Otherwise use the URDF libraries found via Catkin. + * Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + that compute both linear and angular quantities + * Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + JointTypeFloatingBase instead. + * Fixed building issues when building DLL with MSVC++. + * 20 April 2016: New version 2.4.1: + * This is a bugfix release that maintains binary compatibility and only fixes + erroneous behaviour. + * critical: fixed termination criterion for InverseKinematics. The termination + criterion would be evaluated too early and thus report convergence too + early. This was reported independently by Kevin Stein, Yun Fei, and Davide + Corradi. Thanks for the reports! + * critical: fixed CompositeRigidBodyAlgorithm when using spherical joints + (thanks to Sébastien Barthélémy for reporting!) + * 23 February 2015: New version 2.4.0: + * Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() + and ComputeContactImpulsesRangeSpaceSparse() + * Added null-space method ForwardDynamicsContactsNullSpace() + and ComputeContactImpulsesNullSpace() + * Renamed ForwardDynamicsContactsLagrangian() to + ForwardDynamicsContactsDirect() and + ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect() + * Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis() + * Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The + functionality has been integrated into CalcCenterOfMass(). + * CalcPointJacobian() does not clear the argument of the result anymore. + Caller has to ensure that the matrix was set to zero before using this + function. + * Added optional workspace parameters for ForwardDynamicsLagrangian() to + optionally reduce memory allocations + * Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ + which are equivalent to the emulated multidof joints but faster. + * Added optional parameter to CalcCenterOfMass to compute angular momentum. + * Added CalcBodySpatialJacobian() + * Added CalcContactJacobian() + * Added NonlinearEffects() + * Added solving of linear systems using standard Householder QR + * LuaModel: Added LuaModelReadFromLuaState() + * URDFReader: Fixed various issues and using faster joints for floating + base models + * Various performance improvements + +For a complete history see doc/api_changes.txt. + +Documentation +============= + +The documentation is contained in the code and can be extracted with the +tool [doxygen](http://www.doxygen.org). + +To create the documentation simply run + + doxygen Doxyfile + +which will generate the documentation in the subdirectory ./doc/html. The +main page will then be located in ./doc/html/index.html. + +An online version of the generated documentation can be found at +[http://rbdl.bitbucket.org](http://rbdl.bitbucket.org). + +Getting RBDL +============ + +The latest stable code can be obtained from + + https://bitbucket.org/rbdl/rbdl/get/default.zip + +The official mercurial repository can be cloned from + + https://bitbucket.org/rbdl/rbdl + +(See [http://mercurial.selenic.com/](http://mercurial.selenic.com) for +mercurial clients.) + +Building and Installation +========================= + +The RBDL is built using CMake +([http://www.cmake.org](http://www.cmake.org)). To compile the library in +a separate directory in Release mode use: + + mkdir build + cd build/ + cmake -D CMAKE_BUILD_TYPE=Release ../ + make + +For optimal performance it is highly recommended to install the Eigen3 +linear algebra library from +[http://eigen.tuxfamily.org](http://eigen.tuxfamily.org). RBDL also +comes with a simple, albeit much slower math library (SimpleMath) that can +be used by enabling `RBDL_USE_SIMPLE_MATH`, i.e.: + + cmake -D RBDL_USE_SIMPLE_MATH=TRUE ../ + +Licensing +========= + +The library is published under the very permissive zlib free software +license which should allow you to use the software wherever you need. + +This is the full license text (zlib license): + + RBDL - Rigid Body Dynamics Library + Copyright (c) 2011-2016 Martin Felis + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not + be misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. + +Acknowledgements +================ + +Work on this library was originally funded by the [Heidelberg Graduate +School of Mathematical and Computational Methods for the Sciences +(HGS)](http://hgs.iwr.uni-heidelberg.de/hgs.mathcomp/), and the European +FP7 projects [ECHORD](http://echord.eu) (grant number 231143) and +[Koroibot](http://koroibot.eu) (grant number 611909). diff --git a/3rdparty/rbdl/addons/benchmark/CMakeLists.txt b/3rdparty/rbdl/addons/benchmark/CMakeLists.txt new file mode 100644 index 0000000..fd79df0 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/CMakeLists.txt @@ -0,0 +1,60 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +# Perform the proper linking +SET (CMAKE_SKIP_BUILD_RPATH FALSE) +SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") +SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +# Options +SET ( BENCHMARK_SOURCES + model_generator.cc + Human36Model.cc + benchmark.cc + ) + +ADD_EXECUTABLE ( benchmark ${BENCHMARK_SOURCES} ) + +IF (RBDL_BUILD_STATIC) + SET (LIBRARIES rbdl-static) + + IF (RBDL_BUILD_ADDON_LUAMODEL) + SET (LIBRARIES ${LIBRARIES} rbdl_luamodel-static) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + IF (RBDL_BUILD_ADDON_URDFREADER) + SET (LIBRARIES ${LIBRARIES} rbdl_urdfreader-static) + ENDIF (RBDL_BUILD_ADDON_URDFREADER) + + TARGET_LINK_LIBRARIES ( benchmark + rbdl-static + ${LIBRARIES} + ) +ELSE (RBDL_BUILD_STATIC) + SET (LIBRARIES rbdl) + + IF (RBDL_BUILD_ADDON_LUAMODEL) + SET (LIBRARIES ${LIBRARIES} rbdl_luamodel) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + IF (RBDL_BUILD_ADDON_URDFREADER) + SET (LIBRARIES ${LIBRARIES} rbdl_urdfreader) + ENDIF (RBDL_BUILD_ADDON_URDFREADER) + + TARGET_LINK_LIBRARIES ( benchmark + rbdl + ${LIBRARIES} + ) +ENDIF (RBDL_BUILD_STATIC) diff --git a/3rdparty/rbdl/addons/benchmark/Human36Model.cc b/3rdparty/rbdl/addons/benchmark/Human36Model.cc new file mode 100644 index 0000000..565daf4 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Human36Model.cc @@ -0,0 +1,158 @@ +#include "Human36Model.h" + +#include "rbdl/rbdl.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +enum SegmentName { + SegmentPelvis = 0, + SegmentThigh, + SegmentShank, + SegmentFoot, + SegmentMiddleTrunk, + SegmentUpperTrunk, + SegmentUpperArm, + SegmentLowerArm, + SegmentHand, + SegmentHead, + SegmentNameLast +}; + +double SegmentLengths[SegmentNameLast] = { + 0.1457, + 0.4222, + 0.4403, + 0.1037, + 0.2155, + 0.2421, + 0.2817, + 0.2689, + 0.0862, + 0.2429 +}; + +double SegmentMass[SegmentNameLast] = { + 0.8154, + 10.3368, + 3.1609, + 1.001, + 16.33, + 15.96, + 1.9783, + 1.1826, + 0.4453, + 5.0662 +}; + +double SegmentCOM[SegmentNameLast][3] = { + { 0., 0., 0.0891}, + { 0., 0., -0.1729}, + { 0., 0., -0.1963}, + { 0.1254, 0., -0.0516}, + { 0., 0., 0.1185}, + { 0., 0., 0.1195}, + { 0., 0., -0.1626}, + { 0., 0., -0.1230}, + { 0., 0., -0.0680}, + { 0., 0., 1.1214} +}; + +double SegmentRadiiOfGyration[SegmentNameLast][3] = { + { 0.0897, 0.0855, 0.0803}, + { 0.1389, 0.0629, 0.1389}, + { 0.1123, 0.0454, 0.1096}, + { 0.0267, 0.0129, 0.0254}, + { 0.0970, 0.1009, 0.0825}, + { 0.1273, 0.1172, 0.0807}, + { 0.0803, 0.0758, 0.0445}, + { 0.0742, 0.0713, 0.0325}, + { 0.0541, 0.0442, 0.0346}, + { 0.0736, 0.0634, 0.0765} +}; + +Body create_body (SegmentName segment) { + Matrix3d inertia_C (Matrix3d::Zero()); + inertia_C(0,0) = pow(SegmentRadiiOfGyration[segment][0] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(1,1) = pow(SegmentRadiiOfGyration[segment][1] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(2,2) = pow(SegmentRadiiOfGyration[segment][2] * SegmentLengths[segment], 2) * SegmentMass[segment]; + + return RigidBodyDynamics::Body ( + SegmentMass[segment], + RigidBodyDynamics::Math::Vector3d ( + SegmentCOM[segment][0], + SegmentCOM[segment][1], + SegmentCOM[segment][2] + ), + inertia_C); +} + +void generate_human36model (RigidBodyDynamics::Model *model) { + Body pelvis_body = create_body (SegmentPelvis); + Body thigh_body = create_body (SegmentThigh); + Body shank_body = create_body (SegmentShank); + Body foot_body = create_body (SegmentFoot); + Body middle_trunk_body = create_body (SegmentMiddleTrunk); + Body upper_trunk_body = create_body (SegmentUpperTrunk); + Body upperarm_body = create_body (SegmentUpperArm); + Body lowerarm_body = create_body (SegmentLowerArm); + Body hand_body = create_body (SegmentHand); + Body head_body = create_body (SegmentHead); + + Joint free_flyer ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Joint rot_yxz ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_yz ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + Joint fixed (JointTypeFixed); + + model->gravity = Vector3d (0., 0., -9.81); + + unsigned int pelvis_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + model->AddBody (pelvis_id, Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz, thigh_body, "thigh_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz, thigh_body, "thigh_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz, middle_trunk_body, "middletrunk"); + unsigned int uppertrunk_id = model->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "upperarm_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "upperarm_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "head"); +} diff --git a/3rdparty/rbdl/addons/benchmark/Human36Model.h b/3rdparty/rbdl/addons/benchmark/Human36Model.h new file mode 100644 index 0000000..9d95d32 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Human36Model.h @@ -0,0 +1,11 @@ +#ifndef _HUMAN36MODEL_H +#define _HUMAN36MODEL_H + +namespace RigidBodyDynamics { +class Model; +} + +void generate_human36model (RigidBodyDynamics::Model *model); + +/* _HUMAN36MODEL_H */ +#endif diff --git a/3rdparty/rbdl/addons/benchmark/SampleData.h b/3rdparty/rbdl/addons/benchmark/SampleData.h new file mode 100644 index 0000000..3bc75c9 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/SampleData.h @@ -0,0 +1,85 @@ +#ifndef _SAMPLE_DATA_H +#define _SAMPLE_DATA_H + +struct SampleData { + SampleData() : + count (0), q(NULL), qdot(NULL), qddot(NULL), tau(NULL) + {} + ~SampleData() { + deleteData(); + } + SampleData(const SampleData &data) { + count = data.count; + + q = new RigidBodyDynamics::Math::VectorNd[count]; + qdot = new RigidBodyDynamics::Math::VectorNd[count]; + qddot = new RigidBodyDynamics::Math::VectorNd[count]; + tau = new RigidBodyDynamics::Math::VectorNd[count]; + + for (int si = 0; si < count; si++) { + q[si] = data.q[si]; + qdot[si] = data.qdot[si]; + qddot[si] = data.qddot[si]; + tau[si] = data.tau[si]; + } + } + SampleData& operator= (const SampleData &data) { + if (this != &data) { + deleteData(); + *this = SampleData (data); + } + return *this; + } + + unsigned int count; + RigidBodyDynamics::Math::VectorNd *q; + RigidBodyDynamics::Math::VectorNd *qdot; + RigidBodyDynamics::Math::VectorNd *qddot; + RigidBodyDynamics::Math::VectorNd *tau; + + void deleteData() { + count = 0; + + if (q) + delete[] q; + q = NULL; + + if (qdot) + delete[] qdot; + qdot = NULL; + + if (qddot) + delete[] qddot; + qddot = NULL; + + if (tau) + delete[] tau; + tau = NULL; + } + + void fillRandom (int dof_count, int sample_count) { + deleteData(); + count = sample_count; + + q = new RigidBodyDynamics::Math::VectorNd[count]; + qdot = new RigidBodyDynamics::Math::VectorNd[count]; + qddot = new RigidBodyDynamics::Math::VectorNd[count]; + tau = new RigidBodyDynamics::Math::VectorNd[count]; + + for (int si = 0; si < count; si++) { + q[si].resize (dof_count); + qdot[si].resize (dof_count); + qddot[si].resize (dof_count); + tau[si].resize (dof_count); + + for (int i = 0; i < dof_count; i++) { + q[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + qdot[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + qddot[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + tau[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + } + } + } +}; + +#endif diff --git a/3rdparty/rbdl/addons/benchmark/Timer.h b/3rdparty/rbdl/addons/benchmark/Timer.h new file mode 100644 index 0000000..2656f64 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Timer.h @@ -0,0 +1,29 @@ +#ifndef _TIMER_H +#define _TIMER_H + +#include + +struct TimerInfo { + /// time stamp when timer_start() gets called + clock_t clock_start_value; + + /// time stamp when the timer was stopped + clock_t clock_end_value; + + /// duration between clock_start_value and clock_end_value in seconds + double duration_sec; +}; + +inline void timer_start (TimerInfo *timer) { + timer->clock_start_value = clock(); +} + +inline double timer_stop (TimerInfo *timer) { + timer->clock_end_value = clock(); + + timer->duration_sec = static_cast(timer->clock_end_value - timer->clock_start_value) * 1 / CLOCKS_PER_SEC; + + return timer->duration_sec; +} + +#endif diff --git a/3rdparty/rbdl/addons/benchmark/benchmark.cc b/3rdparty/rbdl/addons/benchmark/benchmark.cc new file mode 100644 index 0000000..db44ecd --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/benchmark.cc @@ -0,0 +1,879 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "rbdl/rbdl.h" +#include "model_generator.h" +#include "Human36Model.h" +#include "SampleData.h" +#include "Timer.h" + +#ifdef RBDL_BUILD_ADDON_LUAMODEL +#include "../addons/luamodel/luamodel.h" +bool have_luamodel = true; +#else +bool have_luamodel = false; +#endif + +#ifdef RBDL_BUILD_ADDON_URDFREADER +#include "../addons/urdfreader/urdfreader.h" +bool have_urdfreader = true; +bool urdf_floating_base = false; +#else +bool have_urdfreader = false; +#endif + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int benchmark_sample_count = 1000; +int benchmark_model_max_depth = 5; + +bool benchmark_run_fd_aba = true; +bool benchmark_run_fd_lagrangian = true; +bool benchmark_run_id_rnea = true; +bool benchmark_run_crba = true; +bool benchmark_run_nle = true; +bool benchmark_run_calc_minv_times_tau = true; +bool benchmark_run_contacts = false; +bool benchmark_run_ik = true; + +string model_file = ""; + +enum ContactsMethod { + ContactsMethodLagrangian = 0, + ContactsMethodRangeSpaceSparse, + ContactsMethodNullSpace, + ContactsMethodKokkevis +}; + +double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamics (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i], + sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + MatrixNd H (MatrixNd::Zero(model->dof_count, model->dof_count)); + VectorNd C (VectorNd::Zero(model->dof_count)); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamicsLagrangian (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i], + sample_data.qddot[i], + Math::LinearSolverPartialPivLU, + NULL, + &H, + &C + ); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_inverse_dynamics_RNEA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + InverseDynamics (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.qddot[i], + sample_data.tau[i] + ); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_CRBA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + Math::MatrixNd H = Math::MatrixNd::Zero(model->dof_count, model->dof_count); + Math::MatrixNd identity = Math::MatrixNd::Identity(model->dof_count, model->dof_count); + Math::MatrixNd Hinv = Math::MatrixNd::Zero(model->dof_count, model->dof_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + CompositeRigidBodyAlgorithm (*model, sample_data.q[i], H, true); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_nle_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + NonlinearEffects (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i] + ); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_calc_minv_times_tau_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + CalcMInvTimesTau (*model, sample_data.q[0], sample_data.tau[0], sample_data.qddot[0]); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + CalcMInvTimesTau (*model, sample_data.q[i], sample_data.tau[i], sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + cout << "#DOF: " << setw(3) << model->dof_count + << " #samples: " << sample_count + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + return duration; +} + +double run_contacts_lagrangian_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamicsConstraintsDirect (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + return duration; +} + +double run_contacts_lagrangian_sparse_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamicsConstraintsRangeSpaceSparse (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + return duration; +} + +double run_contacts_null_space (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamicsConstraintsNullSpace (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + return duration; +} + +double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + ForwardDynamicsContactsKokkevis(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + } + + double duration = timer_stop (&tinfo); + + return duration; +} + +double contacts_benchmark (int sample_count, ContactsMethod contacts_method) { + // initialize the human model + Model *model = new Model(); + generate_human36model(model); + + // initialize the constraint sets + unsigned int foot_r = model->GetBodyId ("foot_r"); + unsigned int foot_l = model->GetBodyId ("foot_l"); + unsigned int hand_r = model->GetBodyId ("hand_r"); + unsigned int hand_l = model->GetBodyId ("hand_l"); + + ConstraintSet one_body_one_constraint; + ConstraintSet two_bodies_one_constraint; + ConstraintSet four_bodies_one_constraint; + + ConstraintSet one_body_four_constraints; + ConstraintSet two_bodies_four_constraints; + ConstraintSet four_bodies_four_constraints; + + LinearSolver linear_solver = LinearSolverPartialPivLU; + + one_body_one_constraint.linear_solver = linear_solver; + two_bodies_one_constraint.linear_solver = linear_solver; + four_bodies_one_constraint.linear_solver = linear_solver; + one_body_four_constraints.linear_solver = linear_solver; + two_bodies_four_constraints.linear_solver = linear_solver; + four_bodies_four_constraints.linear_solver = linear_solver; + + // one_body_one + one_body_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_one_constraint.Bind (*model); + + // two_bodies_one + two_bodies_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_one_constraint.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_one_constraint.Bind (*model); + + // four_bodies_one + four_bodies_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.Bind (*model); + + // one_body_four + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_four_constraints.Bind (*model); + + // two_bodies_four + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + two_bodies_four_constraints.Bind (*model); + + // four_bodies_four + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.Bind (*model); + + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + cout << "= No constraints (Articulated Body Algorithm):" << endl; + run_forward_dynamics_ABA_benchmark (model, sample_count); + cout << "= Constraints:" << endl; + double duration; + + // one body one + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &one_body_one_constraint, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count); + } + + cout << "ConstraintSet: 1 Body 1 Constraint : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + // two_bodies_one + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &two_bodies_one_constraint, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count); + } + + cout << "ConstraintSet: 2 Bodies 1 Constraint : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + + // four_bodies_one + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &four_bodies_one_constraint, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count); + } + + cout << "ConstraintSet: 4 Bodies 1 Constraint : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + // one_body_four + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &one_body_four_constraints, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &one_body_four_constraints, sample_count); + } + + cout << "ConstraintSet: 1 Body 4 Constraints : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + // two_bodies_four + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &two_bodies_four_constraints, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count); + } + + cout << "ConstraintSet: 2 Bodies 4 Constraints: " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + + // four_bodies_four + if (contacts_method == ContactsMethodLagrangian) { + duration = run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodRangeSpaceSparse) { + duration = run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ContactsMethodNullSpace) { + duration = run_contacts_null_space (model, &four_bodies_four_constraints, sample_count); + } else { + duration = run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count); + } + + cout << "ConstraintSet: 4 Bodies 4 Constraints: " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + delete model; + + return duration; +} + +double run_single_inverse_kinematics_benchmark(Model *model, std::vector &CS, int sample_count){ + TimerInfo tinfo; + timer_start (&tinfo); + VectorNd qinit = VectorNd::Zero(model->dof_count); + VectorNd qres = VectorNd::Zero(model->dof_count); + VectorNd failures = VectorNd::Zero(model->dof_count); + + for (int i = 0; i < sample_count; i++) { + if (!InverseKinematics(*model, qinit, CS[i], qres)){ + failures[i] = 1; + } + } + double duration = timer_stop (&tinfo); + std::cout << "Success Rate: " << (1-failures.mean())*100 << "% for: "; + return duration; + +} + +double run_all_inverse_kinematics_benchmark (int sample_count){ + + //initialize the human model + Model *model = new Model(); + generate_human36model(model); + + unsigned int foot_r = model->GetBodyId ("foot_r"); + unsigned int foot_l = model->GetBodyId ("foot_l"); + unsigned int hand_r = model->GetBodyId ("hand_r"); + unsigned int hand_l = model->GetBodyId ("hand_l"); + unsigned int head = model->GetBodyId ("head"); + + Vector3d foot_r_point (1., 0., 0.); + Vector3d foot_l_point (-1., 0., 0.); + Vector3d hand_r_point (0., 1., 0.); + Vector3d hand_l_point (1., 0., 1.); + Vector3d head_point (0.,0.,-1.); + + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + + //create constraint sets + std::vector cs_one_point; + std::vector cs_two_point_one_orientation; + std::vector cs_two_full_one_point; + std::vector cs_two_full_two_point_one_orientation; + std::vector cs_five_full; + + for (unsigned int i = 0; i < sample_count; i++){ + Vector3d foot_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_r, foot_r_point); + Vector3d foot_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_l, foot_l_point); + Vector3d hand_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_r, hand_r_point); + Vector3d hand_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_l, hand_l_point); + Vector3d head_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], head , head_point); + + Matrix3d foot_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_r, false); + Matrix3d foot_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_l, false); + Matrix3d hand_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_r, false); + Matrix3d hand_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_l, false); + Matrix3d head_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], head , false); + + //single point + InverseKinematicsConstraintSet one_point; + one_point.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + one_point.step_tol = 1e-12; + cs_one_point.push_back(one_point); + + //two point and one orientation + InverseKinematicsConstraintSet two_point_one_orientation; + two_point_one_orientation.AddPointConstraint(foot_l,foot_l_point, foot_l_position); + two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_point_one_orientation.step_tol = 1e-12; + cs_two_point_one_orientation.push_back(two_point_one_orientation); + + //two full and one point + InverseKinematicsConstraintSet two_full_one_point; + two_full_one_point.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_one_point.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_one_point.AddPointConstraint(head, head_point, head_position); + two_full_one_point.step_tol = 1e-12; + cs_two_full_one_point.push_back(two_full_one_point); + + //two full, two points and one orienation + InverseKinematicsConstraintSet two_full_two_point_one_orientation; + two_full_two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_full_two_point_one_orientation.AddPointConstraint(foot_l, foot_l_point, foot_l_position); + two_full_two_point_one_orientation.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_two_point_one_orientation.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_full_two_point_one_orientation.step_tol = 1e-12; + cs_two_full_two_point_one_orientation.push_back(two_full_two_point_one_orientation); + + //five points 5 orientations + InverseKinematicsConstraintSet five_full; + five_full.AddFullConstraint(foot_r, foot_r_point, foot_r_position, foot_r_orientation); + five_full.AddFullConstraint(foot_l, foot_l_point, foot_l_position, foot_l_orientation); + five_full.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + five_full.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + five_full.AddFullConstraint(head, head_point, head_position, head_orientation); + five_full.step_tol = 1e-12; + cs_five_full.push_back(five_full); + } + + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + double duration; + + duration = run_single_inverse_kinematics_benchmark(model, cs_one_point, sample_count); + cout << "Constraints: 1 Body: 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_point_one_orientation, sample_count); + cout << "Constraints: 3 Bodies: 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_one_point, sample_count); + cout << "Constraints: 3 Bodies: 2 Full 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_two_point_one_orientation, sample_count); + cout << "Constraints: 5 Bodies: 2 Full 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_five_full, sample_count); + cout << "Constraints: 5 Bodies: 5 Full : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + return duration; +} + +void print_usage () { +#if defined (RBDL_BUILD_ADDON_LUAMODEL) || defined (RBDL_BUILD_ADDON_URDFREADER) + cout << "Usage: benchmark [--count|-c ] [--depth|-d ] " << endl; +#else + cout << "Usage: benchmark [--count|-c ] [--depth|-d ]" << endl; +#endif + cout << "Simple benchmark tool for the Rigid Body Dynamics Library." << endl; + cout << " --count | -c : sets the number of sample states that should" << endl; + cout << " be calculated (default: 1000)" << endl; + cout << " --depth | -d : sets maximum depth for the branched test model" << endl; + cout << " which is created increased from 1 to (default: 5)." << endl; +#if defined RBDL_BUILD_ADDON_URDFREADER + cout << " --floating-base | -f : the specified URDF model is a floating base model." << endl; +#endif + cout << " --no-fd : disables benchmarking of forward dynamics." << endl; + cout << " --no-fd-aba : disables benchmark for forwards dynamics using" << endl; + cout << " the Articulated Body Algorithm" << endl; + cout << " --no-fd-lagrangian : disables benchmark for forward dynamics via" << endl; + cout << " solving the lagrangian equation." << endl; + cout << " --no-id-rnea : disables benchmark for inverse dynamics using" << endl; + cout << " the recursive newton euler algorithm." << endl; + cout << " --no-crba : disables benchmark for joint space inertia" << endl; + cout << " matrix computation using the composite rigid" << endl; + cout << " body algorithm." << endl; + cout << " --no-nle : disables benchmark for the nonlinear effects." << endl; + cout << " --no-calc-minv : disables benchmark M^-1 * tau benchmark." << endl; + cout << " --only-contacts | -C : only runs contact model benchmarks." << endl; + cout << " --help | -h : prints this help." << endl; +} + +void disable_all_benchmarks () { + benchmark_run_fd_aba = false; + benchmark_run_fd_lagrangian = false; + benchmark_run_id_rnea = false; + benchmark_run_crba = false; + benchmark_run_nle = false; + benchmark_run_calc_minv_times_tau = false; + benchmark_run_contacts = false; +} + +void parse_args (int argc, char* argv[]) { + int argi = 1; + + while (argi < argc) { + string arg = argv[argi]; + + if (arg == "--help" || arg == "-h") { + print_usage(); + exit (1); + } else if (arg == "--count" || arg == "-c" ) { + if (argi == argc - 1) { + print_usage(); + + cerr << "Error: missing number of samples!" << endl; + exit (1); + } + + argi++; + stringstream count_stream (argv[argi]); + + count_stream >> benchmark_sample_count; + } else if (arg == "--depth" || arg == "-d" ) { + if (argi == argc - 1) { + print_usage(); + + cerr << "Error: missing number for model depth!" << endl; + exit (1); + } + + argi++; + stringstream depth_stream (argv[argi]); + + depth_stream >> benchmark_model_max_depth; +#ifdef RBDL_BUILD_ADDON_URDFREADER + } else if (arg == "--floating-base" || arg == "-f") { + urdf_floating_base = true; +#endif + } else if (arg == "--no-fd" ) { + benchmark_run_fd_aba = false; + benchmark_run_fd_lagrangian = false; + } else if (arg == "--no-fd-aba" ) { + benchmark_run_fd_aba = false; + } else if (arg == "--no-fd-lagrangian" ) { + benchmark_run_fd_lagrangian = false; + } else if (arg == "--no-id-rnea" ) { + benchmark_run_id_rnea = false; + } else if (arg == "--no-crba" ) { + benchmark_run_crba = false; + } else if (arg == "--no-nle" ) { + benchmark_run_nle = false; + } else if (arg == "--no-calc-minv" ) { + benchmark_run_calc_minv_times_tau = false; + } else if (arg == "--only-contacts" || arg == "-C") { + disable_all_benchmarks(); + benchmark_run_contacts = true; + benchmark_run_ik = false; +#if defined (RBDL_BUILD_ADDON_LUAMODEL) || defined (RBDL_BUILD_ADDON_URDFREADER) + } else if (model_file == "") { + model_file = arg; +#endif + } else { + print_usage(); + cerr << "Invalid argument '" << arg << "'." << endl; + exit(1); + } + argi++; + } +} + +int main (int argc, char *argv[]) { + parse_args (argc, argv); + + Model *model = NULL; + + model = new Model(); + + if (model_file != "") { + if (model_file.substr (model_file.size() - 4, 4) == ".lua") { +#ifdef RBDL_BUILD_ADDON_LUAMODEL + RigidBodyDynamics::Addons::LuaModelReadFromFile (model_file.c_str(), model); +#else + cerr << "Could not load Lua model: LuaModel addon not enabled!" << endl; + abort(); +#endif + } + if (model_file.substr (model_file.size() - 5, 5) == ".urdf") { +#ifdef RBDL_BUILD_ADDON_URDFREADER + RigidBodyDynamics::Addons::URDFReadFromFile(model_file.c_str(), model, urdf_floating_base); +#else + cerr << "Could not load URDF model: urdfreader addon not enabled!" << endl; + abort(); +#endif + } + + if (benchmark_run_fd_aba) { + cout << "= Forward Dynamics: ABA =" << endl; + run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_fd_lagrangian) { + cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl; + run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_id_rnea) { + cout << "= Inverse Dynamics: RNEA =" << endl; + run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_crba) { + cout << "= Joint Space Inertia Matrix: CRBA =" << endl; + run_CRBA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_nle) { + cout << "= Nonlinear effects =" << endl; + run_nle_benchmark (model, benchmark_sample_count); + } + + delete model; + + return 0; + } + + rbdl_print_version(); + cout << endl; + + if (benchmark_run_fd_aba) { + cout << "= Forward Dynamics: ABA =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_fd_lagrangian) { + cout << "= Forward Dynamics: Lagrangian (Piv. LU decomposition) =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_id_rnea) { + cout << "= Inverse Dynamics: RNEA =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_crba) { + cout << "= Joint Space Inertia Matrix: CRBA =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_CRBA_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_nle) { + cout << "= Nonlinear Effects =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_nle_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_calc_minv_times_tau) { + cout << "= CalcMInvTimesTau =" << endl; + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_calc_minv_times_tau_benchmark (model, benchmark_sample_count); + + delete model; + } + cout << endl; + } + + if (benchmark_run_contacts) { + cout << "= Contacts: ForwardDynamicsConstraintsLagrangian" << endl; + contacts_benchmark (benchmark_sample_count, ContactsMethodLagrangian); + + cout << "= Contacts: ForwardDynamicsConstraintsRangeSpaceSparse" << endl; + contacts_benchmark (benchmark_sample_count, ContactsMethodRangeSpaceSparse); + + cout << "= Contacts: ForwardDynamicsConstraintsNullSpace" << endl; + contacts_benchmark (benchmark_sample_count, ContactsMethodNullSpace); + + cout << "= Contacts: ForwardDynamicsContactsKokkevis" << endl; + contacts_benchmark (benchmark_sample_count, ContactsMethodKokkevis); + } + + if (benchmark_run_ik) { + cout << "= Inverse Kinematics" << endl; + run_all_inverse_kinematics_benchmark(benchmark_sample_count); + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/benchmark/model_generator.cc b/3rdparty/rbdl/addons/benchmark/model_generator.cc new file mode 100644 index 0000000..743d60b --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/model_generator.cc @@ -0,0 +1,54 @@ +#include "model_generator.h" + +#include "rbdl/rbdl.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +void generate_planar_tree_recursive (Model *model, + unsigned int parent_body_id, + int depth, + double length) { + if (depth == 0) + return; + + // create left child + Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.)); + Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length)); + + Vector3d displacement (-0.5 * length, -0.25 * length, 0.); + unsigned int child_left = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body); + + generate_planar_tree_recursive (model, + child_left, + depth - 1, + length * 0.4); + + displacement.set (0.5 * length, -0.25 * length, 0.); + unsigned int child_right = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body); + + generate_planar_tree_recursive (model, + child_right, + depth - 1, + length * 0.4); +} + +void generate_planar_tree (Model *model, int depth) { + // we first add a single body that is hanging straight down from + // (0, 0, 0). After that we generate the tree recursively such that each + // call adds two children. + // + double length = 1.; + + Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.)); + Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length)); + + unsigned int base_child = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, body); + + generate_planar_tree_recursive ( + model, + base_child, + depth, + length * 0.4); +} + diff --git a/3rdparty/rbdl/addons/benchmark/model_generator.h b/3rdparty/rbdl/addons/benchmark/model_generator.h new file mode 100644 index 0000000..964e28c --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/model_generator.h @@ -0,0 +1,11 @@ +#ifndef _MODEL_GENERATOR_H +#define _MODEL_GENERATOR_H + +namespace RigidBodyDynamics { +class Model; +} + +void generate_planar_tree (RigidBodyDynamics::Model *model, int depth); + +/* _MODEL_GENERATOR_H */ +#endif diff --git a/3rdparty/rbdl/addons/geometry/CMakeLists.txt b/3rdparty/rbdl/addons/geometry/CMakeLists.txt new file mode 100644 index 0000000..d255ed5 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/CMakeLists.txt @@ -0,0 +1,80 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) + +SET ( RBDL_ADDON_GEOMETRY_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_GEOMETRY_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_GEOMETRY_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_GEOMETRY_VERSION + ${RBDL_ADDON_GEOMETRY_VERSION_MAJOR}.${RBDL_ADDON_GEOMETRY_VERSION_MINOR}.${RBDL_ADDON_GEOMETRY_VERSION_PATCH} +) + +PROJECT (RBDL_ADDON_GEOMETRY VERSION ${RBDL_ADDON_GEOMETRY_VERSION}) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl +) + +SET(GEOMETRY_SOURCES + SegmentedQuinticBezierToolkit.cc + SmoothSegmentedFunction.cc + SegmentedQuinticBezierToolkit.h + SmoothSegmentedFunction.h + geometry.h + Function.h +) + +SET(GEOMETRY_HEADERS + geometry.h + Function.h + SegmentedQuinticBezierToolkit.h + SmoothSegmentedFunction.h +) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_geometry-static STATIC ${GEOMETRY_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_geometry-static PROPERTIES PREFIX "lib") + SET_TARGET_PROPERTIES ( rbdl_geometry-static PROPERTIES OUTPUT_NAME "rbdl_geometry") + + TARGET_LINK_LIBRARIES ( + rbdl_geometry-static + rbdl-static + ) + + INSTALL (TARGETS rbdl_geometry-static + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_geometry SHARED ${GEOMETRY_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_geometry PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES ( + rbdl_geometry + rbdl + ) + + INSTALL (TARGETS rbdl_geometry + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + ) + +INSTALL ( FILES ${GEOMETRY_HEADERS} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/geometry + ) \ No newline at end of file diff --git a/3rdparty/rbdl/addons/geometry/Function.h b/3rdparty/rbdl/addons/geometry/Function.h new file mode 100644 index 0000000..0daaf02 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/Function.h @@ -0,0 +1,525 @@ +#ifndef SimTK_SimTKCOMMON_FUNCTION_H_ +#define SimTK_SimTKCOMMON_FUNCTION_H_ + +/* -------------------------------------------------------------------------- * + * Simbody(tm): SimTKcommon * + * -------------------------------------------------------------------------- * + * This is part of the SimTK biosimulation toolkit originating from * + * Simbios, the NIH National Center for Physics-Based Simulation of * + * Biological Structures at Stanford, funded under the NIH Roadmap for * + * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * + * * + * Portions copyright (c) 2008-12 Stanford University and the Authors. * + * Authors: Peter Eastman * + * Contributors: Michael Sherman * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +// Note: this file was moved from Simmath to SimTKcommon 20100601; see the +// Simmath repository for earlier history. + +//#include "SimTKcommon/basics.h" +//#include "SimTKcommon/Simmatrix.h" +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +#include +#include +#include +#include +#include +/** + This abstract class represents a mathematical function that calculates a + value of arbitrary type based on M real arguments. The output type is set + as a template argument, while the number of input components may be + determined at runtime. The name "Function" (with no trailing _) may be + used as a synonym for Function_. + + Subclasses define particular mathematical functions. Predefined subclasses + are provided for several common function types: Function_::Constant, + Function_::Linear, Function_::Polynomial, and Function_::Step. + You can define your own subclasses for other function types. The + Spline_ class also provides a convenient way to create various types of + Functions. + */ + +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + + +template +class Function_ { +public: + class Constant; + class Linear; + class Polynomial; + class Sinusoid; + class Step; + virtual ~Function_() { + } + /** + Calculate the value of this function at a particular point. + + @param x the RigidBodyDynamics::Math::VectorNd of input arguments. Its + size must equal the value returned by getArgumentSize(). + */ + virtual T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const = 0; + /** + Calculate a partial derivative of this function at a particular point. + Which derivative to take is specified by listing the input components + with which to take it. For example, if derivComponents=={0}, that + indicates a first derivative with respective to component 0. If + derivComponents=={0, 0, 0}, that indicates a third derivative with + respective to component 0. If derivComponents=={4, 7}, that indicates a + partial second derivative with respect to components 4 and 7. + + @param derivComponents + The input components with respect to which the derivative should be + taken. Its size must be less than or equal to the value returned + by getMaxDerivativeOrder(). + @param x + The RigidBodyDynamics::Math::VectorNd of input arguments. Its size must + equal the value + returned by getArgumentSize(). + @return + The value of the selected derivative, which is of type T. + */ + virtual T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const = 0; + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(std::vector(derivComponents),x); } + */ + + /** + * Get the number of components expected in the input vector. + */ + virtual int getArgumentSize() const = 0; + /** + * Get the maximum derivative order this Function_ object can calculate. + */ + virtual int getMaxDerivativeOrder() const = 0; +}; + +/** This typedef is used for the very common case that the return type of +the Function object is double. **/ +typedef Function_ Function; + + + +/** + * This is a Function_ subclass which simply returns a fixed value, independent + * of its arguments. + */ +template +class Function_::Constant : public Function_ { +public: + /** + * Create a Function_::Constant object. + * + * @param value the value which should be returned by calcValue(); + * @param argumentSize the value which should be returned by + * getArgumentSize(), with a default of 1. + */ + explicit Constant(T value, int argumentSize=1) + : argumentSize(argumentSize), value(value) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == argumentSize); + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + return static_cast(0); + } + virtual int getArgumentSize() const { + return argumentSize; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(std::vector(derivComponents),x); } + */ +private: + const int argumentSize; + const T value; +}; + +/** + * This is a Function_ subclass whose output value is a linear function of its + * arguments: f(x, y, ...) = ax+by+...+c. + */ +template +class Function_::Linear : public Function_ { +public: + /** + * Create a Function_::Linear object. + * + * @param coefficients + * The coefficients of the linear function. The number of arguments + * expected by the function is equal to coefficients.size()-1. + * coefficients[0] is the coefficient for the first argument, + * coefficients[1] is the coefficient for the second argument, etc. + * The final element of coefficients contains the constant term. + */ + explicit Linear( + const RigidBodyDynamics::Math::VectorNd& coefficients) + : coefficients(coefficients) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == coefficients.size()-1); + T value = static_cast(0); + for (int i = 0; i < x.size(); ++i) + value += x[i]*coefficients[i]; + value += coefficients[x.size()]; + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == coefficients.size()-1); + assert(derivComponents.size() > 0); + if (derivComponents.size() == 1) + return coefficients(derivComponents[0]); + return static_cast(0); + } + virtual int getArgumentSize() const { + return coefficients.size()-1; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const RigidBodyDynamics::Math::VectorNd coefficients; +}; + + +/** + * This is a Function_ subclass whose output value is a polynomial of its + * argument: f(x) = ax^n+bx^(n-1)+...+c. + */ +template +class Function_::Polynomial : public Function_ { +public: + /** + * Create a Function_::Polynomial object. + * + * @param coefficients the polynomial coefficients in order of decreasing + * powers + */ + Polynomial(const RigidBodyDynamics::Math::VectorNd& coefficients) + : coefficients(coefficients) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == 1); + double arg = x[0]; + T value = static_cast(0); + for (int i = 0; i < coefficients.size(); ++i) + value = value*arg + coefficients[i]; + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == 1); + assert(derivComponents.size() > 0); + double arg = x[0]; + T value = static_cast(0); + const int derivOrder = (int)derivComponents.size(); + const int polyOrder = coefficients.size()-1; + for (int i = 0; i <= polyOrder-derivOrder; ++i) { + T coeff = coefficients[i]; + for (int j = 0; j < derivOrder; ++j) + coeff *= polyOrder-i-j; + value = value*arg + coeff; + } + return value; + } + virtual int getArgumentSize() const { + return 1; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const RigidBodyDynamics::Math::VectorNd coefficients; +}; + + +/** + * This is a Function_ subclass whose output value is a sinusoid of its + * argument: f(x) = a*sin(w*x + p) where a is amplitude, w is frequency + * in radians per unit of x, p is phase in radians. + * + * This is only defined for a scalar (double) return value. + */ +template <> +class Function_::Sinusoid : public Function_ { +public: + /** + * Create a Function::Sinusoid object, returning a*sin(w*x+p). + * + * @param[in] amplitude 'a' in the above formula + * @param[in] frequency 'w' in the above formula + * @param[in] phase 'p' in the above formula + */ + Sinusoid(double amplitude, double frequency, double phase=0) + : a(amplitude), w(frequency), p(phase) {} + + void setAmplitude(double amplitude) {a=amplitude;} + void setFrequency(double frequency) {w=frequency;} + void setPhase (double phase) {p=phase;} + + double getAmplitude() const {return a;} + double getFrequency() const {return w;} + double getPhase () const {return p;} + + // Implementation of Function_ virtuals. + + virtual double calcValue( + const RigidBodyDynamics::Math::VectorNd& x) const { + + const double t = x[0]; // we expect just one argument + return a*std::sin(w*t + p); + } + + virtual double calcDerivative( + const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + + const double t = x[0]; // time is the only argument + const int order = derivComponents.size(); + // The n'th derivative is + // sign * a * w^n * sc + // where sign is -1 if floor(order/2) is odd, else 1 + // and sc is cos(w*t+p) if order is odd, else sin(w*t+p) + switch(order) { + case 0: return a* std::sin(w*t + p); + case 1: return a*w* std::cos(w*t + p); + case 2: return -a*w*w* std::sin(w*t + p); + case 3: return -a*w*w*w*std::cos(w*t + p); + default: + const double sign = double(((order/2) & 0x1) ? -1 : 1); + const double sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p); + const double wn = std::pow(w, order); + return sign*a*wn*sc; + } + } + + virtual int getArgumentSize() const {return 1;} // just time + virtual int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + double calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + double a, w, p; +}; + +/** + * This is a Function_ subclass whose output value y=f(x) is smoothly stepped + * from y=y0 to y1 as its input argument goes from x=x0 to x1. This is + * an S-shaped function with first and second derivatives y'(x0)=y'(x1)=0 + * and y''(x0)=y''(x1)==0. The third derivative y''' exists and is continuous + * but we cannot guarantee anything about it at the end points. + */ +template +class Function_::Step : public Function_ { +public: + /** + * Create a Function_::Step object that smoothly interpolates its output + * through a given range as its input moves through its range. + * + * @param y0 Output value when (x-x0)*sign(x1-x0) <= 0. + * @param y1 Output value when (x-x1)*sign(x1-x0) >= 0. + * @param x0 Start of switching interval. + * @param x1 End of switching interval. + * + * @tparam T The template type is the type of y0 and y1. This must + * be a type that supports subtraction and scalar + * multiplication by a double so that we can compute + * an expression like y=y0 + f*(y1-y0) for some double scalar f. + * + * Note that the numerical values of x0 and x1 can be in either order + * x0 < x1 or x0 > x1. + */ + Step(const T& y0, const T& y1, double x0, double x1) + : m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(double(0)*y0), + m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(copysign(1,m_ooxr)) + { + /* + SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_::Step::ctor()", + "A zero-length switching interval is illegal; both ends were %g.", x0); + */ + assert(x0 != x1); + std::printf( "Function_::Step::ctor():" + "A zero-length switching interval " + "is illegal; both ends were %g.", x0); + + } + + T calcValue(const RigidBodyDynamics::Math::VectorNd& xin) const { + /* + SimTK_ERRCHK1_ALWAYS(xin.size() == 1, + "Function_::Step::calcValue()", + "Expected just one input argument but got %d.", xin.size()); + */ + assert(xin.size() == 1); + std::printf( "Function_::Step::calcValue() " + "Expected just one input argument but got %d.", + xin.size()); + + + const double x = xin[0]; + if ((x-m_x0)*m_sign <= 0) return m_y0; + if ((x-m_x1)*m_sign >= 0) return m_y1; + // f goes from 0 to 1 as x goes from x0 to x1. + const double f = step01(m_x0,m_ooxr, x); + return m_y0 + f*m_yr; + } + + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& xin) const { + /* + SimTK_ERRCHK1_ALWAYS(xin.size() == 1, + "Function_::Step::calcDerivative()", + "Expected just one input argument but got %d.", xin.size()); + */ + assert(xin.size() == 1); + std::printf( "Function_::Step::calcDerivative() " + "Expected just one input argument but got %d.", + xin.size()); + + const int derivOrder = (int)derivComponents.size(); + + /* + SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3, + "Function_::Step::calcDerivative()", + "Only 1st, 2nd, and 3rd derivatives of the step are available," + " but derivative %d was requested.", derivOrder); + */ + assert(1 <= derivOrder && derivOrder <= 3); + std::printf("Function_::Step::calcDerivative() " + "Only 1st, 2nd, and 3rd derivatives of the step are available," + " but derivative %d was requested.", derivOrder); + + const double x = xin[0]; + if ((x-m_x0)*m_sign <= 0) return m_zero; + if ((x-m_x1)*m_sign >= 0) return m_zero; + switch(derivOrder) { + case 1: return dstep01(m_x0,m_ooxr, x) * m_yr; + case 2: return d2step01(m_x0,m_ooxr, x) * m_yr; + case 3: return d3step01(m_x0,m_ooxr, x) * m_yr; + default: assert(!"impossible derivOrder"); + } + return NAN*m_yr; /*NOTREACHED*/ + } + + virtual int getArgumentSize() const {return 1;} + int getMaxDerivativeOrder() const {return 3;} + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0) + const T m_zero; // precalculate T(0) + const double m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0) + const double m_sign; // sign(x1-x0) is 1 or -1 + + double step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double u2 = u*u; + double u3 = u2*u; + return (3*u2 - 2*u3); + } + + double dstep01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + double du2 = 2*u*du; + double du3 = 3*u*u*du; + return (3*du2 - 2*du3); + } + + double d2step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + //double ddu = 0; + double ddu2 = 2*du*du;// + 2*u*ddu since ddu=0; + double ddu3 = 3*du*u*du + 3*u*du*du;// + 3*u*u*du; ditto + return (3*ddu2 - 2*ddu3); + } + + double d3step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + //double ddu = 0; + //double dddu = 0; + double dddu2 = 0; //2*du*du;// since ddu=0; + double dddu3 = 3*du*du*du + 3*du*du*du;// ditto + return (3*dddu2 - 2*dddu3); + } +}; + +} +} +} + +#endif // SimTK_SimTKCOMMON_FUNCTION_H_ + + diff --git a/3rdparty/rbdl/addons/geometry/LICENSE b/3rdparty/rbdl/addons/geometry/LICENSE new file mode 100644 index 0000000..6e03346 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/LICENSE @@ -0,0 +1,23 @@ +Rigid Body Dynamics Library Geometry Addon - +Copyright (c) 2016 Matthew Millard + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt b/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/3rdparty/rbdl/addons/geometry/NOTICE b/3rdparty/rbdl/addons/geometry/NOTICE new file mode 100644 index 0000000..d99650d --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/NOTICE @@ -0,0 +1,24 @@ +Author: + Matthew Millard + +Date: + July 2016 + +Notice: these files + + -Function.h + -SegmentedQuinticBezierToolkit.h + -SegmentedQuinticBezierToolkit.cc + -SmoothSegmentedFunction.h + -SmoothSegmentedFunction.cc + -tests/numericalTestFunctions.h + -tests/numericalTestFunctions.cc + -tests/testSmoothSegmentedFunction.cc + +were originally part of OpenSim and have been ported +over to RBDL with some modification. These files are licenced under the +APACHE 2.0 license which, like the zlib license, is quite liberal. The +full licence can be found in this folder in the file "LICENSE_APACHE-2.0.txt" +and online here: + +http://www.apache.org/licenses/LICENSE-2.0.txt diff --git a/3rdparty/rbdl/addons/geometry/README.md b/3rdparty/rbdl/addons/geometry/README.md new file mode 100644 index 0000000..be2861b --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/README.md @@ -0,0 +1,60 @@ +@brief geometry - a set of static tool kits for creating and evaluating curves, + surfaces and solids. This addon is maintained by Matthew + Millard, so if you have problems with it email him. + +@author Matthew Millard + +\copyright 2016 Matthew Millard + +\b Requirements +This addon is standalone as of its first release + +\b Description + This addon currently contains an optimized library for creating and + evaluating 5th order 2D Bezier splines: SegmentedQuinticBezierToolkit.h + and SegmentedQuinticBezierToolkit.cc. In addition, there is a nice class + that can be used to package the memory and functions required to + evaluate these curves: SmoothSegmentedFunction.h and + SmoothSegmentedFunction.cc. + +\b Future Development +In the near future this library will also contain + +1. Geometry tools to represent C2 convex implicit surfaces and enforce + contact constraints between two surfaces. This tool kit will be first + used for simulating foot-ground contact. It could later be used for + 3D muscle wrapping: + + SmoothImplicitSurfaceToolkit + SmoothImplicitSurface + +2. Geometry tools to represent quintic Pythagorean Hodograph curves - which are + special Bezier curves that have an analytic equation for arc-length. This + package will also contain code to represent polar Pythagorean Hodographs + which will be first used to formulate a foot-ground joint. Later this toolkit + will be used for a 2D cable transmission system + (to simulate muscle wrapping). + + SegmentedQuinticPythagoreanHodographToolkit + PolarFunctionToolkit + + +\b Licensing +The following files have been ported over from OpenSim and Simbody and as such +are licenced under the Apache 2.0 Licence: + +SmoothSegmentedFunction.h +SmoothSegmentedFunction.cc +SegmentedQuinticBezierToolkit.h +SegmentedQuinticBezierToolkit.cc +Function.h + +The Apache License is very similar to the zlib license and is quite liberal. +Licensed under the Apache License, Version 2.0 (the "License"); you may +not use this file except in compliance with the License. You may obtain a +copy of the License at http://www.apache.org/licenses/LICENSE-2.0. + +The remaining code has been written from scratch and is licenced under the +zlib license. See the LICENSE file for details. + + diff --git a/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc new file mode 100644 index 0000000..526efcd --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc @@ -0,0 +1,1285 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SegmentedQuinticBezierToolkit.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= +#include "SegmentedQuinticBezierToolkit.h" +#include +#include +#include +#include +#include + +//#include +using namespace RigidBodyDynamics::Addons::Geometry; + +//============================================================================= +// STATICS +//============================================================================= +//using namespace SimTK; +//using namespace OpenSim; +using namespace std; +using namespace RigidBodyDynamics::Math; +static double UTOL_DESIRED = std::numeric_limits::epsilon()*1e2; +static double UTOL_INITIALSOLN = std::numeric_limits::epsilon()*1e11; +static int MAXITER_INITIALSOLN = 12; + +static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample + //each Bezier corner section + +double SegmentedQuinticBezierToolkit::scaleCurviness(double curviness) +{ + double c = 0.1 + 0.8*curviness; + return c; +} + +/** +This function will print cvs file of the column vector col0 and the matrix data +@params col0: A vector that must have the same number of rows as the data matrix + This column vector is printed as the first column +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void SegmentedQuinticBezierToolkit:: + printMatrixToFile( const VectorNd& col0, + const MatrixNd& data, + std::string& filename) +{ + + ofstream datafile; + datafile.open(filename.c_str()); + + for(int i = 0; i < data.rows(); i++){ + datafile << col0[i] << ","; + for(int j = 0; j < data.cols(); j++){ + if(j& curveFit, + MatrixNd& ctrlPts, + VectorNd& xVal, + VectorNd& yVal, + std::string& filename) +{ + std::string caller = "printBezierSplineFitCurves"; + int nbezier = int(ctrlPts.cols()/2.0); + int rows = NUM_SAMPLE_PTS*nbezier - (nbezier-1); + + VectorNd y1Val(rows); + VectorNd y2Val(rows); + + VectorNd ySVal(rows); + VectorNd y1SVal(rows); + VectorNd y2SVal(rows); + + MatrixNd printMatrix(rows,6); + + VectorNd tmp(1); + std::vector deriv1(1); + std::vector deriv2(2); + + deriv1[0] = 0; + deriv2[0] = 0; + deriv2[1] = 0; + double u = 0; + int oidx = 0; + int offset = 0; + for(int j=0; j < nbezier ; j++) + { + if(j > 0){ + offset = 1; + } + + for(int i=0; i=0 && curviness <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + "Error: double argument curviness must be between 0.0 and 1.0."); + */ + if( !(curviness>=0 && curviness <= 1) ){ + + cerr << "SegmentedQuinticBezierToolkit::" + <<"calcQuinticBezierCornerControlPoints" + <<"Error: double argument curviness must be between 0.0 and 1.0." + <<"curviness : " << curviness << " " + << endl; + assert (0); + abort(); + } + + + //1. Calculate the location where the two lines intersect + // (x-x0)*dydx0 + y0 = (x-x1)*dydx1 + y1 + // x*(dydx0-dydx1) = y1-y0-x1*dydx1+x0*dydx0 + // x = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); + + double xC = 0.; + double yC = 0.; + double epsilon = std::numeric_limits::epsilon(); + double rootEPS = sqrt(epsilon); + if(abs(dydx0-dydx1) > rootEPS){ + xC = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); + }else{ + xC = (x1+x0)/2.0; + } + + yC = (xC-x1)*dydx1 + y1; + //Check to make sure that the inputs are consistent with a corner, and will + //not produce an 's' shaped section. To check this we compute the sides of + //a triangle that is formed by the two points that the user entered, and + //also the intersection of the 2 lines the user entered. If the distance + //between the two points the user entered is larger than the distance from + //either point to the intersection loctaion, this function will generate a + //'C' shaped curve. If this is not true, an 'S' shaped curve will result, + //and this function should not be used. + + double xCx0 = (xC-x0); + double yCy0 = (yC-y0); + double xCx1 = (xC-x1); + double yCy1 = (yC-y1); + double x0x1 = (x1-x0); + double y0y1 = (y1-y0); + + + + double a = xCx0*xCx0 + yCy0*yCy0; + double b = xCx1*xCx1 + yCy1*yCy1; + double c = x0x1*x0x1 + y0y1*y0y1; + + //This error message needs to be better. + /* + SimTK_ERRCHK_ALWAYS( ((c > a) && (c > b)), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + "The intersection point for the two lines defined by the input" + "parameters must be consistent with a C shaped corner."); + */ + + + + if( !((c > a) && (c > b)) ){ + cerr << "SegmentedQuinticBezierToolkit" + << "::calcQuinticBezierCornerControlPoints:" + << "The line segments at the end of the curve sections " + << "do not intersect within the domain " + << "("<< x0 << "," << x1 << ") of the curve. " + << "and so there is a chance that curve will not" + << " be monotonic. There are 2 ways to fix this problem: " + << endl + << "1. Add an intermediate point," + << endl + << " 2. Space the domain points more widely " + << endl + << "Details: " + << endl << " a: " << a + << endl << " b: " << b + << endl << " c: " << c << endl; + assert (0); + abort(); + + } + + /* + Value of the 2nd derivative at the end points. + This is not exposed to the user for now, as rarely is possible + or even easy to know what these values should be. Internally + I'm using this here because we get curves with nicer 1st + derivatives than if we take the easy option to get a second + derivative of zero (by setting the middle control points equal + to their neighbors. + */ + + double d2ydx20 = 0; + double d2ydx21 = 0; + + //Start point + xyPts(0,0) = x0; + xyPts(0,1) = y0; + //End point + xyPts(5,0) = x1; + xyPts(5,1) = y1; + + + /* + //Original code - leads to 2 localized corners + xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0)); + xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1)); + //xyPts(2,0) = xyPts(1,0); + //xyPts(2,1) = xyPts(1,1); + + //Second two midpoints + xyPts(3,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0)); + xyPts(3,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1)); + xyPts(4,0) = xyPts(3,0); + xyPts(4,1) = xyPts(3,1); + */ + + //Set the 1st and 4th control points (nearest to the end points) + //to get the correct first derivative + xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0)); + xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1)); + + xyPts(4,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0)); + xyPts(4,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1)); + + //Now go and update the middle points to get the desired 2nd + //derivative at the ends. Note that even if d2ydx2 = 0 the + //resulting curve using this method has a much smoother 1st + //derivative than if the middle control points are set to be + //equal to the 1st and 4th control points. + + double dxdu0 = 5.0*(xyPts(1,0) - xyPts(0,0)); + xyPts(2,0) = xyPts(1,0) + 0.5*(xC-xyPts(1,0)) ; + double d2xdu20 = 20.0*(xyPts(2,0) - 2.0*xyPts(1,0) + xyPts(0,0)); + double d2ydu20 = (dxdu0*dxdu0*(d2ydx20) + d2xdu20*(dydx0)) ; + xyPts(2,1) = d2ydu20*(1.0/20.0) + 2.0*xyPts(1,1) - xyPts(0,1) ; + + double dxdu1 = 5.0*(xyPts(5,0) - xyPts(4,0)); + xyPts(3,0) = xyPts(4,0) + 0.5*(xC-xyPts(4,0)); + double d2xdu21 = 20.0*(xyPts(3,0) - 2.0*xyPts(4,0) + xyPts(5,0) ); + double d2ydu21 = (dxdu1*dxdu1*(d2ydx21) + d2xdu21*(dydx1)); + xyPts(3,1) = d2ydu21*(1.0/20.0) + 2.0*xyPts(4,1) - xyPts(5,1); + + return xyPts; +} + +//============================================================================= +// BASIC QUINTIC BEZIER EVALUATION FUNCTIONS +//============================================================================= + +/* +Multiplications Additions Assignments +21 20 13 +*/ +double SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(double u1, const VectorNd& pts) +{ + double val = -1; + + /* + SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + "Error: double argument u must be between 0.0 and 1.0" + "but %f was entered.",u); + */ + if(!(u1 >= 0 && u1 <= 1)){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal" + << "Error: double argument u must be between 0.0 and 1.0" + << "but " << u1 <<" was entered."; + assert (0); + abort(); + } + + /* + SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + "Error: vector argument pts must have a length of 6."); + */ + if(!(pts.size() == 6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal:" + << "Error: vector argument pts must have a length of 6."; + assert (0); + abort(); + } + + double u2 = u1*u1; + double u3 = u2*u1; + double u4 = u3*u1; + double u5 = u4*u1; + + //v0 = 1; + double v1 = (1-u1); + double v2 = v1*v1; + double v3 = v2*v1; + double v4 = v3*v1; + double v5 = v4*v1; + + val = pts[0] *v5*1.0 + + pts[1]*u1*v4*5.0 + + pts[2]*u2*v3*10.0 + + pts[3]*u3*v2*10.0 + + pts[4]*u4*v1*5.0 + + pts[5]*u5 *1.0; + + + return val; +} + +/* +Detailed Computational Costs + dy/dx Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + dy/dx 1 + total 1 40 38 22 + + d2y/dx2 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d2y/dx2 2 4 1 3 + total 2 78 73 23 + + d3y/dx3 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + + d3y/dx3 4 16 5 6 + total 4 118 105 58 + + d4y/dx4 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + + d4y/dx4 5 44 15 13 + total 5 168 137 71 + + d5y/dx5 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + d5y/du5 6 6 1 + d5x/du5 6 6 1 + + d5y/dx5 7 100 36 28 + total 7 236 170 88 + + d6y/dx6 + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + d5y/du5 6 6 1 + d5x/du5 6 6 1 + + d6y/dx6 9 198 75 46 + total 9 334 209 106 + +*/ +double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( + double u, + const VectorNd& xpts, + const VectorNd& ypts, + int order) +{ + double val = NAN;//SimTK::NaN; + + //Bounds checking on the input + /* + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: double argument u must be between 0.0 and 1.0."); + + SimTK_ERRCHK_ALWAYS( (xpts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument xpts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (ypts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument ypts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (order >= 1), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be greater than."); + + SimTK_ERRCHK_ALWAYS( (order <= 6), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be less than, or equal to 6."); + */ + if( !(u>=0 && u <= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + assert(0); + abort(); + + } + + if( !(xpts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument xpts must have a length of 6." + << endl; + assert(0); + abort(); + + } + if( !(ypts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument ypts must have a length of 6." + << endl; + assert(0); + abort(); + } + + if( !(order >= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than." + << endl; + assert(0); + abort(); + } + + if( !(order <= 6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be less than, or equal to 6." + << endl; + assert(0); + abort(); + } + //std::string localCaller = caller; + //localCaller.append(".calcQuinticBezierCurveDerivDYDX"); + //Compute the derivative d^n y/ dx^n + switch(order){ + case 1: //Calculate dy/dx + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double dydx = dydu/dxdu; + + val = dydx; + //Question: + //how is a divide by zero treated? Is SimTK::INF returned? + } + break; + case 2: //Calculate d^2y/dx^2 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + + //Optimized code from Maple - + //see MuscleCurveCodeOpt_20120210 for details + double t1 = 0.1e1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double d2ydx2 = (d2ydu2 * t1 - dydu / t3 * d2xdu2) * t1; + + val = d2ydx2; + + } + break; + case 3: //Calculate d^3y/dx^3 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//(dxdu ^ 2); + double t4 = 1 / t3; + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (dydu * t4); + double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2 + + 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1 + - (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1; + + val = d3ydx3; + } + break; + case 4: //Calculate d^4y/dx^4 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d2ydu2 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t23 = (dydu * t9); + double t27 = (dydu * t4); + double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11 + - t27 * d3xdu3; + double t43 = d2ydu2 * t1 - t27 * d2xdu2; + double t47 = t43 * t4; + double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2 + + 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3 + - 6 * dydu / t17 * t11 * d2xdu2 + + 6 * t23 * d2xdu2 * d3xdu3 + - t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2 + + 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1 + - (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1; + + + val = d4ydx4; + + } + break; + case 5: + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d3ydu3 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t18 = 1 / t17; + double t20 = (t11 * d2xdu2); + double t23 = (d2ydu2 * t9); + double t24 = (d2xdu2 * d3xdu3); + double t27 = (d2ydu2 * t4); + double t33 = t11*t11;//(t11 ^ 2); + double t36 = (dydu * t18); + double t40 = (dydu * t9); + double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t47 = (dydu * t4); + double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2 + + 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3 + - 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24 + - 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33 + - 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41 + + 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5; + double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11 + - 3 * t27 * d3xdu3 - 6 * t36 * t20 + + 6 * t40 * t24 - t47 * d4xdu4; + double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11 + - t47 * d3xdu3; + double t77 = t73 * t4; + double t82 = d2ydu2 * t1 - t47 * d2xdu2; + double t86 = t82 * t9; + double t89 = t82 * t4; + double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11 + - t89 * d3xdu3; + double t105 = t73 * t1 - t89 * d2xdu2; + double t109 = t105 * t4; + double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2 + + 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3 + - 6 * t82 * t18 * t20 + + 6 * t86 * t24 - t89 * d4xdu4) * t1 + - 2 * t99 * t4 * d2xdu2 + + 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1 + - (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1; + + + val = d5ydx5; + + } + break; + case 6: + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6); + double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6); + + double t1 = dxdu*dxdu;//(dxdu ^ 2); + double t3 = (0.1e1 / t1 / dxdu); + double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t8 = t1*t1;//(t1 ^ 2); + double t9 = 1 / t8; + double t11 = (t5 * d2xdu2); + double t14 = (d3ydu3 * t3); + double t15 = (d2xdu2 * d3xdu3); + double t19 = (0.1e1 / t8 / dxdu); + double t21 = t5*t5;//(t5 ^ 2); + double t24 = (d2ydu2 * t9); + double t25 = (t5 * d3xdu3); + double t28 = (d2ydu2 * t3); + double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t32 = (d2xdu2 * d4xdu4); + double t41 = (dydu * t19); + double t45 = (dydu * t9); + double t49 = (dydu * t3); + double t56 = 1 / dxdu; + double t61 = 1 / t1; + double t62 = (dydu * t61); + double t67 = (d4ydu4 * t61); + double t70 = (d2ydu2 * t61); + double t73 = (d3ydu3 * t61); + double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11 + + 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21 + - 180 * t24 * t25 + + 30 * t28 * t29 + 40 * t28 * t32 + - 120 * dydu / t8 / t1 * t21 * d2xdu2 + + 240 * t41 *t11*d3xdu3 + - 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4 + + 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56 + - 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6 + - 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3 + - 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4; + + double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5 + - 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15 + - 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25 + + 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5; + + double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5 + - 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15 + - t62 * d4xdu4; + + double t120 = t116 * t61; + double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5 + - t62 * d3xdu3; + double t133 = t129 * t3; + double t136 = t129 * t61; + double t141 = d2ydu2 * t56 - t62 * d2xdu2; + double t145 = t141 * t9; + double t148 = t141 * t3; + double t153 = t141 * t61; + double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2 + + 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3 + - 24 * t129 * t9 * t11 + 24 * t133 * t15 + - 4 * t136 * d4xdu4 + + 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29 + + 8 * t148 * t32 - t153 * d5xdu5; + + double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5 + - 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15 + - t153 * d4xdu4; + + double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5 + - t153 * d3xdu3; + + double t183 = t179 * t61; + double t188 = t129 * t56 - t153 * d2xdu2; + double t192 = t188 * t3; + double t195 = t188 * t61; + double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5 + - t195 * d3xdu3; + double t211 = t179 * t56 - t195 * d2xdu2; + double t215 = t211 * t61; + double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2 + + 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3 + - 6 * t188 * t9 *t11 + + 6 * t192 * t15 - t195 * d4xdu4) * t56 + - 2 * t205 * t61 * d2xdu2 + + 2 * t211*t3*t5-t215*d3xdu3)*t56 + - (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56; + + + val = d6ydx6; + } + break; + default: + val = NAN; //SimTK::NaN; + } + return val; +} + +/* Computational Cost Details + Divisions Multiplications Additions Assignments +dx/du 20 19 11 +d2x/du2 17 17 9 +d3y/du3 14 14 6 + +*/ +double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( + double u, + const VectorNd& pts, + int order) +{ + double val = -1; + /* + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: double argument u must be between 0.0 and 1.0."); + + SimTK_ERRCHK_ALWAYS( (pts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument pts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (order >= 1), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be greater than, or equal to 1"); + */ + if( !(u>=0 && u <= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + assert(0); + abort(); + } + if( !(pts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument pts must have a length of 6." + << endl; + assert(0); + abort(); + } + + if( !(order >= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than, or equal to 1" + << endl; + assert(0); + abort(); + } + + //Compute the Bezier point + double p0 = pts[0]; + double p1 = pts[1]; + double p2 = pts[2]; + double p3 = pts[3]; + double p4 = pts[4]; + double p5 = pts[5]; + + switch(order){ + case 1: + { + double t1 = u*u;//u ^ 2; + double t2 = t1*t1;//t1 ^ 2; + double t4 = t1 * u; + double t5 = t4 * 0.20e2; + double t6 = t1 * 0.30e2; + double t7 = u * 0.20e2; + double t10 = t2 * 0.25e2; + double t11 = t4 * 0.80e2; + double t12 = t1 * 0.90e2; + double t16 = t2 * 0.50e2; + val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1) + + p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1) + + p2 * (-t16 + t4 * 0.120e3 - t12 + t7) + + p3 * (t16 - t11 + t6) + + p4 * (-t10 + t5) + + p5 * t2 * 0.5e1; + + } + break; + case 2: + { + double t1 = u*u;//u ^ 2; + double t2 = t1 * u; + double t4 = t1 * 0.60e2; + double t5 = u * 0.60e2; + double t8 = t2 * 0.100e3; + double t9 = t1 * 0.240e3; + double t10 = u * 0.180e3; + double t13 = t2 * 0.200e3; + val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2) + + p1 * (t8 - t9 + t10 - 0.40e2) + + p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2) + + p3 * (t13 - t9 + t5) + + p4 * (-t8 + t4) + + p5 * t2 * 0.20e2; + + } + break; + case 3: + { + double t1 = u*u;//u ^ 2; + double t3 = u * 0.120e3; + double t6 = t1 * 0.300e3; + double t7 = u * 0.480e3; + double t10 = t1 * 0.600e3; + val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2) + + p1 * (t6 - t7 + 0.180e3) + + p2 * (-t10 + u * 0.720e3 - 0.180e3) + + p3 * (t10 - t7 + 0.60e2) + + p4 * (-t6 + t3) + + p5 * t1 * 0.60e2; + + } + break; + case 4: + { + double t4 = u * 0.600e3; + double t7 = u * 0.1200e4; + val = p0 * (u * (-0.120e3) + 0.120e3) + + p1 * (t4 - 0.480e3) + + p2 * (-t7 + 0.720e3) + + p3 * (t7 - 0.480e3) + + p4 * (-t4 + 0.120e3) + + p5 * u * 0.120e3; + } + break; + case 5: + { + val = p0 * (-0.120e3) + + p1 * 0.600e3 + + p2 * (-0.1200e4) + + p3 * 0.1200e4 + + p4 * (-0.600e3) + + p5 * 0.120e3; + } + break; + default: + val=0; + + } + + return val; + + +} + +double SegmentedQuinticBezierToolkit::clampU(double u){ + double uC = u; + if(u<0.0){ + uC=0; + } + if(u>1.0){ + uC=1; + } + return uC; +} + +/*Detailed Computational Costs + Comparisons Div Mult Additions Assignments + Geuss calculation 1 1 1 + + Newton Iter + f 21 20 13 + df 20 19 11 + update 4 1 3 6 + total 4 1 41 42 30 + \endverbatim + + To evaluate u to SimTK::Eps*100 this typically involves 2 Newton + iterations, yielding a total cost of + + \verbatim + Comparisons Div Mult Additions Assignments + eval U 7+8=15 2 82 42 60 +*/ +double SegmentedQuinticBezierToolkit::calcU(double ax, + const VectorNd& bezierPtsX, + double tol, + int maxIter) +{ + //Check to make sure that ax is in the curve domain + double minX = std::numeric_limits::max(); + double maxX = -minX; + for(int i=0; i maxX) + maxX = bezierPtsX[i]; + if(bezierPtsX[i] < minX) + minX = bezierPtsX[i]; + } + + /* + SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, + "SegmentedQuinticBezierToolkit::calcU", + "Error: input ax was not in the domain of the Bezier curve specified \n" + "by the control points in bezierPtsX."); + */ + if( !(ax >= minX && ax <= maxX) ){ + cerr << "SegmentedQuinticBezierToolkit::calcU:" + << "Error: input ax was not in the domain of the " + << "Bezier curve specified by the control points in bezierPtsX." + << endl; + assert(0); + abort(); + } + + double u = ax/(maxX-minX); + double f = 0; + + u = clampU(u); + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + + //Use the bisection method to get a good initial + //start for the Newton method. This is necessary + //as these curves, though C2, can be so nonlinear + //that the Newton method oscillates unless it is + //close to the initial solution. + if(abs(f) > tol){ + double uL = 0; + double uR = 1; + + double fL = calcQuinticBezierCurveVal(uL,bezierPtsX)-ax; + double fR = calcQuinticBezierCurveVal(uR,bezierPtsX)-ax; + + int iterBisection = 0; + + while(iterBisection < MAXITER_INITIALSOLN + && min(abs(fL),abs(fR)) > UTOL_INITIALSOLN ){ + u = 0.5*(uL+uR); + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + + if(signbit(f) == signbit(fL)){ + fL = f; + uL = u; + }else{ + fR = f; + uR = u; + } + iterBisection++; + } + + if(abs(fL) < abs(fR)){ + u = uL; + f = fL; + }else{ + u = uR; + f = fR; + } + } + + + + double df = 0; + double du = 0; + int iter = 0; + bool pathologic = false; + double fprev = f; + double stepLength = 1.0; + double perturbation01 = 0.0; + //Take Newton steps to the desired tolerance + while((abs(f) > min(tol,UTOL_DESIRED)) + && (iter < maxIter) + && (pathologic == false) ){ + //Take a Newton step + df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1); + + if(abs(df) > 0){ + du = -f/df; + u = u + stepLength*du; + u = clampU(u); + fprev = f; + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + }else{ + //This should never happen. If we are unluky enough to get here + //purturb the current solution and continue until we run out of + //iterations. + perturbation01 = double(rand()%100)/100.0; + u = u + perturbation01*0.1; + u = clampU(u); + } + + iter++; + } + + //Check for convergence + if( abs(f) > tol ){ + std::stringstream errMsg; + errMsg.precision(17); + errMsg << "SegmentedQuinticBezierToolkit::calcU:" << endl + << "Error: desired tolerance of " << tol << endl + << " on U not met by the Newton iteration." << endl + << " A tolerance of " << f << " was reached." << endl + << " Curve range x(u): " << minX << "-" << maxX << endl + << " Desired x: " << ax << " closest u " << u << endl + << " Bezier points " << endl << bezierPtsX << endl; + cerr << errMsg.str(); + assert(0); + abort(); + } + + return u; +} +/* + +Cost: n comparisons, for a quintic Bezier curve with n-spline sections + + Comp Div Mult Add Assignments +Cost 3*n+2 1*n 3 + +*/ +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const MatrixNd& bezierPtsX) +{ + int idx = 0; + bool flag_found = false; + + for(int i=0; i= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){ + idx = i; + i = bezierPtsX.cols(); + flag_found = true; + } + } + + //Check if the value x is identically the last point + if(flag_found == false && x == bezierPtsX(5,bezierPtsX.cols()-1)){ + idx = bezierPtsX.cols()-1; + flag_found = true; + } + + /* + SimTK_ERRCHK_ALWAYS( (flag_found == true), + "SegmentedQuinticBezierToolkit::calcIndex", + "Error: A value of x was used that is not within the Bezier curve set."); + */ + if( !(flag_found == true)){ + cerr << "SegmentedQuinticBezierToolkit::calcIndex" + << "Error: A value of x was used that is not" + << " within the Bezier curve set." << endl; + assert(0); + abort(); + } + + + + + return idx; +} + +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const std::vector& bezierPtsX) +{ + int idx = 0; + bool flag_found = false; + + int n = bezierPtsX.size(); + for(int i=0; i= bezierPtsX[i][0] && x < bezierPtsX[i][5] ){ + idx = i; + flag_found = true; + break; + } + } + + //Check if the value x is identically the last point + if(!flag_found && x == bezierPtsX[n-1][5]){ + idx = n-1; + flag_found = true; + } + + if(!(flag_found == true)){ + cerr << "SegmentedQuinticBezierToolkit::calcIndex " + << "Error: A value of x was used that is not " + << "within the Bezier curve set." + << endl; + assert(0); + abort(); + } + + return idx; +} + + + +/* + Comp Div Mult Additions Assignments +calcIdx 3*3+2=11 1*3=3 3 +calcU 15 2 82 42 60 +calcQuinticBezierCurveVal + 21 20 13 +Total 26 2 103 65 76 +\endverbatim + +Ignoring the costs associated with the integrator itself, and assuming +that the integrator evaluates the function 6 times per integrated point, +the cost of evaluating the integral at each point in vX is: + +\verbatim + Comp Div Mult Additions Assignments +RK45 on 1pt 6*(26 2 103 65 76) +Total 156 12 618 390 456 +\endverbatim + +Typically the integral is evaluated 100 times per section in order to +build an accurate spline-fit of the integrated function. Once again, +ignoring the overhead of the integrator, the function evaluations alone +for the current example would be + +\verbatim +RK45 on 100pts per section, over 3 sections + Comp Div Mult Additions Assignments + 3*100*(156 12 618 390 456 +Total 46,800 3600 185,400 117,000 136,000 + +*/ +/* +MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( + const VectorNd& vX, + double ic0, double intAcc, + double uTol, int uMaxIter, + const MatrixNd& mX, const MatrixNd& mY, + const SimTK::Array_& aSplineUX, + bool flag_intLeftToRight, + const std::string& caller) +{ + MatrixNd intXY(vX.size(),2); + BezierData bdata; + bdata._mX = mX; + bdata._mY = mY; + bdata._initalValue = ic0; + bdata._aArraySplineUX = aSplineUX; + bdata._uMaxIter = uMaxIter; + bdata._uTol = uTol; + bdata._flag_intLeftToRight = flag_intLeftToRight; + bdata._name = caller; + + //These aren't really times, but I'm perpetuating the SimTK language + //so that I don't make a mistake + double startTime = vX(0); + double endTime = vX(vX.size()-1); + + if(flag_intLeftToRight){ + bdata._startValue = startTime; + }else{ + bdata._startValue = endTime; + } + + MySystem sys(bdata); + State initState = sys.realizeTopology(); + initState.setTime(startTime); + + + RungeKuttaMersonIntegrator integ(sys); + integ.setAccuracy(intAcc); + integ.setFinalTime(endTime); + integ.setReturnEveryInternalStep(false); + integ.initialize(initState); + + int idx = 0; + double nextTimeInterval = 0; + Integrator::SuccessfulStepStatus status; + + while (idx < vX.nelt()) { + if(idx < vX.nelt()){ + if(flag_intLeftToRight){ + nextTimeInterval = vX(idx); + }else{ + nextTimeInterval = endTime-vX(vX.size()-idx-1); + } + } + status=integ.stepTo(nextTimeInterval); + + // Use this for variable step output. + //status = integ.stepTo(Infinity); + + if (status == Integrator::EndOfSimulation) + break; + + const State& state = integ.getState(); + + if(flag_intLeftToRight){ + intXY(idx,0) = nextTimeInterval; + intXY(idx,1) = (double)state.getZ()[0]; + }else{ + intXY(vX.size()-idx-1,0) = vX(vX.size()-idx-1); + intXY(vX.size()-idx-1,1) = (double)state.getZ()[0]; + } + idx++; + + } + //intXY.resizeKeep(idx,2); + return intXY; +} +*/ diff --git a/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h new file mode 100644 index 0000000..bb76346 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h @@ -0,0 +1,836 @@ +#ifndef SEGMENTEDQUINTICBEZIERTOOLKIT_H_ +#define SEGMENTEDQUINTICBEZIERTOOLKIT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: SegmentedQuinticBezierToolkit.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +#include +#include +#include "Function.h" + +/** +This is a low level Quintic Bezier curve class that contains functions to design +continuous sets of 'C' shaped Bezier curves, and to evaluate their values and +derivatives. A set in this context is used to refer to 2 or more quintic Bezier +curves that are continuously connected to eachother to form one smooth curve, +hence the name QuinticBezierSet. + +In the special case when this class is being used to generate and evaluate +2D Bezier curves, that is x(u) and y(u), there are also functions to evaluate +y(x), the first six derivatives of y(x), and also the first integral of y(x). + +This class was not designed to be a stand alone Quintic Bezier class, but rather +was developed out of necessity to model muscles. I required curves that, when +linearly extrapolated, were C2 continuous, and by necessity I had to use +quintic Bezier curves. In addition, the curves I was developing were functions +in x,y space, allowing many of the methods (such as the evaluation of y(x) given +that x(u) and y(u), the derivatives of y(x), and its first integral) to be +developed, though in general this can't always be done. + +I have parcelled all of these tools into their own class so that others may more +easily use and develop this starting point for their own means. I used the +following text during the development of this code: + +Mortenson, Michael E (2006). Geometric Modeling Third Edition. Industrial Press +Inc., New York. Chapter 4 was quite helpful. + +Future Upgrades + +1. Analytical Inverse to x(u): + I think this is impossible because it is not possible, in general, to find the + roots to a quintic polynomial, however, this fact may not preclude forming the + inverse curve. The impossibility of finding the roots to a quintic polynomial + was proven by Abel (Abel's Impossibility Theorem) and Galois + + http://mathworld.wolfram.com/QuinticEquation.html + + At the moment I am approximating the curve u(x) using cubic splines to return + an approximate value for u(x), which I polish using Newton's method to the + desired precision. + + **Note as of Nov 2015** + -> The cubic spline approximation of the inverse curve has been + removed. Since there is no spline class in RBDL (and I'm not + motivated to port it over from SimTK) this functionality does + not work. In addition, I've since found that this nice inverse + only saves a few Newton iterations over a calculated guess. + It's not worth the effort to include. + +2. Analytical Integral of y(x): + This is possible using the Divergence Theorem applied to 2D curves. A nice + example application is shown in link 2 for computing the area of a closed + cubic Bezier curve. While I have been able to get the simple examples to work, + I have failed to successfully compute the area under a quintic Bezier curve + correctly. I ran out of time trying to fix this problem, and so at the present + time I am numerically computing the integral at a number of knot points and + then evaluating the spline to compute the integral value. + + a. http://en.wikipedia.org/wiki/Divergence_theorem + b. http://objectmix.com/graphics/133553-area-closed-bezier-curve.html + + **Note as of Nov 2015** + -> The splined numeric integral of the curve has been removed. There + is not an error-controlled numerical integrator in RBDL and so + it is not straight forward to include this feature. + -> For later: discuss options with Martin. + +3. calcU: + Currently the Bezier curve value and its derivative are computed separately to + evaluate f and df in the Newton iteration to solve for u(x). Code optimized to + compute both of these quantites at the same time could cut the cost of + evaluating x(u) and dx/du in half. Since these quantities are evaluated in an + iterative loop, this one change could yield substantial computational savings. + +4. calcIndex: +The function calcIndex computes which spline the contains the point of interest. +This function has been implemented assuming a small number of Bezier curve sets, +and so it simply linearly scans through the sets to determine the correct one to +use. This function should be upgraded to use the bisection method if large +quintic Bezier curve sets are desired. + +5. The addition of additional Bezier control point design algorithms, to create + 'S' shaped curves, and possibly do subdivision. + +6. Low level Code Optimization: +I have exported all of the low level code as optimized code from Maple. Although +the code produced using this means is reasonably fast, it is usally possible +to obtain superior performance (and sometimes less round off error) by +doing this work by hand. + +Computational Cost Details +All computational costs assume the following operation costs: + +\verbatim +Operation Type : #flops ++,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 +\endverbatim + +These relative weightings will vary processor to processor, and so any of +the quoted computational costs are approximate. + + + RBDL Port Notes + +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + +3. There have been some improvements to the function calcU in the + SegmentedQuinticBezierToolkit.cc code. This function evaluates + u such that x(u) - x* = 0. This is done using a Newton method. + However, because these curves can be very nonlinear, the Newton + method requires a very good initial start. In the OpenSim version + this good initial guess was provided by splined interpolation of + u(x). In the RBDL version this initial guess is provided by using + a bisection method until the error of x(u)-x* is within + sqrt(sqrt(tol)) or 2 Newton steps of the desired tolerance. + +@author Matt Millard +@version 0.0 + +*/ +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + +class SegmentedQuinticBezierToolkit +{ + + + public: + + /** + This scales the users value of curviness to be between [0+delta, 1-delta] + because if curviness is allowed to equal 0 or 1, the second derivative + becomes quite violent and the resulting curve is difficult to fit + splines to. + + @param curviness + @retval a scaled version of curviness + + */ + static double scaleCurviness(double curviness); + + /** + This function will compute the u value that correesponds to the given x + for a quintic Bezier curve. + + @param ax The x value + @param bezierPtsX The 6 Bezier point values + @param tol The desired tolerance on u. + @param maxIter The maximum number of Newton iterations allowed + + \b aborts \b + -if ax is outside the range defined in this Bezier spline section + -if the desired tolerance is not met + -if the derivative goes to 0 to machine precision + + This function will compute the u value that correesponds to the given x + for a quintic Bezier curve. This is accomplished by using an approximate + spline inverse of u(x) to get a very good initial guess, and then one or + two Newton iterations to polish the answer to the desired tolerance. + + Computational Costs + \verbatim + ~219 flops + \endverbatim + + Example: + @code + double xVal = 2; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + RigidBodyDynamics::Math::VectorNd x(100); + RigidBodyDynamics::Math::VectorNd u(100); + + //Now evalutate u at the given xVal + double u = SegmentedQuinticBezierToolkit:: + calcU(xVal,vX, 1e-12,20); + + @endcode + */ + static double calcU( + double ax, + const RigidBodyDynamics::Math::VectorNd& bezierPtsX, + double tol, + int maxIter); + + + + /** + Given a set of Bezier curve control points, return the index of the + set of control points that x lies within. + + @param x A value that is interpolated by the set of Bezier + curves + @param bezierPtsX A matrix of 6xn Bezier control points + + \b aborts \b + -If the index is not located within this set of Bezier points + + Given a set of Bezier curve control points, return the index of the + set of control points that x lies within. This function has been coded + assuming a small number of Bezier curve sets (less than 10), and so, + it simply scans through the Bezier curve sets until it finds the correct + one. + + + Computational Costs + Quoted for a Bezier curve set containing 1 to 5 curves. + \verbatim + ~9-25 + \endverbatim + + Example: + @code + RigidBodyDynamics::Math::MatrixNd mX(6,2); + + //The first set of spline points + mX(0,0) = -2; + mX(1,0) = -1; + mX(2,0) = -1; + mX(3,0) = 1; + mX(4,0) = 1; + mX(5,0) = 2; + //The second set of spline points + mX(0,1) = 2; + mX(1,1) = 3; + mX(2,1) = 3; + mX(3,1) = 5; + mX(4,1) = 5; + mX(5,1) = 6; + + //The value of x for which we want the index for + double xVal = 1.75; + int idx = SegmentedQuinticBezierToolkit::calcIndex(xVal,mX); + @endcode + + + */ + static int calcIndex( + double x, + const RigidBodyDynamics::Math::MatrixNd& bezierPtsX); + + static int calcIndex( + double x, + const std::vector& bezierPtsX); + + + + + + /** + Calculates the value of a quintic Bezier curve at value u. + + @param u The independent variable of a Bezier curve, which ranges + between 0.0 and 1.0. + @param pts The locations of the control points in 1 dimension. + + \b aborts \b + -If u is outside of [0,1] + -if pts has a length other than 6 + @return The value of the Bezier curve located at u. + + Calculates the value of a quintic Bezier curve at value u. This + calculation is acheived by mulitplying a row vector comprised of powers + of u, by the 6x6 coefficient matrix associated with a quintic Bezier + curve, by the vector of Bezier control points, pV, in a particular + dimension. The code to compute the value of a quintic bezier curve has + been optimized to have the following cost: + + Computational Costs + \verbatim + ~54 flops + \endverbatim + + + + The math this function executes is decribed in pseudo code as the + following: + + \verbatim + uV = [u^5 u^4 u^3 u^2 u 1]; + + cM = [ -1 5 -10 10 -5 1; + 5 -20 30 -20 5 0; + -10 30 -30 10 0 0; + 10 -20 10 0 0 0; + -5 5 0 0 0 0; + 1 0 0 0 0 0 ]; + pV = [x1; x2; x3; x4; x5; x6]; + + xB = (uV*cM)*pV + \endverbatim + + + + Example: + @code + double u = 0.5; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,vX); + @endcode + + + */ + static double calcQuinticBezierCurveVal( + double u, + const RigidBodyDynamics::Math::VectorNd& pts); + + /** + Calculates the value of a quintic Bezier derivative curve at value u. + @param u The independent variable of a Bezier curve, which ranges + between 0.0 and 1.0. + @param pts The locations of the control points in 1 dimension. + @param order The desired order of the derivative. Order must be >= 1 + + \b aborts \b + -u is outside [0,1] + -pts is not 6 elements long + -if order is less than 1 + @return The value of du/dx of Bezier curve located at u. + + Calculates the value of a quintic Bezier derivative curve at value u. + This calculation is acheived by taking the derivative of the row vector + uV and multiplying it by the 6x6 coefficient matrix associated with a + quintic Bezier curve, by the vector of Bezier control points, pV, in a + particular dimension. + + Pseudo code for the first derivative (order == 1) would be + \verbatim + uV = [5*u^4 4*u^3 3*u^2 2u 1 0]; + + cM = [ -1 5 -10 10 -5 1; + 5 -20 30 -20 5 0; + -10 30 -30 10 0 0; + 10 -20 10 0 0 0; + -5 5 0 0 0 0; + 1 0 0 0 0 0 ]; + pV = [x1; x2; x3; x4; x5; x6]; + + dxdu = (uV*cM)*pV + \endverbatim + + Note that the derivative of uV only needed to be computed to compute + dxdu. This process is continued for all 5 derivatives of x(u) until + the sixth and all following derivatives, which are 0. Higher derivatives + w.r.t. to U are less expensive to compute than lower derivatives. + + Computational Costs + \verbatim + dy/dx : ~50 flops + d2x/du2: ~43 flops + d3x/du3: ~34 flops + d4x/du4: ~26 flops + d5x/du5: ~15 flops + d6x/du6: ~1 flop + \endverbatim + + + Example: + @code + double u = 0.5; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + double dxdu =calcQuinticBezierCurveDerivU(u,vX,1); + @endcode + */ + static double calcQuinticBezierCurveDerivU( + double u, + const RigidBodyDynamics::Math::VectorNd& pts, + int order); + + /** + Calculates the value of dydx of a quintic Bezier curve derivative at u. + + @param u The u value of interest. Note that u must be [0,1] + @param xpts The 6 control points associated with the x axis + @param ypts The 6 control points associated with the y axis + @param order The order of the derivative. Currently only orders from 1-6 + can be evaluated + + \b aborts \b + -If u is outside [0,1] + -If xpts is not 6 elements long + -If ypts is not 6 elements long + -If the order is less than 1 + -If the order is greater than 6 + @retval The value of (d^n y)/(dx^n) evaluated at u + + Calculates the value of dydx of a quintic Bezier curve derivative at u. + Note that a 2D Bezier curve can have an infinite number of derivatives, + because x and y are functions of u. Thus + + \verbatim + dy/dx = (dy/du)/(dx/du) + d^2y/dx^2 = d/du(dy/dx)*du/dx + = [(d^2y/du^2)*(dx/du) - (dy/du)*(d^2x/du^2)]/(dx/du)^2 + *(1/(dx/du)) + etc. + \endverbatim + + Computational Costs + + This obviously only functions when the Bezier curve in question has a + finite derivative. Additionally, higher order derivatives are more + numerically expensive to evaluate than lower order derivatives. For + example, here are the number of operations required to compute the + following derivatives + \verbatim + Name : flops + dy/dx : ~102 + d2y/dx2 : ~194 + d3y/dx3 : ~321 + d4y/dx4 : ~426 + d5y/dx5 : ~564 + d6y/dx6 : ~739 + \endverbatim + + Example: + @code + RigidBodyDynamics::Math::VectorNd vX(6), vY(6); + + double u = 0.5; + + vX(0) = 1; + vX(1) = 1.01164; + vX(2) = 1.01164; + vX(3) = 1.02364; + vX(4) = 1.02364; + vY(5) = 1.04; + + vY(0) = 0; + vY(1) = 3e-16; + vY(2) = 3e-16; + vY(3) = 0.3; + vY(4) = 0.3; + vY(5) = 1; + + + d2ydx2 = SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( + u,vX, vY, 2); + @endcode + + */ + static double calcQuinticBezierCurveDerivDYDX( + double u, + const RigidBodyDynamics::Math::VectorNd& xpts, + const RigidBodyDynamics::Math::VectorNd& ypts, + int order); + + + /** + Calculates the location of quintic Bezier curve control points to + create a C shaped curve like that shown in the figure. Using a series + of these simple and predictably shaped Bezier curves it is easy to build + quite complex curves. + + \image html fig_GeometryAddon_quinticCornerSections.png + + + @param x0 First intercept x location + @param y0 First intercept y location + @param dydx0 First intercept slope + @param x1 Second intercept x location + @param y1 Second intercept y location + @param dydx1 Second intercept slope + @param curviness A parameter that ranges between 0 and 1 to denote a + straight line or a curve + + \b aborts \b + -If the curviness parameter is less than 0, or greater than 1; + -If the points and slopes are chosen so that an "S" shaped curve would + be produced. This is tested by examining the points (x0,y0) and + (x1,y1) together with the intersection (xC,yC) of the lines beginning + at these points with slopes of dydx0 and dydx1 form a triangle. If the + line segment from (x0,y0) to (x1,y1) is not the longest line segment, + an exception is thrown. This is an overly conservative test as it + prevents very deep 'V' shapes from being respresented. + + @return a RigidBodyDynamics::Math::MatrixNd of 6 points Matrix(6,2) that + correspond to the X, and Y control points for a quintic Bezier curve that + has the above properties + + + Calculates the location of quintic Bezier curve control points to + create a C shaped curve that intersects points 0 (x0, y0) and point 1 + (x1, y1) with slopes dydx0 and dydx1 respectively, and a second + derivative of 0. The curve that results can approximate a line + (curviness = 0), or in a smooth C shaped curve (curviniess = 1) + + The current implementation of this function is not optimized in anyway + and has the following costs: + + Computational Costs + \verbatim + ~55 flops + \endverbatim + + + Example: + @code + double x0 = 1; + double y0 = 0; + double dydx0 = 0; + double x1 = 1.04; + double y1 = 1; + double dydx1 = 43; + double c = 0.75; + + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0, y0, dydx0,x1,y1,dydx01, + c); + @endcode + + */ + static RigidBodyDynamics::Math::MatrixNd + calcQuinticBezierCornerControlPoints( double x0, double y0, + double dydx0, + double x1, double y1, + double dydx1, + double curviness); + + /* + This function numerically integrates the Bezier curve y(x). + + @param vX Values of x to evaluate the integral of y(x) + @param ic0 The initial value of the integral + @param intAcc Accuracy of the integrated solution + @param uTol Tolerance on the calculation of the intermediate u term + @param uMaxIter Maximum number of iterations allowed for u to reach its + desired tolerance. + @param mX The 6xn matrix of Bezier control points for x(u) + @param mY The 6xn matrix of Bezier control points for y(u) + + @param flag_intLeftToRight Setting this flag to true will evaluate the + integral from the left most point to the right most + point. Setting this flag to false will cause the + integral to be evaluated from right to left. + @param name Name of caller. + @return RigidBodyDynamics::Math::MatrixNd Col 0: X vector, Col 1: int(y(x)) + + + This function numerically integrates the Bezier curve y(x), when really + both x and y are specified in terms of u. Evaluate the integral at the + locations specified in vX and return the result. + + Computational Costs + + This the expense of this function depends on the number of points in + vX, the points for which the integral y(x) must be calculated. The + integral is evaluated using a Runge Kutta 45 integrator, and so each + point requires 6 function evaluations. + (http://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method) + + The cost of evaluating 1 Bezier curve y(x) scales with the number + of points in xVal: + \verbatim + ~1740 flops per point + \endverbatim + + The example below is quite involved, but just so it can show you an + example of how to create the array of Spline objects that approximate + the function u(x). Although the example has been created for only 1 + Bezier curve set, simply changing the size and entries of the matricies + _mX and _mY will allow multiple sets to be integrated. + + + Example: + @code + //Integrator and u tolerance settings + double INTTOL = 1e-12; + double UTOL = 1e-14; + int MAXITER= 10; + + //Make up a Bezier curve - these happen to be the control points + //for a tendon curve + RigidBodyDynamics::Math::MatrixNd _mX(6,1), _mY(6,1); + _mX(0)= 1; + _mX(1)= 1.01164; + _mX(2)= 1.01164; + _mX(3)= 1.02364; + _mX(4)= 1.02364; + _mX(5)= 1.04; + + _mY(0) = 0; + _mY(1) = 3.10862e-16; + _mY(2) = 3.10862e-16; + _mY(3) = 0.3; + _mY(4) = 0.3; + _mY(5) = 1; + + _numBezierSections = 1; + bool _intx0x1 = true; //Says we're integrating from x0 to x1 + ////////////////////////////////////////////////// + //Generate the set of splines that approximate u(x) + ////////////////////////////////////////////////// + RigidBodyDynamics::Math::VectorNd u(NUM_SAMPLE_PTS); + //Used for the approximate inverse + RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); + //Used for the approximate inverse + + //Used to generate the set of knot points of the integral of y(x) + RigidBodyDynamics::Math::VectorNd + xALL(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); + _arraySplineUX.resize(_numBezierSections); + int xidx = 0; + + for(int s=0; s < _numBezierSections; s++){ + //Sample the local set for u and x + for(int i=0;i 1){ + //Skip the last point of a set that has another set of points + //after it. Why? The last point and the starting point of the + //next set are identical in value. + if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){ + xALL(xidx) = x(i); + xidx++; + } + }else{ + xALL(xidx) = x(i); + xidx++; + } + } + //Create the array of approximate inverses for u(x) + _arraySplineUX[s] = SimTK::SplineFitter:: + fitForSmoothingParameter(3,x,u,0).getSpline(); + } + + + ////////////////////////////////////////////////// + //Compute the integral of y(x) and spline the result + ////////////////////////////////////////////////// + + RigidBodyDynamics::Math::VectorNd yInt = SegmentedQuinticBezierToolkit:: + calcNumIntBezierYfcnX(xALL,0,INTTOL, UTOL, MAXITER,_mX, _mY, + _arraySplineUX,_name); + + if(_intx0x1==false){ + yInt = yInt*-1; + yInt = yInt - yInt(yInt.nelt()-1); + } + + _splineYintX = SimTK::SplineFitter:: + fitForSmoothingParameter(3,xALL,yInt,0).getSpline(); + + @endcode + + + */ + +//MM: Can't port this over to RBDL as RBDL doesn't have an error +// controlled integrator. I could add this if a dependency +// like Boost was allowed. +// +// static RigidBodyDynamics::Math::MatrixNd +// calcNumIntBezierYfcnX( +// const RigidBodyDynamics::Math::VectorNd& vX, +// double ic0, +// double intAcc, +// double uTol, +// int uMaxIter, +// const RigidBodyDynamics::Math::MatrixNd& mX, +// const RigidBodyDynamics::Math::MatrixNd& mY, +// const SimTK::Array_& aSplineUX, +// bool flag_intLeftToRight,const std::string& name); + + + private: + + + /** + This function will print cvs file of the column vector col0 and the + matrix data. + + @param col0 A vector that must have the same number of rows as the + data matrix. This column vector is printed as the first + column + @param data A matrix of data + @param filename The name of the file to print + */ + static void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + std::string& filename); + + /** + @param curveFit a function that evaluates a curve + @param ctrlPts control point locations for the fitted Bezier curve + @param xVal the x values at the control points + @param yVal the y values at the control points + @param filename of the output file. + + */ + static void printBezierSplineFitCurves( + const Function_& curveFit, + RigidBodyDynamics::Math::MatrixNd& ctrlPts, + RigidBodyDynamics::Math::VectorNd& xVal, + RigidBodyDynamics::Math::VectorNd& yVal, + std::string& filename); + + /** + This function will return a value that is equal to u, except when u is + outside of[0,1], then it is clamped to be 0, or 1 + @param u The parameter to be clamped + @retval u but restricted to 0,1. + */ + static double clampU(double u); + + +///@cond +/** +Class that contains data that describes the Bezier curve set. This class is used +by the function calcNumIntBezierYfcnX, which evaluates the numerical integral +of a Bezier curve set. +*/ +class BezierData { + public: + /**A 6xn matrix of Bezier control points for the X axis (domain)*/ + RigidBodyDynamics::Math::MatrixNd _mX; + /**A 6xn matrix of Bezier control points for the Y axis (range)*/ + RigidBodyDynamics::Math::MatrixNd _mY; + /**An n element array containing the approximate spline fits of the + inverse function of x(u), namely u(x)*/ + //std::vector< std::vector > _aArraySplineUX; + /**The initial value of the integral*/ + double _initalValue; + /**The tolerance to use when computing u. Solving u(x) can only be done + numerically at the moment, first by getting a good guess (using the + splines) and then using Newton's method to polish the value up. This + is the tolerance that is used in the polishing stage*/ + double _uTol; + /**The maximum number of interations allowed when evaluating u(x) using + Newton's method. In practice the guesses are usually very close to the + actual solution, so only 1-3 iterations are required.*/ + int _uMaxIter; + /**If this flag is true the function is integrated from its left most + control point to its right most. If this flag is false, the function + is integrated from its right most control point to its left most.*/ + //bool _flag_intLeftToRight; + /**The starting value*/ + //double _startValue; + + /**The name of the curve being intergrated. This is used to generate + useful error messages when something fails*/ + std::string _name; +}; +///@endcond + + +}; + +} +} +} + + +#endif diff --git a/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc b/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc new file mode 100644 index 0000000..69094d9 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc @@ -0,0 +1,844 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunction.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + /* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= +#include "SmoothSegmentedFunction.h" +#include +#include + +//============================================================================= +// STATICS +//============================================================================= +//using namespace SimTK; +//using namespace OpenSim; +using namespace std; +using namespace RigidBodyDynamics::Addons::Geometry; + +static bool DEBUG = false; +static double UTOL = std::numeric_limits::epsilon()*1e6; +static double INTTOL = std::numeric_limits::epsilon()*1e2; +static double SQRTEPS = std::sqrt(numeric_limits::epsilon()); +static int MAXITER = 20; +static int NUM_SAMPLE_PTS = 100; +//============================================================================= +// UTILITY FUNCTIONS +//============================================================================= +/* + DETAILED COMPUTATIONAL COSTS: + ========================================================================= + WITHOUT INTEGRAL + _________________________________________________________________________ + Function Comp Div Mult Add Assignments + _________________________________________________________________________ + member assign M:2, 9 + curve gen: m,m*100 m m*100 m m*100*(4) + +m*100(3) +m*100*(3) + + Function detail + Evaluations Function + m SimTK::SplineFitter:: + fitForSmoothingParameter(3,x,u,0).getSpline(); + Cost: ? + + m*100 SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal + Cost: Mult Add Assignments + 21 20 13 + + Total ~typically > 2100*m multiplications, additions, + > 1000*m assignments + > 100*m divisions + _________________________________________________________________________ + Comp Div Mult Add Assignments + Total: m+m*100(3) m*100 m*100*21 m*100*20 m*100*13 + +m+m*100*3 +m*100*4+9+M:2 + + m*Cost(SimTK::SplineFitter ...) + ========================================================================= + ADDITIONAL COST OF COMPUTING THE INTEGRAL CURVE + + Comp Div Mult Add Assign + RK45 Fn.Eval m*100*(156 12 618 390 456) + RK45 Overhead m*100*(? ? ? ? ? ) + Spline cost m*100*(? ? ? ? ? ) + + Total: ~typically > 100,000's mult, additions, assignments + > 40,000 comparisions + > 3000 divisions + + ========================================================================= + M: Matrix + V: Vector + + N.B. These costs are dependent on SegmentedQuinticBezierToolkit +*/ +SmoothSegmentedFunction:: + SmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name): + _x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1), + _name(name) +{ + + + _numBezierSections = mX.cols(); + _mXVec.resize(_numBezierSections); + _mYVec.resize(_numBezierSections); + for(int s=0; s < _numBezierSections; s++){ + _mXVec[s] = mX.col(s); + _mYVec[s] = mY.col(s); + } +} + +//============================================================================== + SmoothSegmentedFunction::SmoothSegmentedFunction(): + _x0(NAN),_x1(NAN), + _y0(NAN),_y1(NAN), + _dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET") + { + //_arraySplineUX.resize(0); + _mXVec.resize(0); + _mYVec.resize(0); + //_splineYintX = SimTK::Spline(); + _numBezierSections = (int)NAN; + } + +//============================================================================== +void SmoothSegmentedFunction:: + updSmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name) +{ + + if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ){ + cerr<<"SmoothSegmentedFunction::updSmoothSegmentedFunction " + << _name.c_str() + <<": matrices mX and mY must have 6 rows, and the same" + <<" number of columns." + << endl; + assert(0); + abort(); + } + + _x0 =x0; + _x1 =x1; + _y0 =y0; + _y1 =y1; + _dydx0=dydx0; + _dydx1=dydx1; + + if(mX.cols() != _mXVec.size()){ + _mXVec.resize(mX.cols()); + _mYVec.resize(mY.cols()); + } + + _numBezierSections = mX.cols(); + for(int s=0; s < mX.cols(); s++){ + _mXVec[s] = mX.col(s); + _mYVec[s] = mY.col(s); + } + + _name = name; +} +//============================================================================== +void SmoothSegmentedFunction::shift(double xShift, double yShift) +{ + _x0 += xShift; + _x1 += xShift; + _y0 += yShift; + _y1 += yShift; + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.at(i).rows();++j){ + _mXVec.at(i)[j] += xShift; + _mYVec.at(i)[j] += yShift; + } + } + +} + +void SmoothSegmentedFunction::scale(double xScale, double yScale) +{ + + if( abs( xScale ) <= SQRTEPS){ + cerr<<"SmoothSegmentedFunction::scale " + << _name.c_str() + <<": xScale must be greater than sqrt(eps). Setting xScale to such" + <<" a small value will cause the slope of the curve to approach " + <<" infinity, or become undefined." + << endl; + assert(0); + abort(); + } + + _x0 *= xScale; + _x1 *= xScale; + _y0 *= yScale; + _y1 *= yScale; + _dydx0 *= yScale/xScale; + _dydx1 *= yScale/xScale; + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.at(i).rows();++j){ + _mXVec.at(i)[j] *= xScale; + _mYVec.at(i)[j] *= yScale; + } + } + +} + + +//============================================================================== + + + /*Detailed Computational Costs + ________________________________________________________________________ + If x is in the Bezier Curve + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + SegmentedQuinticBezierToolkit:: + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 + calcQuinticBezierCurveVal 21 20 13 + total 15+3*m+2 2 103 62+1*m 76 + + *Approximate. Uses iteration +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + 1 1 2 1 +________________________________________________________________________ + + */ + +double SmoothSegmentedFunction::calcValue(double x) const +{ + double yVal = 0; + if(x >= _x0 && x <= _x1 ) + { + int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); + double u = SegmentedQuinticBezierToolkit:: + calcU(x,_mXVec[idx], UTOL, MAXITER); + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,_mYVec[idx]); + }else{ + if(x < _x0){ + yVal = _y0 + _dydx0*(x-_x0); + }else{ + yVal = _y1 + _dydx1*(x-_x1); + } + } + + return yVal; +} + +double SmoothSegmentedFunction::calcValue( + const RigidBodyDynamics::Math::VectorNd& ax) const +{ + + if( !(ax.size() == 1) ){ + cerr<<"SmoothSegmentedFunction::calcValue " << _name.c_str() + <<": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but a function with " + << ax.size() << " elements was entered" + << endl; + assert(0); + abort(); + + } + + return calcValue(ax[0]); +} + +/*Detailed Computational Costs +________________________________________________________________________ +If x is in the Bezier Curve, and dy/dx is being evaluated + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ +Overhead: + SegmentedQuinticBezierToolkit:: + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 + Derivative Evaluation: + **calcQuinticBezierCurveDYDX 21 20 13 + dy/du 20 19 11 + dx/du 20 19 11 + dy/dx 1 + + total 17+3*m 3 143 m+100 98 + +*Approximate. Uses iteration +**Higher order derivatives cost more +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + 1 1 +________________________________________________________________________ + */ + +double SmoothSegmentedFunction::calcDerivative(double x, int order) const +{ + //return calcDerivative( SimTK::Array_(order,0), + // RigidBodyDynamics::Math::VectorNd(1,x)); + double yVal = 0; + + //QUINTIC SPLINE + + + if(order==0){ + yVal = calcValue(x); + }else{ + if(x >= _x0 && x <= _x1){ + int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); + double u = SegmentedQuinticBezierToolkit:: + calcU(x,_mXVec[idx],UTOL,MAXITER); + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx], + _mYVec[idx], order); +/* + std::cout << _mX(3, idx) << std::endl; + std::cout << _mX(idx) << std::endl;*/ + }else{ + if(order == 1){ + if(x < _x0){ + yVal = _dydx0; + }else{ + yVal = _dydx1;} + }else{ + yVal = 0;} + } + } + + return yVal; +} + + + +double SmoothSegmentedFunction:: + calcDerivative( const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& ax) const +{ + /* + for(int i=0; i < (signed)derivComponents.size(); i++){ + SimTK_ERRCHK2_ALWAYS( derivComponents[i] == 0, + "SmoothSegmentedFunction::calcDerivative", + "%s: derivComponents can only be populated with 0's because " + "SmoothSegmentedFunction is only valid for a 1D function, but " + "derivComponents had a value of %i in it", + _name.c_str(), derivComponents[i]); + } + SimTK_ERRCHK2_ALWAYS( derivComponents.size() <= 6, + "SmoothSegmentedFunction::calcDerivative", + "%s: calcDerivative is only valid up to a 6th order derivative" + " but derivComponents had a size of %i", + _name.c_str(), derivComponents.size()); + + SimTK_ERRCHK2_ALWAYS( ax.size() == 1, + "SmoothSegmentedFunction::calcValue", + "%s: Argument x must have only 1 element, as this function is " + "designed only for 1D functions, but ax had a size of %i", + _name.c_str(), ax.size()); + */ + + for(int i=0; i < (signed)derivComponents.size(); i++){ + if( !(derivComponents[i] == 0)){ + cerr << "SmoothSegmentedFunction::calcDerivative " + << _name.c_str() + <<": derivComponents can only be populated with 0's because " + << "SmoothSegmentedFunction is only valid for a 1D function," + << " but derivComponents had a value of " + << derivComponents[i] << " in it" + << endl; + assert(0); + abort(); + + } + } + + if( !(derivComponents.size() <= 6)){ + cerr << "SmoothSegmentedFunction::calcDerivative " << _name.c_str() + << ": calcDerivative is only valid up to a 6th order derivative" + << " but derivComponents had a size of " + << derivComponents.size() + << endl; + assert(0); + abort(); + } + + if( !(ax.size() == 1) ){ + cerr << "SmoothSegmentedFunction::calcValue " << _name.c_str() + << ": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but ax had a size of " + << ax.size() + << endl; + assert(0); + abort(); + } + + + return calcDerivative(ax[0], derivComponents.size()); +} + +/*Detailed Computational Costs +________________________________________________________________________ +If x is in the Bezier Curve, and dy/dx is being evaluated + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + *spline.calcValue 7 2 3 1 + +*Approximate cost of evaluating a cubic spline with 100 knots, where +the bisection method is used to find the correct index +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + *spline.calcValue 1 2 3 1 + integral eval 2 4 5 1 + total 3 6 8 2 + +*Approximate cost of evaluating a cubic spline at its last knot point +________________________________________________________________________ + +*/ +/* +double SmoothSegmentedFunction::calcIntegral(double x) const +{ + SimTK_ERRCHK1_ALWAYS(_computeIntegral, + "SmoothSegmentedFunction::calcIntegral", + "%s: This curve was not constructed with its integral because" + "computeIntegral was false",_name.c_str()); + + double yVal = 0; + if(x >= _x0 && x <= _x1){ + yVal = _splineYintX.calcValue(RigidBodyDynamics::Math::VectorNd(1,x)); + }else{ + //LINEAR EXTRAPOLATION + if(x < _x0){ + RigidBodyDynamics::Math::VectorNd tmp(1); + tmp(0) = _x0; + double ic = _splineYintX.calcValue(tmp); + if(_intx0x1){//Integrating left to right + yVal = _y0*(x-_x0) + + _dydx0*(x-_x0)*(x-_x0)*0.5 + + ic; + }else{//Integrating right to left + yVal = -_y0*(x-_x0) + - _dydx0*(x-_x0)*(x-_x0)*0.5 + + ic; + } + }else{ + RigidBodyDynamics::Math::VectorNd tmp(1); + tmp(0) = _x1; + double ic = _splineYintX.calcValue(tmp); + if(_intx0x1){ + yVal = _y1*(x-_x1) + + _dydx1*(x-_x1)*(x-_x1)*0.5 + + ic; + }else{ + yVal = -_y1*(x-_x1) + - _dydx1*(x-_x1)*(x-_x1)*0.5 + + ic; + } + } + } + + return yVal; +} +*/ +/* +bool SmoothSegmentedFunction::isIntegralAvailable() const +{ + return _computeIntegral; +} +*/ + +/* +bool SmoothSegmentedFunction::isIntegralComputedLeftToRight() const +{ + return _intx0x1; +} +*/ + +int SmoothSegmentedFunction::getArgumentSize() const +{ + return 1; +} + +int SmoothSegmentedFunction::getMaxDerivativeOrder() const +{ + return 6; +} + +std::string SmoothSegmentedFunction::getName() const +{ + return _name; +} + +void SmoothSegmentedFunction::setName(const std::string &name) +{ + _name = name; +} + +RigidBodyDynamics::Math::VectorNd + SmoothSegmentedFunction::getCurveDomain() const +{ + RigidBodyDynamics::Math::VectorNd xrange(2); + + xrange[0] = 0; + xrange[1] = 0; + if (!_mXVec.empty()) { + xrange[0] = _mXVec[0][0]; + xrange[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1]; + } + return xrange; +} + +/////////////////////////////////////////////////////////////////////////////// +// Utility functions +/////////////////////////////////////////////////////////////////////////////// + +/*Detailed Computational Costs + +_______________________________________________________________________ + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + + *overhead (17+3*m 2 82 42+m 63)*7 + 119+21*m 14 574 294+7m 441 + + calcValue 21 20 13 + calcDerivative: dy/dx 1 40 38 22 + : d2y/dx2 2 78 73 23 + : d3y/dx3 4 118 105 58 + : d4y/dx4 5 168 137 71 + : d5y/dx5 7 236 170 88 + : d6y/dx6 9 334 209 106 + + **calcIntegral 7 2 3 1 + + total per point 126+21*m 42 1571 1049 823 + total per elbow 126k+21k*m 42k 1571k 1049k 823k + + *Approximate. Overhead associated with finding the correct Bezier + spline section, and evaluating u(x). + Assumes 2 Newton iterations in calcU + + **Approximate. Includes estimated cost of evaluating a cubic spline + with 100 knots +*/ +RigidBodyDynamics::Math::MatrixNd + SmoothSegmentedFunction::calcSampledCurve(int maxOrder, + double domainMin, + double domainMax) const{ + + int pts = 1; //Number of points between each of the spline points used + //to fit u(x), and also the integral spline + if( !(maxOrder <= getMaxDerivativeOrder()) ){ + cerr << "SmoothSegmentedFunction::calcSampledCurve " + << "Derivative order past the maximum computed order requested" + << endl; + assert(0); + abort(); + } + + double x0,x1,delta; + //y,dy,d1y,d2y,d3y,d4y,d5y,d6y,iy + RigidBodyDynamics::Math::VectorNd + midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); + RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); + + //Generate a sample of X values inside of the curve that is denser where + //the curve is more curvy. + double u; + int idx = 0; + for(int s=0; s < _numBezierSections; s++){ + //Sample the local set for u and x + for(int i=0;i 1){ + //Skip the last point of a set that has another set of points + //after it. Why? The last point and the starting point of the + //next set are identical in value. + if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){ + midX[idx] = x[i]; + idx++; + } + }else{ + midX[idx] = x[i]; + idx++; + } + } + } + + + RigidBodyDynamics::Math::VectorNd xsmpl(pts*(midX.size()-1)+2*10*pts); + + RigidBodyDynamics::Math::MatrixNd results; + + /* + if(_computeIntegral){ + results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2+1); + }else{ + + } + */ + results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2); + + //Array initialization is so ugly ... + std::vector d1y(1),d2y(2),d3y(3),d4y(4),d5y(5),d6y(6); + d1y[0]=0; + d2y[0] = 0; + d2y[1] = 0; + for(int i=0;i<3;i++) + d3y[i]=0; + for(int i=0;i<4;i++) + d4y[i]=0; + for(int i=0;i<5;i++) + d5y[i]=0; + for(int i=0;i<6;i++) + d6y[i]=0; + + //generate some sample points in the extrapolated region + idx = 0; + x0 = _x0 - 0.1*(_x1-_x0); + if(domainMin < x0) + x0 = domainMin; + + x1 = _x0; + delta = (0.1)*(x1-x0)/(pts); + + for(int j=0; j x1) + x1 = domainMax; + + delta = (1.0/9.0)*(x1-x0)/(pts); + + for(int j=0;j=1) + results(i,2) = calcDerivative(d1y,ax); + + if(maxOrder>=2) + results(i,3) = calcDerivative(d2y,ax); + + if(maxOrder>=3) + results(i,4) = calcDerivative(d3y,ax); + + if(maxOrder>=4) + results(i,5) = calcDerivative(d4y,ax); + + if(maxOrder>=5) + results(i,6) = calcDerivative(d5y,ax); + + if(maxOrder>=6) + results(i,7) = calcDerivative(d6y,ax); + + /* + if(_computeIntegral){ + results(i,maxOrder+2) = calcIntegral(ax(0)); + } + */ + } + return results; +} + +void SmoothSegmentedFunction::getXControlPoints( + RigidBodyDynamics::Math::MatrixNd& mat) const +{ + mat.resize(_mXVec.size(), _mXVec.at(0).size()); + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.size();++j){ + mat(i,j) = _mXVec.at(i)[j]; + } + } + +} +void SmoothSegmentedFunction::getYControlPoints( + RigidBodyDynamics::Math::MatrixNd& mat) const +{ +mat.resize(_mYVec.size(), _mYVec.at(0).size()); + +for(int i=0; i<_mYVec.size();++i){ + for(int j=0; j<_mYVec.size();++j){ + mat(i,j) = _mYVec.at(i)[j]; + } +} + +} + + +/*Detailed Computational Costs + +_______________________________________________________________________ + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + + *overhead (17+3*m 2 82 42+m 63)*3 + 51+9m 6 246 126+3m 189 + + calcValue 21 20 13 + calcDerivative : dy/dx 1 40 38 22 + : d2y/dx2 2 78 73 23 + + **calcIntegral 7 2 3 1 + + total per point 58+9m 9 387 260+3m 248 + total per elbow 5.8k+900m 900 38.7k 26k+300m 24.8k + + *Approximate. Overhead associated with finding the correct Bezier + spline section, and evaluating u(x). + Assumes 2 Newton iterations in calcU + + **Approximate. Includes estimated cost of evaluating a cubic spline + with 100 knots +*/ +void SmoothSegmentedFunction::printCurveToCSVFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + double domainMin, + double domainMax) const +{ + //Only compute up to the 2nd derivative + RigidBodyDynamics::Math::MatrixNd results = + calcSampledCurve(2,domainMin,domainMax); + std::vector colNames(results.cols()); + colNames[0] = "x"; + colNames[1] = "y"; + colNames[2] = "dy/dx"; + colNames[3] = "d2y/dx2"; + /* + if(results.cols() == 5){ + colNames[4] = "int_y(x)"; + } + */ + std::string fname = fileNameWithoutExtension; + fname.append(".csv"); + printMatrixToFile(results,colNames,path,fname); +} +/* +This function will print cvs file of the column vector col0 and the matrix data +*/ +void SmoothSegmentedFunction:: + printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, + std::vector& colNames, + const std::string& path, + const std::string& filename) const +{ + + ofstream datafile; + std::string fullpath = path; + + if(fullpath.length() > 0) + fullpath.append("/"); + + fullpath.append(filename); + + datafile.open(fullpath.c_str(),std::ios::out); + + if(!datafile){ + datafile.close(); + cerr << "SmoothSegmentedFunction::printMatrixToFile " + << _name.c_str() << ": Failed to open the file path: " + << fullpath.c_str() + << endl; + assert(0); + abort(); + } + + + for(int i = 0; i < (signed)colNames.size(); i++){ + if(i < (signed)colNames.size()-1) + datafile << colNames[i] << ","; + else + datafile << colNames[i] << "\n"; + } + + for(int i = 0; i < data.rows(); i++){ + for(int j = 0; j < data.cols(); j++){ + if(j +#include + + + + /** + This class contains the quintic Bezier curves, x(u) and y(u), that have been + created by SmoothSegmentedFunctionFactory to follow a physiologically + meaningful muscle characteristic. A SmoothSegmentedFunction cannot be + created directly,you must use SmoothSegmentedFunctionFactory to create the + muscle curve of interest. + + Computational Cost Details + All computational costs assume the following operation costs: + + \verbatim + Operation Type : #flops + +,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 + \endverbatim + + These relative weightings will vary processor to processor, and so any + of the quoted computational costs are approximate. + + + RBDL Port Notes +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + + + @author Matt Millard + @version 0.0 + + + */ + +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + + class SmoothSegmentedFunction : public Function_ + { + + + public: + + ///The default constructor, which populates the member data fields with + ///NaN's + SmoothSegmentedFunction(); + + //SmoothSegmentedFunction(); + /** + Creates a set of quintic Bezier curves. + + @param mX The matrix of quintic Bezier x point locations (6xn). + Each column vector is the 6 control points required + for each quintic Bezier curve. For C0 continuity + adjacent columns must share the last and first control + points. For C1 continuity the last 2 and first two + control points of adjacent curves should be on the same + curve. + + @param mY The matrix of quintic Bezier y point locations (6xn). + + @param x0 The minimum x value. This is used for the linear + extrapolation of the Bezier curve. This parameter is + explicitly asked for, rather than computed, to prevent + rounding error from reducing the accuracy of the + linear extrapolation. + @param x1 The maximum x value. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y0 The value of y(x) at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y1 The value of y(x) at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx0 The value of dy/dx at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx1 The value of dy/dx at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + + Computational Costs + Generating the integral curve is not cheap, and so should only be used + when if it will be evaluated during a simulation. + \verbatim + Computatonal Cost Per Bezier Section: + Without Integral : 4,100 flops + \endverbatim + + */ + SmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name); + + + /** + Updates the Bezier curve conrol points. The updated curve must have + the same number of control points as the original curve. + + @param mX The matrix of quintic Bezier x point locations (6xn). + Each column vector is the 6 control points required + for each quintic Bezier curve. For C0 continuity + adjacent columns must share the last and first control + points. For C1 continuity the last 2 and first two + control points of adjacent curves should be on the same + curve. + + @param mY The matrix of quintic Bezier y point locations (6xn). + + @param x0 The minimum x value. This is used for the linear + extrapolation of the Bezier curve. This parameter is + explicitly asked for, rather than computed, to prevent + rounding error from reducing the accuracy of the + linear extrapolation. + @param x1 The maximum x value. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y0 The value of y(x) at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y1 The value of y(x) at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx0 The value of dy/dx at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx1 The value of dy/dx at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + + */ + void updSmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name); + + /** + This function will shift the entire SmoothSegmentedFunction by xShift + and yShift. Setting xShift = yShift = 0.0 will leave the curve unchanged. + + @param xShift - the amount to shift the curve in the x-direction. + @param yShift - the amount to shift the curve in the y-direction + */ + void shift(double xShift, double yShift); + + /** + This function will scale the curve in the x and y directions. Setting + xScale=yScale=1.0 will leave the curve unchanged. + + \b aborts \b + -If abs(xScale) < sqrt(eps) + + @param xScale: the amount to scale the curve in the x direction + @param yScale: the amount to scale the curve in the y direction + */ + void scale(double xScale, double yScale); + + + + /**Calculates the value of the curve this object represents. + + @param x The domain point of interest + + \b aborts \b + -If ax does not have a size of 1 + @returns The value of the curve + + + The curve is parameterized as a set of Bezier curves. If x is within the + domain of these Bezier curves they will be evaluated. If x is outside + of the domain of these Bezier curves a linear extrapolation will be + evalulated + + + Computational Costs + \verbatim + x in curve domain : ~282 flops + x in linear section: ~5 flops + \endverbatim + */ + double calcValue(double x) const; + + + + /**Calculates the value of the derivative of the curve this object + represents. + + @param x The domain point of interest. + + @param order The order of the derivative to compute. Note that order must + be between 0 and 2. Calling 0 just calls calcValue. + + \b aborts \b + -If anything but 0's are stored in derivComponents + -If more than the 6th derivative is asked for + -If ax has a size other than 1 + + @return The value of the d^ny/dx^n th derivative evaluated at x + + + + Computational Costs + \verbatim + x in curve domain : ~391 flops + x in linear section: ~2 flops + \endverbatim + + */ + double calcDerivative(double x, int order) const; + + + + + /*This will return the value of the integral of this objects curve + evaluated at x. + + @param x the domain point of interest + + \b aborts \b + -If the function does not have a pre-computed integral + @return the value of the functions integral evaluated at x + + The integral is approximate, though its errors are small. + The integral is computed by numerically integrating the function when + the constructor for this class is called (if computeIntegral is true) and + then splining the result, thus the regions between the knot points may + have some error in them. A very fine mesh of points is used to create the + spline so the errors will be small + + Computational Costs + \verbatim + x in curve domain : ~13 flops + x in linear section: ~19 flops + \endverbatim + + */ + //double calcIntegral(double x) const; + + /* + Returns a bool that indicates if the integral curve has been computed. + + @return true if the integral of this function is available, false if + it has not been computed. + */ + //bool isIntegralAvailable() const; + + /* + Returns a bool that indicates if the integral computed is compuated left + to right, or right to left. + + @return true if the integral was computed left to right, and false if the + integral was computed right to left. Note that the output of + this function is only valid if isIntegralAvailable() returns + true. + */ + //bool isIntegralComputedLeftToRight() const; + + /** + Returns a string that is the name for this curve, which is set at the + time of construction and cannot be changed after construction. + + @return The string name this object was given during construction*/ + std::string getName() const; + + /** + Sets the name of the SmoothSegmentedFunction object. + + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + */ + void setName(const std::string &name); + + /** + This function returns a SimTK::Vec2 that contains in its 0th element + the lowest value of the curve domain, and in its 1st element the highest + value in the curve domain of the curve. Outside of this domain the curve + is approximated using linear extrapolation. + + @return The minimum and maximum value of the domain, x, of the curve + y(x). Within this range y(x) is a curve, outside of this range + the function y(x) is a C2 (continuous to the second + derivative) linear extrapolation*/ + RigidBodyDynamics::Math::VectorNd getCurveDomain() const; + + /**This function will generate a csv file (of 'name_curveName.csv', where + name is the one used in the constructor) of the muscle curve, and + 'curveName' corresponds to the function that was called from + SmoothSegmentedFunctionFactory to create the curve. + + @param path The full path to the location. Note '/' slashes must be used, + and do not put a '/' after the last folder. + @param fileNameWithoutExtension The name of the file to write, not + including the file extension + @param domainMin + the left most domain point of the curve to print. The curve + will extend to at least this point. + @param domainMax + the right most domain point of the curve to print. The + printed curve will extend at least to this point, perhaps + beyond. + \b aborts \b + -If the filename is empty + + For example the tendon + curve for a muscle named 'glutmax' will be: + + 'glutmax_tendonForceLengthCurve.csv' + + The file will contain the following columns: + + \verbatim + Col# 1, 2, 3, 4, 5 + x, y, dy/dx, d2y/dx2, iy + \endverbatim + + Where iy is the integral of y(x). If the curve has been set not to have + an integral, this column will not exist. + + The curve will be sampled from its linear extrapolation region, through + the curve, out to the other linear extrapolation region. The width of + each linear extrapolation region is 10% of the entire range of x, or + 0.1*(x1-x0). + + The number of rows used will vary from curve to curve. Each quintic + Bezier curve section will have 100 samples. Each linearily extrapolated + region will have 10 samples each. Some muscle curves (the tendon, + parallel elements, compressive elements) consist of only 1 elbow, and so + these matrices will have only 100+20 rows. The force velocity curve is + made up of 2 elbows and will have 200+20 rows. The active force length + curve has 5 elbows, and so its sampled matrix will have 500+20 rows + + Computational Costs + This varies depending on the curve (as mentioned above). + \verbatim + ~97,400 to 487,000 flops + \endverbatim + + Example + To read the csv file with a header in from Matlab, you need to use + csvread set so that it will ignore the header row. This is accomplished + by using the extra two numerical arguments for csvread to tell the + function to begin reading from the 1st row, and the 0th index (csvread + is 0 indexed). + \verbatim + data=csvread('test_tendonForceLengthCurve.csv',1,0); + \endverbatim + + */ + void printCurveToCSVFile(const std::string& path, + const std::string& fileNameWithoutExtension, + double domainMin, + double domainMax) const; + + + /** + @param maxOrder The maximum derivative order to compute + @param domainMin + @param domainMax + \b aborts \b + -If the requested derivatve order is greater than + getMaxDerivativeOrder() + @returns a matrix populated with x,y,dy/dx ... d^ny/dx^n,iy + + + This function will generate a RigidBodyDynamics::Math::MatrixNd populated + with samples of the muscle curves values, derivatives (up to 6) and its + first integral (if available). The matrix has the following columns: + + \verbatim + Col# 1, 2, 3, 4, 5, 6, 7, 8, 9, + x, y, dy/dx, d2y/dx2, d3y/dx3, d4y/dx4, d5y/dx5, d6y/dx6, iy + \endverbatim + + Where iy is the integral of y(x). If the curve has been set not to have + an integral, this column will not exist. + + The curve will be sampled from its + linear extrapolation region, through the curve, out to the other linear + extrapolation region. The width of each linear extrapolation region is + 10% of the entire range of x, or 0.1*(x1-x0). + + The rows used will vary from curve to curve. Each quintic Bezier curve + section will have 100 samples + 20 samples for the linear extrapolation + region. Some muscle curves (the tendon, parallel elements, compressive + elements) consist of only 1 elbow, and so these matrices will have only + 100+20 rows. The force velocity curve is made up of 2 elbows and will + have 200+20 rows. The active force length curve has 5 elbows, and so its + sampled matrix will have 500+20 rows + + */ + RigidBodyDynamics::Math::MatrixNd calcSampledCurve(int maxOrder, + double domainMin, + double domainMax) const; + + + void getXControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const; + void getYControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const; + + private: + + + /**Nov 2015: Not needed in the RBDL port. + Array of spline fit functions X(u) for each Bezier elbow*/ + //Nov 2015: Not needed in the RBDL port. + //SimTK::Array_ _arraySplineUX; + /**Nov 2015: Not included in the RBDL port (RBDL doesn't have an + error controlled integrator to generate this curve) + Spline fit of the integral of the curve y(x)*/ + //SimTK::Spline _splineYintX; + + /**Bezier X1,...,Xn control point locations. Control points are + stored in 6x1 vectors in the order above*/ + std::vector _mXVec; + /**Bezier Y1,...,Yn control point locations. Control points are + stored in 6x1 vectors in the order above*/ + std::vector _mYVec; + + /**The number of quintic Bezier curves that describe the relation*/ + int _numBezierSections; + + /**The minimum value of the domain*/ + double _x0; + /**The maximum value of the domain*/ + double _x1; + /**The minimum value of the range*/ + double _y0; + /**The maximum value of the range*/ + double _y1; + /**The slope at _x0*/ + double _dydx0; + /**The slope at _x1*/ + double _dydx1; + /*This is the users */ + //bool _computeIntegral; + + /*This variable, when true, indicates that the user wants the integral + from left to right (x0 to x1). If it is false, the integral from right + to left (x1 to x0) is computed*/ + //bool _intx0x1; + /**The name of the function**/ + std::string _name; + + /**No human should be constructing a SmoothSegmentedFunction, so the + constructor is made private so that mere mortals cannot look at it. + SmoothSegmentedFunctionFactory should be used to create + MuscleCurveFunctions and that's why its a friend*/ + friend class SmoothSegmentedFunctionFactory; + + + + + + /** + This function will print cvs file of the column vector col0 and the + matrix data + + @param data A matrix of data + @param colnames Array of column headings + @param path The desired path to the folder to write the file + @param filename The name of the file to print + \b aborts \b + -If the desired file cannot be created and openened, perhaps + because the path doesn't exist. + */ + void printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, + std::vector& colnames, + const std::string& path, + const std::string& filename) const; + + /** + Refer to the documentation for calcValue(double x) + because this function is identical in function to + calcValue(double x), but requires different inputs. + This is a required virtual function required because this class extends + the Function interface. + */ + double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const; + /*virtual*/ + + /** Refer to the documentation for calcDerivative(double x, int order) + because this function is identical in function to + calcDerivative(double x, int order), but requires different inputs. + This is a required virtual function required because this class extends + the Function interface.*/ + double calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const; + /*virtual*/ + + /**This will return the size of the vector that the + calcValue(const RigidBodyDynamics::Math::VectorNd& x) require. This is a + required virtual function required because this class extends the + Function interface, though is only needed if you call + + double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const; + + or + + double calcDerivative( const SimTK::Array_& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const; + + Since this class is implementing strictly scalar functions you can use + the simplified versions of calcValue(double x) and + calcDerivative(double x, int order) instead. + + */ + int getArgumentSize() const; /*virtual*/ + + /**@return The maximum order derivative that this object is capable of + returning*/ + /*virtual*/ + int getMaxDerivativeOrder() const; + + + }; + +} +} +} + + + +#endif diff --git a/3rdparty/rbdl/addons/geometry/geometry.h b/3rdparty/rbdl/addons/geometry/geometry.h new file mode 100644 index 0000000..702c2fb --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/geometry.h @@ -0,0 +1,16 @@ +/* + * + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + + +#ifndef GEOMETRY_H_ +#define GEOMETRY_H_ + +#include "Function.h" +#include "SegmentedQuinticBezierToolkit.h" +#include "SmoothSegmentedFunction.h" + +#endif diff --git a/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt b/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt new file mode 100644 index 0000000..45931fa --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt @@ -0,0 +1,67 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) +CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION + ${RBDL_ADDON_GEOMETRY_TESTS_VERSION_MAJOR}.${RBDL_ADDON_GEOMETRY_TESTS_VERSION_MINOR}.${RBDL_ADDON_GEOMETRY_TESTS_VERSION_PATCH} +) + + +PROJECT (RBDL_GEOMETRY_TESTS VERSION ${RBDL_ADDON_GEOMETRY_TESTS_VERSION}) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) +INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) + +SET ( GEOMETRY_TESTS_SRCS + testSmoothSegmentedFunction.cc + numericalTestFunctions.cc + numericalTestFunctions.h + ../geometry.h + ../SegmentedQuinticBezierToolkit.h + ../SmoothSegmentedFunction.h + ../SegmentedQuinticBezierToolkit.cc + ../SmoothSegmentedFunction.cc + ) + +INCLUDE_DIRECTORIES ( ../ ) + + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +ADD_EXECUTABLE ( rbdl_geometry_tests ${GEOMETRY_TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_geometry_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runGeometryTests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +TARGET_LINK_LIBRARIES ( rbdl_geometry_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) +ADD_CUSTOM_COMMAND (TARGET rbdl_geometry_tests + POST_BUILD + COMMAND ./runGeometryTests + COMMENT "Running automated addon geometry tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) \ No newline at end of file diff --git a/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc b/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc new file mode 100644 index 0000000..6afe038 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc @@ -0,0 +1,534 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + + +#include "numericalTestFunctions.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + + +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename) +{ + + ofstream datafile; + datafile.open(filename.c_str()); + + for(int i = 0; i < data.rows(); i++){ + datafile << col0[i] << ","; + for(int j = 0; j < data.cols(); j++){ + if(j y_C3max){ + c3 = 1; + y = y_C3max; + } + + h = pow( ( (EPSILON*y*2.0)/(c3) ) , 1.0/3.0); + + //Now check that h to the left and right are at least similar + //If not, take the smallest one. + xL = x[i]-h/2; + xR = x[i]+h/2; + + fL = mcf.calcDerivative(xL, order-1); + fR = mcf.calcDerivative(xR, order-1); + + //Just for convenience checking ... + dyNUM = (fR-fL)/h; + dy = mcf.calcDerivative(x[i],order); + err = abs(dy-dyNUM); + + /*if(err > tol && abs(dy) > rootEPS && order <= 2){ + err = err/abs(dy); + if(err > tol) + cout << "rel tol exceeded" << endl; + }*/ + + dyV[i] = dyNUM; + + } + + + return dyV; +} + +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult) +{ + bool flag_continuous = true; + + double xL = 0; // left shoulder point + double xR = 0; // right shoulder point + double yL = 0; // left shoulder point function value + double yR = 0; // right shoulder point function value + double dydxL = 0; // left shoulder point derivative value + double dydxR = 0; // right shoulder point derivative value + + double xVal = 0; //x value to test + double yVal = 0; //Y(x) value to test + + double yValEL = 0; //Extrapolation to yVal from the left + double yValER = 0; //Extrapolation to yVal from the right + + double errL = 0; + double errR = 0; + + double errLMX = 0; + double errRMX = 0; + + + for(int i =1; i < xV.size()-1; i++){ + xVal = xV[i]; + yVal = yV.calcDerivative(xVal, order); + + xL = 0.5*(xV[i]+xV[i-1]); + xR = 0.5*(xV[i]+xV[i+1]); + + yL = yV.calcDerivative(xL,order); + yR = yV.calcDerivative(xR,order); + + dydxL = yV.calcDerivative(xL,order+1); + dydxR = yV.calcDerivative(xR,order+1); + + + yValEL = yL + dydxL*(xVal-xL); + yValER = yR - dydxR*(xR-xVal); + + errL = abs(yValEL-yVal); + errR = abs(yValER-yVal); + + errLMX = abs(yV.calcDerivative(xL,order+2)*0.5*(xVal-xL)*(xVal-xL)); + errRMX = abs(yV.calcDerivative(xR,order+2)*0.5*(xR-xVal)*(xR-xVal)); + + errLMX*=taylorErrorMult; + errRMX*=taylorErrorMult; + + if(errLMX < minValueSecondDerivative) + errLMX = minValueSecondDerivative; + + if(errRMX < minValueSecondDerivative) + errRMX = minValueSecondDerivative; // to accomodate numerical + //error in errL + + if(errL > errLMX || errR > errRMX){ + flag_continuous = false; + } + } + + return flag_continuous; +} + +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS) +{ + double dir = y[y.size()-1]-y[0]; + bool isMonotonic = true; + + if(dir < 0){ + for(int i =1; i y[i-1]+EPSILON*multEPS){ + isMonotonic = false; + //printf("Monotonicity broken at idx %i, since %fe-16 > %fe-16\n", + // i,y(i)*1e16,y(i-1)*1e16); + printf("Monotonicity broken at idx %i, since " + "y(i)-y(i-1) < tol, (%f*EPSILON < EPSILON*%i) \n", + i,((y[i]-y[i-1])/EPSILON), multEPS); + } + } + } + if(dir > 0){ + for(int i =1; i = 0; i=i-1){ + width = abs(x[i]-x[i+1]); + inty[i] = inty[i+1] + width*(0.5)*(y[i]+y[i+1]); + } + } + + + return inty; +} + + +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b) +{ + double error = 0; + double cerror=0; + for(int i = 0; i< a.size(); i++) + { + cerror = abs(a[i]-b[i]); + if(cerror > error){ + error = cerror; + } + } + return error; +} + + + +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol) +{ + //cout << " TEST: C2 Continuity " << endl; + + int multC0 = 5; + int multC1 = 50; + int multC2 = 200; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,0); + } + + + bool c0 = isFunctionContinuous(fcnSample, mcf, 0, continuityTol, multC0); + bool c1 = isFunctionContinuous(fcnSample, mcf, 1, continuityTol, multC1); + bool c2 = isFunctionContinuous(fcnSample, mcf, 2, continuityTol, multC2); + + + + return (c0 && c1 && c2); + //printf( " passed: C2 continuity established to a multiple\n" + // " of the next Taylor series error term.\n " + // " C0,C1, and C2 multiples: %i,%i and %i\n", + // multC0,multC1,multC2); + //cout << endl; +} + + +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample) +{ + //cout << " TEST: Monotonicity " << endl; + int multEps = 10; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,1); + } + + bool monotonic = isVectorMonotonic(fcnSample,10); + return monotonic; + //printf(" passed: curve is monotonic to %i*EPSILON",multEps); + //cout << endl; +} + + +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol) +{ + //cout << " TEST: Derivative correctness " << endl; + int maxDer = 4;//mcf.getMaxDerivativeOrder() - 2; + + RigidBodyDynamics::Math::MatrixNd numSample(mcfSample.rows(),maxDer); + RigidBodyDynamics::Math::MatrixNd relError(mcfSample.rows(),maxDer); + + + RigidBodyDynamics::Math::VectorNd domainX = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int j=0; j tol){ + relError(j,i) = relError(j,i)/mcfSample(j,i+2); + } + } + + } + + RigidBodyDynamics::Math::VectorNd errRelMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + RigidBodyDynamics::Math::VectorNd errAbsMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + + double absTol = 5*tol; + + bool flagError12=false; + RigidBodyDynamics::Math::VectorNd tolExceeded12V = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + int tolExceeded12 = 0; + int tolExceeded34 = 0; + for(int j=0;j tol && mcfSample(i,j+2) > tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + if(mcfSample(i,j+2) > tol) + if(errRelMax[j] < abs(relError(i,j))) + errRelMax[j] = abs(relError(i,j)); + + //This is a harder test: here we're comparing absolute error + //so the tolerance margin is a little higher + if(relError(i,j) > absTol && mcfSample(i,j+2) <= tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + + if(mcfSample(i,j+2) < tol) + if(errAbsMax[j] < abs(relError(i,j))) + errAbsMax[j] = abs(relError(i,j)); + + + } + + /* + if(flagError12 == true){ + printf("Derivative %i Rel Error Exceeded:\n",j); + printf("x dx_relErr dx_calcVal dx_sample" + " dx2_relErr dx2_calcVal dx2_sample\n"); + for(int i=0; i +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + +static double EPSILON = numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); +static double TOL = std::numeric_limits::epsilon()*1e4; + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; + +/** +This function will print cvs file of the column vector col0 and the matrix + data + +@params col0: A vector that must have the same number of rows as the data matrix + This column vector is printed as the first column +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** +This function will print cvs file of the matrix + data + +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** + This function computes a standard central difference dy/dx. If + extrap_endpoints is set to 1, then the derivative at the end points is + estimated by linearly extrapolating the dy/dx values beside the end points + + @param x domain vector + @param y range vector + @param extrap_endpoints: (false) Endpoints of the returned vector will be + zero, because a central difference + is undefined at these endpoints + (true) Endpoints are computed by linearly + extrapolating using a first difference from + the neighboring 2 points + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd + calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool extrap_endpoints); + +/** + This function computes a standard central difference dy/dx at each point in + a vector x, for a SmoothSegmentedFunction mcf, to a desired tolerance. This + function will take the best step size at each point to minimize the + error caused by taking a numerical derivative, and the error caused by + numerical rounding error: + + For a step size of h/2 to the left and to the right of the point of + interest the error is + error = 1/4*h^2*c3 + r*f(x)/h, (1) + + Where c3 is the coefficient of the 3rd order Taylor series expansion + about point x. Thus c3 can be computed if the order + 2 derivative is + known + + c3 = (d^3f(x)/dx^3)/(6) (2) + + And r*f(x)/h is the rounding error that occurs due to the central + difference. + + Taking a first derivative of 1 and solving for h yields + + h = (r*f(x)*2/c3)^(1/3) + + Where r is EPSILON + + @param x domain vector + @param mcf the SmoothSegmentedFunction of interest + @param order the order of the numerical derivative + @param tolerance desired tolerance on the returned result + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd +calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + SmoothSegmentedFunction& mcf, + double tol, int order); + +/** + This function tests numerically for continuity of a curve. The test is + performed by taking a point on the curve, and then two points (called the + shoulder points) to the left and right of the point in question. The + shoulder points are located half way between the sample points in xV. + + The value of the function's derivative is evaluated at each of the shoulder + points and used to linearly extrapolate from the shoulder points back to the + original point. If the original point and the linear extrapolations of each + of the shoulder points agree within a tolerance, then the curve is assumed + to be continuous. This tolerance is evaluated to be the smaller of the 2nd + derivative times a multiplier (taylorErrorMult) or minValueSecondDerivative + + + @param x Values to test for continuity + + @param yx The SmoothSegmentedFunction to test + + @param order The order of the curve of SmoothSegmentedFunction to test + for continuity + + @param minValueSecondDerivative the minimum value allowed for the 2nd + term in the Taylor series. Here we need to define a minimum because + this 2nd term is used to define a point specific tolerance for the + continuity test. If the 2nd derivative happens to go to zero, we + still cannot let the minimum error tolerance to go to zero - else + the test will fail on otherwise good curves. + + @param taylorErrorMult This scales the error tolerance. The default error + tolerance is the the 2nd order Taylor series term. This term is + dependent on the size of the higher-order-terms and the sample + spacing used for xV (since the shoulder points are picked half- + way between the sampled points) +*/ +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult); + + +/** +This function will scan through a vector and determine if it is monotonic or +not + +@param y the vector of interest +@param multEPS The tolerance on the monotoncity check, expressed as a scaling of + EPSILON +@return true if the vector is monotonic, false if it is not +*/ +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS); + + +/** +This function will compute the numerical integral of y(x) using the trapezoidal +method + +@param x the domain vector +@param y the range vector, of y(x), evaluated at x +@param flag_TrueIntForward_FalseIntBackward + When this flag is set to true, the integral of y(x) will be evaluated from + left to right, starting with int(y(0)) = 0. When this flag is false, then + y(x) will be evaluated from right to left with int(y(n)) = 0, where n is + the maximum number of elements. +@return the integral of y(x) +*/ +RigidBodyDynamics::Math::VectorNd calcTrapzIntegral( + RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool flag_TrueIntForward_FalseIntBackward); + + +/** + @param a The first vector + @param b The second vector + @return Returns the maximum absolute difference between vectors a and b +*/ +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b); + + +/* +This function tests the SmoothSegmentedFunction to see if it is C2 continuous. +This function works by using the applying the function isFunctionContinuous +multiple times. For details of the method used please refer to the doxygen for +isFunctionContinuous. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. +@param continuityTol +@return bool: true if the curve is C2 continuous to the desired tolernace + +*/ +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol); + +/* + 4. The MuscleCurveFunctions which are supposed to be monotonic will be + tested for monotonicity. +*/ +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample); + +/** +This function compares the i^th derivative return by a SmoothSegmented function +against a numerical derivative computed using a central difference applied to +the (i-1)^th derivative of the function. This function first checks the +1st derivative and continues checking derivatives until the 4th derivative. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. + +@param tol : the tolerance used to assess the relative error between the + numerically computed derivatives and the derivatives returned by + the SmoothSegmentedFunction +@return bool: true if all of the derivatives up to the 4th (hard coded) are + within a relative tolerance of tol w.r.t. to the numerically + computed derivatives +*/ +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol); diff --git a/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc b/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc new file mode 100644 index 0000000..e9596e4 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc @@ -0,0 +1,483 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + +#include "geometry.h" +#include "numericalTestFunctions.h" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + + + + +/** + This function will create a quintic Bezier curve y(x) and sample it, its + first derivative w.r.t. U (dx(u)/du and dy(u)/du), and its first derivative + w.r.t. to X and print it to the screen. +*/ +void testSegmentedQuinticBezierDerivatives( + int maximumNumberOfToleranceViolations) +{ + //cout <<"**************************************************"<tol) + //printf("Error > Tol: (%i,%i): %f > %f\n",j,i,errorDer(j,i),tol); + } + } + //errorDer.cwiseAbs(); + //cout << errorDer << endl; + + //printf("...absolute tolerance of %f met\n", tol); + + //cout << " TEST: Bezier Curve Derivative DYDX to d6y/dx6" << endl; + RigidBodyDynamics::Math::MatrixNd numericDerXY(analyticDerXU.rows(), 6); + RigidBodyDynamics::Math::MatrixNd analyticDerXY(analyticDerXU.rows(),6); + + for(int i=0; i< analyticDerXU.rows(); i++) + { + analyticDerXY(i,0) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,1); + analyticDerXY(i,1) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,2); + analyticDerXY(i,2) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,3); + analyticDerXY(i,3) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,4); + analyticDerXY(i,4) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,5); + analyticDerXY(i,5) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,6); + } + + + for(int j=0; jrelTol){ + //printf("Error > Tol: (%i,%i): %f > %f\n",i,j, + // errorDerXY(i,j),relTol); + relTolExceeded++; + } + } + } + //cout << relTolExceeded << endl; + + //The relative tolerance gets exceeded occasionally in locations of + //rapid change in the curve. Provided there are only a few locations + //where the relative tolerance of 5% is broken, the curves should be + //regarded as being good. Ten errors out of a possible 100*6 data points + //seems relatively small. + CHECK(relTolExceeded < maximumNumberOfToleranceViolations); + + + //std::string fname = "analyticDerXY.csv"; + //printMatrixToFile(analyticDerXY,fname); + //fname = "numericDerXY.csv"; + //printMatrixToFile(numericDerXY,fname); + //fname = "errorDerXY.csv"; + //printMatrixToFile(errorDerXY,fname); + //printf(" ...relative tolerance of %f not exceeded more than %i times\n" + // " across all 6 derivatives, with 100 samples each\n", + // relTol, 10); + //cout <<"**************************************************"< + +DESCRIPTION + +rbdl_luamodel allows to load models that are specified as Lua scripts. Lua +is an open-source light-weight scripting language (http://www.lua.org). + +This addon comes with a standalone utility that can show various +information of a lua model such as degree of freedom overview or model +hierarchy. It is located in addons/luamodel/rbdl_luamodel_test. Use the -h +switch to see available options. + +Note: this addon is not even remotely as thoroughly tested as the RBDL +itself so please use it with some suspicion. + +DOCUMENTATION + +The documentation for this addon is built with the RBDL documentation. You +can find it in Modules -> Addon: rbdl_luamodel. + +LICENSING + +This code is published under the zlib license, however some parts of the +CMake scripts are taken from other projects and are licensed under +different terms. + +Full license text: + +------- +rbdl_luamodel - load RBDL models from Lua files +Copyright (c) 2011-2012 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/luamodel/luamodel.cc b/3rdparty/rbdl/addons/luamodel/luamodel.cc new file mode 100644 index 0000000..cc82b5c --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luamodel.cc @@ -0,0 +1,508 @@ +#include "rbdl/rbdl.h" +#include "luamodel.h" + +#include +#include + +#include "luatables.h" + +extern "C" +{ +#include +#include +#include +} + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +template<> +Vector3d LuaTableNode::getDefault(const Vector3d &default_value) { + Vector3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + cerr << "LuaModel Error: invalid 3d vector!" << endl; + abort(); + } + + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + } + + stackRestore(); + + return result; +} + +template<> +SpatialVector LuaTableNode::getDefault( + const SpatialVector &default_value +) { + SpatialVector result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 6) { + cerr << "LuaModel Error: invalid 6d vector!" << endl; + abort(); + } + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + result[3] = vector_table[4]; + result[4] = vector_table[5]; + result[5] = vector_table[6]; + } + + stackRestore(); + + return result; +} + +template<> +Matrix3d LuaTableNode::getDefault(const Matrix3d &default_value) { + Matrix3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + cerr << "LuaModel Error: invalid 3d matrix!" << endl; + abort(); + } + + if (vector_table[1].length() != 3 + || vector_table[2].length() != 3 + || vector_table[3].length() != 3) { + cerr << "LuaModel Error: invalid 3d matrix!" << endl; + abort(); + } + + result(0,0) = vector_table[1][1]; + result(0,1) = vector_table[1][2]; + result(0,2) = vector_table[1][3]; + + result(1,0) = vector_table[2][1]; + result(1,1) = vector_table[2][2]; + result(1,2) = vector_table[2][3]; + + result(2,0) = vector_table[3][1]; + result(2,1) = vector_table[3][2]; + result(2,2) = vector_table[3][3]; + } + + stackRestore(); + + return result; +} + +template<> +SpatialTransform LuaTableNode::getDefault( + const SpatialTransform &default_value +) { + SpatialTransform result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + result.r = vector_table["r"].getDefault(Vector3d::Zero(3)); + result.E = vector_table["E"].getDefault(Matrix3d::Identity (3,3)); + } + + stackRestore(); + + return result; +} + +template<> +Joint LuaTableNode::getDefault(const Joint &default_value) { + Joint result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + int joint_dofs = vector_table.length(); + + if (joint_dofs == 1) { + string dof_string = vector_table[1].getDefault(""); + if (dof_string == "JointTypeSpherical") { + stackRestore(); + return Joint(JointTypeSpherical); + } else if (dof_string == "JointTypeEulerZYX") { + stackRestore(); + return Joint(JointTypeEulerZYX); + } + if (dof_string == "JointTypeEulerXYZ") { + stackRestore(); + return Joint(JointTypeEulerXYZ); + } + if (dof_string == "JointTypeEulerYXZ") { + stackRestore(); + return Joint(JointTypeEulerYXZ); + } + if (dof_string == "JointTypeTranslationXYZ") { + stackRestore(); + return Joint(JointTypeTranslationXYZ); + } + } + + if (joint_dofs > 0) { + if (vector_table[1].length() != 6) { + cerr << "LuaModel Error: invalid joint motion subspace description at " + << this->keyStackToString() << endl; + abort(); + } + } + + switch (joint_dofs) { + case 0: + result = Joint(JointTypeFixed); + break; + case 1: + result = Joint (vector_table[1].get()); + break; + case 2: + result = Joint( + vector_table[1].get(), + vector_table[2].get() + ); + break; + case 3: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get() + ); + break; + case 4: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get() + ); + break; + case 5: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get() + ); + break; + case 6: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get(), + vector_table[6].get() + ); + break; + default: + cerr << "Invalid number of DOFs for joint." << endl; + abort(); + break; + } + } + + stackRestore(); + + return result; +} + +template<> +Body LuaTableNode::getDefault(const Body &default_value) { + Body result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + double mass = 0.; + Vector3d com (Vector3d::Zero(3)); + Matrix3d inertia (Matrix3d::Identity(3,3)); + + mass = vector_table["mass"]; + com = vector_table["com"].getDefault(com); + inertia = vector_table["inertia"].getDefault(inertia); + + result = Body (mass, com, inertia); + } + + stackRestore(); + + return result; +} + +namespace RigidBodyDynamics { + +namespace Addons { + +bool LuaModelReadFromTable (LuaTable &model_table, Model *model, bool verbose); +bool LuaModelReadConstraintsFromTable ( + LuaTable &model_table, + Model *model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +); + +typedef map StringIntMap; +StringIntMap body_table_id_map; + +RBDL_DLLAPI +bool LuaModelReadFromLuaState (lua_State* L, Model* model, bool verbose) { + assert (model); + + LuaTable model_table = LuaTable::fromLuaState (L); + + return LuaModelReadFromTable (model_table, model, verbose); +} + +RBDL_DLLAPI +bool LuaModelReadFromFile (const char* filename, Model* model, bool verbose) { + if(!model) { + std::cerr << "Model not provided." << std::endl; + assert(false); + abort(); + } + + LuaTable model_table = LuaTable::fromFile (filename); + + return LuaModelReadFromTable (model_table, model, verbose); +} + + +RBDL_DLLAPI +bool LuaModelReadFromFileWithConstraints ( + const char* filename, + Model* model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +) { + if(!model) { + std::cerr << "Model not provided." << std::endl; + assert(false); + abort(); + } + if(constraint_sets.size() != constraint_set_names.size()) { + std::cerr << "Number of constraint sets different from the number of \ + constraint set names." << std::endl; + assert(false); + abort(); + } + + LuaTable model_table = LuaTable::fromFile (filename); + bool modelLoaded = LuaModelReadFromTable (model_table, model, verbose); + bool constraintsLoaded = LuaModelReadConstraintsFromTable (model_table, model + , constraint_sets, constraint_set_names, verbose); + for(size_t i = 0; i < constraint_sets.size(); ++i) { + constraint_sets[i].Bind(*model); + } + + return modelLoaded && constraintsLoaded; +} + + +bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) { + if (model_table["gravity"].exists()) { + model->gravity = model_table["gravity"].get(); + + if (verbose) + cout << "gravity = " << model->gravity.transpose() << endl; + } + + int frame_count = model_table["frames"].length(); + + body_table_id_map["ROOT"] = 0; + + for (int i = 1; i <= frame_count; i++) { + if (!model_table["frames"][i]["parent"].exists()) { + cerr << "Parent not defined for frame " << i << "." << endl; + abort(); + } + + string body_name = model_table["frames"][i]["name"].getDefault(""); + string parent_name = model_table["frames"][i]["parent"].get(); + unsigned int parent_id = body_table_id_map[parent_name]; + + SpatialTransform joint_frame + = model_table["frames"][i]["joint_frame"].getDefault(SpatialTransform()); + Joint joint + = model_table["frames"][i]["joint"].getDefault(Joint(JointTypeFixed)); + Body body = model_table["frames"][i]["body"].getDefault(Body()); + + unsigned int body_id + = model->AddBody (parent_id, joint_frame, joint, body, body_name); + body_table_id_map[body_name] = body_id; + + if (verbose) { + cout << "==== Added Body ====" << endl; + cout << " body_name : " << body_name << endl; + cout << " body id : " << body_id << endl; + cout << " parent_id : " << parent_id << endl; + cout << " joint dofs : " << joint.mDoFCount << endl; + for (unsigned int j = 0; j < joint.mDoFCount; j++) { + cout << " " << j << ": " << joint.mJointAxes[j].transpose() << endl; + } + cout << " joint_frame: " << joint_frame << endl; + } + } + + return true; +} + +bool LuaModelReadConstraintsFromTable ( + LuaTable &model_table, + Model *model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +) { + for(size_t i = 0; i < constraint_set_names.size(); ++i) { + + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + .exists()) { + cerr << "Constraint set not existing: " << constraint_set_names[i] << "." + << endl; + assert(false); + abort(); + } + + size_t nConstraints = model_table["constraint_sets"] + [constraint_set_names[i].c_str()] + .length(); + + for(size_t ci = 0; ci < nConstraints; ++ci) { + if(!model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["constraint_type"].exists()) { + cerr << "constraint_type not specified." << endl; + assert(false); + abort(); + } + string constraintType = model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["constraint_type"] + .getDefault(""); + if(constraintType == "contact") { + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["body"].exists()) { + cerr << "body not specified." << endl; + assert(false); + abort(); + } + constraint_sets[i].AddContactConstraint + (model->GetBodyId(model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["body"] + .getDefault("").c_str()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["point"].getDefault(Vector3d::Zero()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["normal"].getDefault(Vector3d::Zero()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["name"].getDefault("").c_str() + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["normal_acceleration"].getDefault(0.)); + if(verbose) { + cout << "==== Added Constraint from '" << constraint_set_names[i] + << "' ====" << endl; + cout << " type = contact" << endl; + cout << " body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["body"].getDefault("") << endl; + cout << " body point = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["point"].getDefault(Vector3d::Zero()).transpose() + << endl; + cout << " world normal = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["normal"].getDefault(Vector3d::Zero()).transpose() + << endl; + cout << " constraint name = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["name"].getDefault("") << endl; + cout << " normal acceleration = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["normal_acceleration"].getDefault(0.) << endl; + } + } + else if(constraintType == "loop") { + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].exists()) { + cerr << "predecessor_body not specified." << endl; + assert(false); + abort(); + } + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_body"].exists()) { + cerr << "successor_body not specified." << endl; + assert(false); + abort(); + } + constraint_sets[i].AddLoopConstraint(model->GetBodyId + (model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].getDefault("").c_str()) + , model->GetBodyId(model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["successor_body"] + .getDefault("").c_str()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["predecessor_transform"].getDefault(SpatialTransform()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["successor_transform"].getDefault(SpatialTransform()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["axis"].getDefault(SpatialVector::Zero()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["stabilization_coefficient"].getDefault(1.) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["name"].getDefault("").c_str()); + if(verbose) { + cout << "==== Added Constraint from '" << constraint_set_names[i] + << "' ====" << endl; + cout << " type = loop" << endl; + cout << " predecessor body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].getDefault("") << endl; + cout << " successor body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_body"].getDefault("") << endl; + cout << " predecessor body transform = " << endl + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_transform"] + .getDefault(SpatialTransform()) << endl; + cout << " successor body transform = " << endl + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_transform"] + .getDefault(SpatialTransform()) << endl; + cout << " constraint axis (in predecessor frame) = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["axis"].getDefault(SpatialVector::Zero()) + .transpose() << endl; + cout << " stabilization coefficient = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["stabilization_coefficient"].getDefault(1.) << endl; + cout << " constraint name = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["name"].getDefault("").c_str() << endl; + } + } + else { + cerr << "Invalid constraint type: " << constraintType << endl; + abort(); + } + } + } + + return true; +} + +} + +} diff --git a/3rdparty/rbdl/addons/luamodel/luamodel.h b/3rdparty/rbdl/addons/luamodel/luamodel.h new file mode 100644 index 0000000..776a4dd --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luamodel.h @@ -0,0 +1,309 @@ +#ifndef _RBDL_LUAMODEL_H +#define _RBDL_LUAMODEL_H + +#include +#include +#include + +extern "C" { + struct lua_State; +}; + +namespace RigidBodyDynamics { + +struct Model; +struct ConstraintSet; + +namespace Addons { + +/** \page addon_luamodel_page Addon: rbdl_luamodel +* @{ +* +* \section luamodel_introduction Lua Models +* +* The Addon LuaModel allows to load \link RigidBodyDynamics::Model Models\endlink +* that are specified as Lua scripts. Lua is +* an open-source light-weight scripting language (http://www.lua.org). +* This addon is not enabled by default and one has to enable it by +* setting BUILD_ADDON_LUAMODEL to true in when configuring the RBDL with +* CMake. +* +* This addon comes with a standalone utility that can show various +* information of a lua model such as degree of freedom overview or model +* hierarchy. It is located in addons/luamodel/rbdl_luamodel_test. Use the -h +* switch to see available options. +* +* Note: this addon is not even remotely as thoroughly tested as the RBDL +* itself so please use it with some suspicion. +* +* \section luamodel_format Format Overview +* +* Models have to be specified as a specially formatted Lua table which must +* be returned by the script, i.e. if the model is specified in the table +* "model = { ... }" the script has to return this when executed. Within the +* returned table, rbdl_luamodel goes through the table "frames" and builds +* the model from the individual Frame Information Tables (see further down +* for more information about those). +* +* A valid file could look like this: +* +* \code +* model = { +* frames = { +* { +* +* }, +* { +* +* } +* } +* } +* +* return model +* \endcode +* +* Apart from the frames you can also specify gravity in the model file. +* +* Example: +* \code +* model = { +* gravity = {0., 0., -9.81} +* +* frames = { +* ... +* } +* } +* \endcode +* +* Finally, several constraint sets can be defined for the model. +* +* Example: +* \code +* model = { +* constraint_sets = { +* constraint_set_1_name = { +* { +* ... +* }, +* { +* ... +* }, +* }, +* constraint_set_2_name = { +* ... +* }, +* }, +* } +* \endcode +* +* \note The table frames must contain all Frame Information Tables as a list +* and individual tables must *not* be specified with a key, i.e. +* \code +* frames = { +* some_frame = { +* ... +* }, +* { +* .. +* } +* } +* \endcode +* is not possible as Lua does not retain the order of the individual +* frames when an explicit key is specified. +* +* \section luamodel_frame_table Frame Information Table +* +* The Frame Information Table is searched for values needed to call +* Model::AddBody(). The following fields are used by rbdl_luamodel +* (everything else is ignored): +* +* \par name (required, type: string): +* Name of the body that is being added. This name must be unique. +* +* \par parent (required, type: string): +* If the value is "ROOT" the parent frame of this body is assumed to be +* the base coordinate system, otherwise it must be the exact same string +* as was used for the "name"-field of the parent frame. +* +* \par body (optional, type: table) +* Specification of the dynamical parameters of the body. It uses the +* values (if existing): +* \code +* mass (scalar value, default 1.), +* com (3-d vector, default: (0., 0., 0.)) +* inertia (3x3 matrix, default: identity matrix) +* \endcode +* \par +* to create a \ref RigidBodyDynamics::Body. +* +* \par joint (optional, type: table) +* Specifies the type of joint, fixed or up to 6 degrees of freedom. Each +* entry in the joint table should be a 6-d that defines the motion +* subspace of a single degree of freedom. +* \par +* Default joint type is a fixed joint that attaches the body rigidly to +* its parent. +* \par +* 3-DoF joints are described by using strings. The following 3-DoF +* joints are available: +* +* - "JointTypeSpherical": for singularity-free spherical joints +* - "JointTypeEulerZYX": Euler ZYX joint +* - "JointTypeEulerXYZ": Euler XYZ joint +* - "JointTypeEulerYXZ": Euler YXZ joint +* - "JointTypeTranslationXYZ": Free translational joint +* +* \par +* Examples: +* \code +* joint_fixed = {} +* joint_translate_x = { {0., 0., 0., 1., 0., 0.} } +* joint_translate_xy = { +* {0., 0., 0., 1., 0., 0.}, +* {0., 0., 0., 0., 1., 0.} +* } +* joint_rotate_zyx = { +* {0., 0., 1., 0., 0., 0.}, +* {0., 1., 0., 0., 0., 0.}, +* {1., 0., 0., 0., 0., 0.}, +* } +* joint_rotate_euler_zyx = { "JointTypeEulerZYX" } +* \endcode +* +* \par joint_frame (optional, type: table) +* Specifies the origin of the joint in the frame of the parent. It uses +* the values (if existing): +* \code +* r (3-d vector, default: (0., 0., 0.)) +* E (3x3 matrix, default: identity matrix) +* \endcode +* \par +* for which r is the translation and E the rotation of the joint frame +* +* \section luamodel_constraint_table Constraint Information Table +* The Constraint Information Table is searched for values needed to call +* ConstraintSet::AddContactConstraint() or ConstraintSet::AddLoopConstraint(). +* The employed fields are defined as follows.. Please note that different fields +* may be used for different constraint types. +* +* \par constraint_type (required, type: string) +* Specifies the type of constraints, either 'contact' or 'loop', other +* values will cause a failure while reading the file. +* +* \par name (optional, type: string) +* Specifies a name for the constraint. +* +* The following fields are used exclusively for contact constraints: +* +* \par body (required, type: string) +* The name of the body involved in the constraint. +* +* \par point(optional, type: table) +* The position of the contact point in body coordinates. Defaults to +* (0, 0, 0). +* +* \par normal(optional, type: table) +* The normal along which the constraint acts in world coordinates. +* Defaults to (0, 0, 0). +* +* \par normal_acceleration(optional, type: number) +* The normal acceleration along the constraint axis. Defaults to 0. +* +* The following fields are used exclusively for loop constraints: +* +* \par predecessor_body (required, type: string) +* The name of the predecessor body involved in the constraint. +* +* \par successor_body (required, type: string) +* The name of the successor body involved in the constraint. +* +* \par predecessor_transform (optional, type: table) +* The transform of the predecessor constrained frame with respect to +* the predecessor body frame. Defaults to the identity transform. +* +* \par sucessor_transform (optional, type: table) +* The transform of the sucessor constrained frame with respect to +* the sucessor body frame. Defaults to the identity transform. +* +* \par axis (optional, type: table) +* The 6d spatial axis along which the constraint acts, in coordinates +* relative to the predecessor constrained frame. Defaults to (0,0,0,0,0,0). +* +* \par stabilization_coefficient(optional, type: number) +* The stabilization coefficient for Baumgarte stabilization. Defaults to 1. +* +* \section luamodel_example Example +* Here is an example of a model: +* \include samplemodel.lua +* +* \section luamodel_example_constraints Example with constraints +* Here is an example of a model including constraints: +* \include sampleconstrainedmodel.lua +* +*/ + +/** \brief Reads a model from a Lua file. + * + * \param filename the name of the Lua file. + * \param model a pointer to the output Model structure. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model was read successfully. + * + * \note See \ref luamodel_introduction for information on how to define the + * Lua model. + */ +RBDL_DLLAPI +bool LuaModelReadFromFile ( + const char* filename, + Model* model, + bool verbose = false); + +/** \brief Reads a model and constraint sets from a Lua file. + * + * \param filename the name of the Lua file. + * \param model a pointer to the output Model structure. + * \param constraint_sets reference to a std::vector of ConstraintSet structures + * in which to save the information read from the file. + * \param constraint_set_names reference to a std::vector of std::string + * specifying the names of the constraint sets to be read from the Lua file. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model and constraint sets were read successfully. + * + * \note constraint_sets and constraint_set_names are required to have the same + * size. See \ref luamodel_introduction for information on how to define the + * Lua model. + */ +RBDL_DLLAPI +bool LuaModelReadFromFileWithConstraints ( + const char* filename, + Model* model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose = false); + +/** \brief Reads a model from a lua_State. + * + * \param L a pointer to the lua_State. + * \param model a pointer to the output Model structure. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model was read successfully. + */ +RBDL_DLLAPI +bool LuaModelReadFromLuaState ( + lua_State* L, + Model* model, + bool verbose = false); + +/** @} */ +} + +} + +/* _RBDL_LUAMODEL_H */ +#endif diff --git a/3rdparty/rbdl/addons/luamodel/luatables.cc b/3rdparty/rbdl/addons/luamodel/luatables.cc new file mode 100644 index 0000000..a829a5f --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luatables.cc @@ -0,0 +1,932 @@ +/* + * LuaTables++ + * Copyright (c) 2013-2014 Martin Felis . + * All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "luatables.h" + +#include +#include +#include +#include +#include +#include + +extern "C" +{ + #include + #include + #include +} + +#include /* defines FILENAME_MAX */ + +#if defined(WIN32) || defined (_WIN32) + #include + #define get_current_dir _getcwd + #define DIRECTORY_SEPARATOR "\\" +#elif defined(linux) || defined (__linux) || defined(__linux__) || defined(__APPLE__) + #include + #define get_current_dir getcwd + #define DIRECTORY_SEPARATOR "/" +#else + #error Platform not supported! +#endif + +using namespace std; + +std::string get_file_directory (const char* filename) { + string name (filename); + string result = name.substr(0, name.find_last_of (DIRECTORY_SEPARATOR) + 1); + + if (result == "") + result = "./"; +#if defined (WIN32) || defined (_WIN32) + else if (result.substr(1,2) != ":\\") + result = string(".\\") + result; +#else + else if (result.substr(0,string(DIRECTORY_SEPARATOR).size()) != DIRECTORY_SEPARATOR && result[0] != '.') + result = string("./") + result; +#endif + + return result; +} + +// char encoded serialize function that is available in plaintext in +// utils/serialize.lua. Converted using lua auto.lua serialize.lua +const char serialize_lua[] = { + + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x69, 0x73, 0x6c, + 0x69, 0x73, 0x74, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, + 0x20, 0x30, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, + 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x28, + 0x74, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, 0x20, 0x69, 0x74, 0x65, 0x6d, + 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x2b, 0x20, 0x31, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x3d, 0x20, + 0x6e, 0x69, 0x6c, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, + 0x65, 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x6c, 0x61, + 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x6f, 0x72, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, + 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, + 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, + 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, + 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, + 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, + 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x0a, + 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, 0x65, + 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x7e, 0x3d, 0x20, 0x23, + 0x74, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x63, 0x6d, 0x70, + 0x5f, 0x61, 0x6c, 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x20, 0x28, 0x61, 0x2c, 0x20, + 0x62, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, + 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x6f, 0x72, 0x20, 0x74, 0x79, + 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, + 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, + 0x6d, 0x62, 0x65, 0x72, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x61, 0x20, 0x3c, 0x20, 0x62, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3c, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x67, 0x65, 0x6e, + 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, + 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, + 0x69, 0x63, 0x65, 0x73, 0x20, 0x3d, 0x20, 0x7b, 0x7d, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, + 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, + 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x6b, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x73, 0x6f, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x63, 0x6d, 0x70, 0x5f, 0x61, 0x6c, + 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, 0x78, 0x74, 0x20, 0x28, 0x74, 0x2c, 0x20, 0x73, 0x74, 0x61, 0x74, + 0x65, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, + 0x65, 0x73, 0x20, 0x3d, 0x20, 0x67, 0x65, 0x6e, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, + 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, + 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x20, 0x3d, 0x20, 0x31, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, + 0x67, 0x65, 0x74, 0x6e, 0x28, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x5d, 0x20, 0x3d, 0x3d, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, + 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x20, 0x2b, 0x20, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, + 0x73, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, + 0x78, 0x74, 0x2c, 0x20, 0x74, 0x2c, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, + 0x20, 0x28, 0x6f, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x0a, + 0x20, 0x20, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x22, + 0x22, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, + 0x3d, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, 0x3d, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x20, 0x22, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, + 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x6f, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, + 0x22, 0x22, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, + 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x69, 0x73, 0x6c, 0x69, 0x73, + 0x74, 0x28, 0x6f, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x22, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, + 0x75, 0x62, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x69, 0x70, 0x61, 0x69, 0x72, + 0x73, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, 0x62, + 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, + 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, + 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, + 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x76, 0x20, 0x2e, 0x2e, + 0x20, 0x22, 0x5c, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, + 0x6e, 0x67, 0x20, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, + 0x61, 0x62, 0x6c, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, + 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x6f, 0x2e, 0x64, 0x6f, 0x6e, 0x74, 0x5f, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x5f, 0x6d, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x22, 0x7b, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, + 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, + 0x3d, 0x20, 0x22, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x2d, 0x2d, 0x20, 0x6d, 0x61, 0x6b, 0x65, 0x20, 0x73, 0x75, + 0x72, 0x65, 0x20, 0x74, 0x68, 0x61, 0x74, 0x20, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x65, 0x64, 0x20, 0x6b, + 0x65, 0x79, 0x73, 0x20, 0x61, 0x72, 0x65, 0x20, 0x70, 0x72, 0x6f, 0x70, 0x65, 0x72, 0x6c, 0x79, 0x20, 0x61, + 0x72, 0x65, 0x20, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x69, 0x66, 0x69, 0x65, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6b, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, + 0x69, 0x6e, 0x67, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, + 0x20, 0x20, 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, + 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, + 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x28, 0x6b, 0x29, 0x20, 0x2e, + 0x2e, 0x20, 0x22, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x70, 0x72, 0x69, 0x6e, 0x74, 0x20, 0x28, 0x22, 0x6e, 0x6f, 0x74, 0x20, 0x73, 0x65, + 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6e, 0x74, 0x72, 0x79, 0x20, 0x6f, 0x66, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, + 0x20, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, 0x0a, +}; + +// +// Lua Helper Functions +// +void bail(lua_State *L, const char *msg){ + std::cerr << msg << lua_tostring(L, -1) << endl; + abort(); +} + +void stack_print (const char *file, int line, lua_State *L) { + cout << file << ":" << line << ": stack size: " << lua_gettop(L) << endl;; + for (int i = 1; i < lua_gettop(L) + 1; i++) { + cout << file << ":" << line << ": "; + cout << i << ": "; + if (lua_istable (L, i)) + cout << " table: " << lua_topointer (L, i) << endl; + else if (lua_isnumber (L, i)) + cout << " number: " << lua_tonumber (L, i) << endl; + else if (lua_isuserdata (L, i)) { + void* userdata = (void*) lua_touserdata (L, i); + cout << " userdata (" << userdata << ")" << endl; + } else if (lua_isstring (L, i)) + cout << " string: " << lua_tostring(L, i) << endl; + else if (lua_isfunction (L, i)) + cout << " function" << endl; + else if (lua_isnil (L, i)) + cout << " nil" << endl; + else + cout << " unknown: " << lua_typename (L, lua_type (L, i)) << endl; + } +} + +void l_push_LuaKey (lua_State *L, const LuaKey &key) { + if (key.type == LuaKey::Integer) + lua_pushnumber (L, key.int_value); + else + lua_pushstring(L, key.string_value.c_str()); +} + +bool query_key_stack (lua_State *L, std::vector key_stack) { + for (int i = key_stack.size() - 1; i >= 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + return false; + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_gettable (L, -2); + + // return if key is not found + if (lua_isnil(L, -1)) { + return false; + } + } + + return true; +} + +void create_key_stack (lua_State *L, std::vector key_stack) { + for (int i = key_stack.size() - 1; i > 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + lua_pop(L, 1); + lua_newtable(L); + lua_pushvalue(L, -1); + lua_setglobal(L, key_stack[key_stack.size() - 1].string_value.c_str()); + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_pushvalue (L, -1); + lua_gettable (L, -3); + + if (lua_isnil(L, -1)) { + // parent, key, nil + lua_pop(L, 1); // parent, key + lua_newtable(L); // parent, key, table + lua_insert(L, -2); // parent, table, key + lua_pushvalue(L, -2); // parent, table, key, table + lua_settable (L, -4); // parent, table + } + } +} + +// +// LuaTableNode +// +std::vector LuaTableNode::getKeyStack() { + std::vector result; + + const LuaTableNode *node_ptr = this; + + do { + result.push_back (node_ptr->key); + node_ptr = node_ptr->parent; + } while (node_ptr != NULL); + + return result; +} + +std::string LuaTableNode::keyStackToString() { + std::vector key_stack = getKeyStack(); + + ostringstream result_stream (""); + for (int i = key_stack.size() - 1; i >= 0; i--) { + if (key_stack[i].type == LuaKey::String) + result_stream << "[\"" << key_stack[i].string_value << "\"]"; + else + result_stream << "[" << key_stack[i].int_value << "]"; + } + + return result_stream.str(); +} + +bool LuaTableNode::stackQueryValue() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + return query_key_stack (L, key_stack); +} + +void LuaTableNode::stackCreateValue() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + create_key_stack (L, key_stack); +} + +LuaTable LuaTableNode::stackQueryTable() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + if (!query_key_stack (L, key_stack)) { + std::cerr << "Error: could not query table " << key << "." << std::endl; + abort(); + } + + return LuaTable::fromLuaState (L); +} + +LuaTable LuaTableNode::stackCreateLuaTable() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + create_key_stack (L, key_stack); + + // create new table for the CustomType + lua_newtable(luaTable->L); // parent, CustomTable + // add table of CustomType to the parent + stackPushKey(); // parent, CustomTable, key + lua_pushvalue(luaTable->L, -2); // parent, CustomTable, key, CustomTable + lua_settable(luaTable->L, -4); + + LuaTable result; + result.luaStateRef = luaTable->luaStateRef->acquire(); + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + lua_pop (luaTable->luaStateRef->L, 2); + + return result; +} + +void LuaTableNode::stackPushKey() { + l_push_LuaKey (luaTable->L, key); +} + +void LuaTableNode::stackRestore() { + lua_pop (luaTable->L, lua_gettop(luaTable->L) - stackTop); + + luaTable->popRef(); +} + +bool LuaTableNode::exists() { + bool result = true; + + if (!stackQueryValue()) + result = false; + + stackRestore(); + + return result; +} + +void LuaTableNode::remove() { + if (stackQueryValue()) { + lua_pop(luaTable->L, 1); + + if (lua_gettop(luaTable->L) != 0) { + l_push_LuaKey (luaTable->L, key); + lua_pushnil (luaTable->L); + lua_settable (luaTable->L, -3); + } else { + lua_pushnil (luaTable->L); + lua_setglobal (luaTable->L, key.string_value.c_str()); + } + } + + stackRestore(); +} + +size_t LuaTableNode::length() { + size_t result = 0; + + if (stackQueryValue()) { +#if LUA_VERSION_NUM == 501 + result = lua_objlen(luaTable->L, -1); +#elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(luaTable->L, -1); +#endif + } + + stackRestore(); + + return result; +} + +std::vector LuaTableNode::keys() { + std::vector result; + + if (stackQueryValue()) { + // loop over all keys + lua_pushnil(luaTable->L); + while (lua_next(luaTable->L, -2) != 0) { + if (lua_isnumber(luaTable->L, -2)) { + double number = lua_tonumber (luaTable->L, -2); + double frac; + if (modf (number, &frac) == 0) { + LuaKey key (static_cast(number)); + result.push_back (key); + } + } else if (lua_isstring (luaTable->L, -2)) { + LuaKey key (lua_tostring(luaTable->L, -2)); + result.push_back (key); + } else { + cerr << "Warning: invalid LuaKey type for key " << lua_typename(luaTable->L, lua_type(luaTable->L, -2)) << "!" << endl; + } + + lua_pop(luaTable->L, 1); + } + } + + stackRestore(); + + return result; +} + + +template<> bool LuaTableNode::getDefault(const bool &default_value) { + bool result = default_value; + + if (stackQueryValue()) { + result = lua_toboolean (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> float LuaTableNode::getDefault(const float &default_value) { + float result = default_value; + + if (stackQueryValue()) { + result = static_cast(lua_tonumber (luaTable->L, -1)); + } + + stackRestore(); + + return result; +} + +template<> double LuaTableNode::getDefault(const double &default_value) { + double result = default_value; + + if (stackQueryValue()) { + result = lua_tonumber (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> std::string LuaTableNode::getDefault(const std::string &default_value) { + std::string result = default_value; + + if (stackQueryValue() && lua_isstring(luaTable->L, -1)) { + result = lua_tostring (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> void LuaTableNode::set(const bool &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushboolean(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const float &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, static_cast(value)); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const double &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const std::string &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushstring(luaTable->L, value.c_str()); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +// +// LuaTable +// +LuaTable::LuaTable (const LuaTable &other) : + filename (other.filename), + referencesGlobal (other.referencesGlobal) { + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } +} + +LuaTable& LuaTable::operator= (const LuaTable &other) { + if (&other != this) { + if (luaStateRef) { + // cleanup any existing reference + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + + // if this is the last, delete the Lua state + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } + + filename = other.filename; + luaStateRef = other.luaStateRef; + referencesGlobal = other.referencesGlobal; + + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } + } + + return *this; +} + +LuaTable::~LuaTable() { + if (luaRef != -1) { + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + + if (luaStateRef) { + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } +} + +int LuaTable::length() { + pushRef(); + + if ((lua_gettop(L) == 0) || (lua_type (L, -1) != LUA_TTABLE)) { + cerr << "Error: cannot query table length. No table on stack!" << endl; + abort(); + } + size_t result = 0; + +#if LUA_VERSION_NUM == 501 + result = lua_objlen(L, -1); +#elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(L, -1); +#endif + + popRef(); + + return result; +} + +void LuaTable::pushRef() { + assert (luaStateRef); + assert (luaStateRef->L); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + L = luaStateRef->L; +} + +void LuaTable::popRef() { + if (!referencesGlobal) { + lua_pop (luaStateRef->L, 1); + } + L = NULL; +} + +LuaTable LuaTable::fromFile (const char* _filename) { + LuaTable result; + + result.filename = _filename; + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); + + // Add the directory of _filename to package.path + result.addSearchPath(get_file_directory (_filename).c_str()); + + // run the file we + if (luaL_dofile (result.luaStateRef->L, _filename)) { + bail (result.luaStateRef->L, "Error running file: "); + } + + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + return result; +} + +LuaTable LuaTable::fromLuaExpression (const char* lua_expr) { + LuaTable result; + + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); + + if (luaL_loadstring (result.luaStateRef->L, lua_expr)) { + bail (result.luaStateRef->L, "Error compiling expression!"); + } + + if (lua_pcall (result.luaStateRef->L, 0, LUA_MULTRET, 0)) { + bail (result.luaStateRef->L, "Error running expression!"); + } + + if (lua_gettop(result.luaStateRef->L) != 0) { + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + } else { + result.referencesGlobal = true; + } + + return result; +} + +LuaTable LuaTable::fromLuaState (lua_State* L) { + LuaTable result; + + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = L; + result.luaStateRef->count = 1; + result.luaStateRef->freeOnZeroRefs = false; + + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + return result; +} + +void LuaTable::addSearchPath(const char* path) { + if (luaStateRef->L == NULL) { + cerr << "Error: Cannot add search path: Lua state is not initialized!" << endl; + abort(); + } + + lua_getglobal(luaStateRef->L, "package"); + lua_getfield (luaStateRef->L, -1, "path"); + if (lua_type(luaStateRef->L, -1) != LUA_TSTRING) { + cerr << "Error: could not get package.path!" << endl; + abort(); + } + + string package_path = lua_tostring (luaStateRef->L, -1); + package_path = package_path + string (";") + string(path) + "?.lua;"; + + lua_pushstring(luaStateRef->L, package_path.c_str()); + lua_setfield (luaStateRef->L, -3, "path"); + + lua_pop(luaStateRef->L, 2); +} + +std::string LuaTable::serialize() { + pushRef(); + + std::string result; + + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } + + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } + + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + if (lua_pcall (L, 1, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + cerr << "Cannot serialize global Lua state!" << endl; + abort(); + } + + lua_pop (L, lua_gettop(L) - current_top); + + popRef(); + + return result; +} + +std::string LuaTable::orderedSerialize() { + pushRef(); + + std::string result; + + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } + + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } + + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + lua_pushstring (L, ""); + lua_pushboolean (L, true); + if (lua_pcall (L, 3, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + cerr << "Cannot serialize global Lua state!" << endl; + abort(); + } + + lua_pop (L, lua_gettop(L) - current_top); + + popRef(); + + return result; +} diff --git a/3rdparty/rbdl/addons/luamodel/luatables.h b/3rdparty/rbdl/addons/luamodel/luatables.h new file mode 100644 index 0000000..b02b180 --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luatables.h @@ -0,0 +1,261 @@ +/* + * LuaTables++ + * Copyright (c) 2013-2014 Martin Felis . + * All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef LUATABLES_H +#define LUATABLES_H + +#include +#include +#include +#include +#include + +#include + +// Forward declaration for Lua +extern "C" { +struct lua_State; +} + +struct RBDL_DLLAPI LuaKey { + enum Type { + String, + Integer + }; + + Type type; + int int_value; + std::string string_value; + + bool operator<( const LuaKey& rhs ) const { + if (type == String && rhs.type == Integer) { + return false; + } else if (type == Integer && rhs.type == String) { + return true; + } else if (type == Integer && rhs.type == Integer) { + return int_value < rhs.int_value; + } + + return string_value < rhs.string_value; + } + + LuaKey (const char* key_value) : + type (String), + int_value (0), + string_value (key_value) { } + LuaKey (int key_value) : + type (Integer), + int_value (key_value), + string_value ("") {} +}; + +inline std::ostream& operator<<(std::ostream& output, const LuaKey &key) { + if (key.type == LuaKey::Integer) + output << key.int_value << "(int)"; + else + output << key.string_value << "(string)"; + return output; +} +struct RBDL_DLLAPI LuaTable; +struct RBDL_DLLAPI LuaTableNode; + +struct RBDL_DLLAPI LuaTableNode { + LuaTableNode() : + parent (NULL), + luaTable (NULL), + key("") + { } + LuaTableNode operator[](const char *child_str) { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_str); + + return child_node; + } + LuaTableNode operator[](int child_index) { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_index); + + return child_node; + } + bool stackQueryValue(); + void stackPushKey(); + void stackCreateValue(); + void stackRestore(); + LuaTable stackQueryTable(); + LuaTable stackCreateLuaTable(); + + std::vector getKeyStack(); + std::string keyStackToString(); + + bool exists(); + void remove(); + size_t length(); + std::vector keys(); + + // Templates for setters and getters. Can be specialized for custom + // types. + template + void set (const T &value); + template + T getDefault (const T &default_value); + + template + T get() { + if (!exists()) { + std::cerr << "Error: could not find value " << keyStackToString() << "." << std::endl; + abort(); + } + return getDefault (T()); + } + + // convenience operators (assignment, conversion, comparison) + template + void operator=(const T &value) { + set(value); + } + template + operator T() { + return get(); + } + template + bool operator==(T value) { + return value == get(); + } + template + bool operator!=(T value) { + return value != get(); + } + + LuaTableNode *parent; + LuaTable *luaTable; + LuaKey key; + int stackTop; +}; + +template +bool operator==(T value, LuaTableNode node) { + return value == (T) node; +} +template +bool operator!=(T value, LuaTableNode node) { + return value != (T) node; +} + +template<> bool LuaTableNode::getDefault(const bool &default_value); +template<> double LuaTableNode::getDefault(const double &default_value); +template<> float LuaTableNode::getDefault(const float &default_value); +template<> std::string LuaTableNode::getDefault(const std::string &default_value); + +template<> void LuaTableNode::set(const bool &value); +template<> void LuaTableNode::set(const float &value); +template<> void LuaTableNode::set(const double &value); +template<> void LuaTableNode::set(const std::string &value); + +/// Reference counting Lua state +struct RBDL_DLLAPI LuaStateRef { + LuaStateRef () : + L (NULL), + count (0), + freeOnZeroRefs(true) + {} + + LuaStateRef* acquire() { + count = count + 1; + return this; + } + + int release() { + count = count - 1; + return count; + } + + lua_State *L; + unsigned int count; + bool freeOnZeroRefs; +}; + +struct RBDL_DLLAPI LuaTable { + LuaTable () : + filename (""), + luaStateRef (NULL), + luaRef(-1), + L (NULL), + referencesGlobal (false) + {} + LuaTable (const LuaTable &other); + LuaTable& operator= (const LuaTable &other); + ~LuaTable(); + + LuaTableNode operator[] (const char* key) { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + LuaTableNode operator[] (int key) { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + int length(); + void addSearchPath (const char* path); + std::string serialize (); + + /// Serializes the data in a predictable ordering. + std::string orderedSerialize (); + + /// Pushes the Lua table onto the stack of the internal Lua state. + // I.e. makes the Lua table active that is associated with this + // LuaTable. + void pushRef(); + /// Pops the Lua table from the stack of the internal Lua state. + // Cleans up a previous pushRef() + void popRef(); + + static LuaTable fromFile (const char *_filename); + static LuaTable fromLuaExpression (const char* lua_expr); + static LuaTable fromLuaState (lua_State *L); + + std::string filename; + LuaStateRef *luaStateRef; + int luaRef; + lua_State *L; + + bool referencesGlobal; +}; + +/* LUATABLES_H */ +#endif diff --git a/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc b/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc new file mode 100644 index 0000000..4c85cb9 --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc @@ -0,0 +1,100 @@ +#include "rbdl/rbdl.h" +#include "rbdl/rbdl_utils.h" +#include "luamodel.h" + +#include +#include +#include + +using namespace std; + +using namespace RigidBodyDynamics::Math; + +void usage (const char* argv_0) { + cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] " << endl; + cerr << " -v | --verbose enable additional output" << endl; + cerr << " -d | --dof-overview print an overview of the degress of freedom" << endl; + cerr << " -m | --model-hierarchy print the hierarchy of the model" << endl; + cerr << " -o | --body-origins print the origins of all bodies that have names" << endl; + cerr << " -c | --center_of_mass print center of mass for bodies and full model" << endl; + cerr << " -h | --help print this help" << endl; + exit (1); +} + +int main (int argc, char *argv[]) { + if (argc < 2 || argc > 4) { + usage(argv[0]); + } + + bool verbose = false; + bool dof_overview = false; + bool model_hierarchy = false; + bool body_origins = false; + bool center_of_mass = false; + + string filename = argv[1]; + + for (int i = 1; i < argc; i++) { + if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose") + verbose = true; + else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview") + dof_overview = true; + else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy") + model_hierarchy = true; + else if (string(argv[i]) == "-o" || string (argv[i]) == "--body-origins") + body_origins = true; + else if (string(argv[i]) == "-c" || string (argv[i]) == "--center-of-mass") + center_of_mass = true; + else if (string(argv[i]) == "-h" || string (argv[i]) == "--help") + usage(argv[0]); + else + filename = argv[i]; + } + + RigidBodyDynamics::Model model; + + if (!RigidBodyDynamics::Addons::LuaModelReadFromFile(filename.c_str(), &model, verbose)) { + cerr << "Loading of lua model failed!" << endl; + return -1; + } + + cout << "Model loading successful!" << endl; + + if (dof_overview) { + cout << "Degree of freedom overview:" << endl; + cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model); + } + + if (model_hierarchy) { + cout << "Model Hierarchy:" << endl; + cout << RigidBodyDynamics::Utils::GetModelHierarchy (model); + } + + if (body_origins) { + cout << "Body Origins:" << endl; + cout << RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(model); + } + + if (center_of_mass) { + VectorNd q_zero (VectorNd::Zero (model.q_size)); + VectorNd qdot_zero (VectorNd::Zero (model.qdot_size)); + RigidBodyDynamics::UpdateKinematics (model, q_zero, qdot_zero, qdot_zero); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.mBodies[i].mIsVirtual) + continue; + + SpatialRigidBodyInertia rbi_base = model.X_base[i].apply(model.I[i]); + Vector3d body_com = rbi_base.h / rbi_base.m; + cout << setw(12) << model.GetBodyName (i) << ": " << setw(10) << body_com.transpose() << endl; + } + + Vector3d model_com; + double mass; + RigidBodyDynamics::Utils::CalcCenterOfMass (model, q_zero, qdot_zero, mass, model_com); + cout << setw(14) << "Model COM: " << setw(10) << model_com.transpose() << endl; + cout << setw(14) << "Model mass: " << mass << endl; + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua b/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua new file mode 100644 index 0000000..b93186e --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua @@ -0,0 +1,267 @@ +-- 5b3d.lua +-- Copyright (c) 2016 Davide Corradi + +-- Parameters + +m1 = 2 +l1 = 2 +r1 = 0.2 +Izz1 = m1 * l1 * l1 / 3 + +m2 = 2 +l2 = 2 +r2 = 0.2 +Izz2 = m2 * l2 * l2 / 3 + +bodies = { + + virtual = { + mass = 0, + com = {0, 0, 0}, + inertia = { + {0, 0, 0}, + {0, 0, 0}, + {0, 0, 0}, + }, + }, + + link1 = { + mass = m1, + com = {l1/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz1}, + }, + }, + + link2 = { + mass = m2, + com = {l2/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz2}, + }, + }, + +} + +joints = { + + revZ = { + {0, 0, 1, 0, 0, 0}, + }, + + trnXYZ = { + {0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + }, + +} + +meshes = { + link1 = { + name = 'link1', + dimensions = {l1, r1, r1}, + color = {1, 0, 0}, + src = 'unit_cylinder_medres_z.obj', + mesh_center = {l1/2, 0, 0}, + }, + link2 = { + name = 'link2', + dimensions = {l2, r2, r2}, + color = {0, 1, 0}, + src = 'unit_cylinder_medres_z.obj', + mesh_center = {l2/2, 0, 0}, + }, +} + +model = { + + gravity = {0, 0, 0}, + + configuration = { + axis_front = { 1., 0., 0.}, + axis_right = { 0., -1., 0.}, + axis_up = { 0., 0., 1.}, + }, + + frames = { + + { + name = 'base', + parent = 'ROOT', + body = bodies.virtual, + joint = joints.trnXYZ, + }, + { + name = 'l11', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + visuals = { meshes.link1 }, + }, + { + name = 'l12', + parent = 'l11', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + visuals = { meshes.link2 }, + }, + { + name = 'l21', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + visuals = { meshes.link1 }, + }, + { + name = 'l22', + parent = 'l21', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + visuals = { meshes.link2 }, + }, + + }, + + constraint_sets = { + loop_constraints = { + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + + all_constraints = { + { + constraint_type = 'contact', + body = 'base', + point = {0, 0, 0}, + normal = {1, 0, 0}, + name = 'baseTX', + normal_acceleration = 0, + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 1, 0}, + name = 'baseTY', + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 0, 1}, + name = 'baseTZ', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + }, + +} + +return model \ No newline at end of file diff --git a/3rdparty/rbdl/addons/luamodel/samplemodel.lua b/3rdparty/rbdl/addons/luamodel/samplemodel.lua new file mode 100644 index 0000000..d2d501a --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/samplemodel.lua @@ -0,0 +1,87 @@ +inertia = { + {1.1, 0.1, 0.2}, + {0.3, 1.2, 0.4}, + {0.5, 0.6, 1.3} +} + +pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } +thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia } +shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } +foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } + +bodies = { + pelvis = pelvis, + thigh_right = thigh, + shank_right = shank, + thigh_left = thigh, + shank_left = shank +} + +joints = { + freeflyer = { + { 0., 0., 0., 1., 0., 0.}, + { 0., 0., 0., 0., 1., 0.}, + { 0., 0., 0., 0., 0., 1.}, + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + spherical_zyx = { + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + rotational_y = { + { 0., 1., 0., 0., 0., 0.} + }, + fixed = {} +} + +model = { + frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.spherical_zyx, + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.thigh_right, + joint = joints.fixed + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.spherical_zyx + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.thigh_left, + joint = joints.rotational_y + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.thigh_left, + joint = joints.fixed + }, + } +} + +return model diff --git a/3rdparty/rbdl/addons/muscle/CMakeLists.txt b/3rdparty/rbdl/addons/muscle/CMakeLists.txt new file mode 100644 index 0000000..fbc5317 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/CMakeLists.txt @@ -0,0 +1,89 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) +CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_MUSCLE_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_MUSCLE_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_MUSCLE_VERSION + ${RBDL_ADDON_MUSCLE_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_VERSION_PATCH} +) + + +PROJECT (RBDL_ADDON_MUSCLE VERSION ${RBDL_ADDON_MUSCLE_VERSION}) + + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl +) + + +SET(MUSCLE_SOURCES + muscle.h + Millard2016TorqueMuscle.h + Millard2016TorqueMuscle.cc + MuscleFunctionFactory.h + MuscleFunctionFactory.cc + TorqueMuscleFunctionFactory.h + TorqueMuscleFunctionFactory.cc + csvtools.h + csvtools.cc +) + +SET(MUSCLE_HEADERS + muscle.h + Millard2016TorqueMuscle.h + MuscleFunctionFactory.h + TorqueMuscleFunctionFactory.h + csvtools.h +) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_muscle-static STATIC ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES PREFIX "lib") + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES OUTPUT_NAME "rbdl_muscle") + TARGET_LINK_LIBRARIES (rbdl_muscle-static + rbdl_geometry-static + rbdl-static + ) + + + INSTALL (TARGETS rbdl_muscle-static + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_muscle SHARED ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES (rbdl_muscle + rbdl_geometry + rbdl + ) + + + INSTALL (TARGETS rbdl_muscle + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + ) + +INSTALL ( FILES ${MUSCLE_HEADERS} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/muscle + ) diff --git a/3rdparty/rbdl/addons/muscle/LICENSE b/3rdparty/rbdl/addons/muscle/LICENSE new file mode 100644 index 0000000..a25f3e9 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/LICENSE @@ -0,0 +1,23 @@ +Rigid Body Dynamics Library Muscle Addon - +Copyright (c) 2016 Matthew Millard + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt b/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc new file mode 100644 index 0000000..304561d --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc @@ -0,0 +1,1445 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : muscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "Millard2016TorqueMuscle.h" +#include "TorqueMuscleFunctionFactory.h" +#include "csvtools.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +static double EPSILON = std::numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); + +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + +/************************************************************* + Table Access Structure Names +*************************************************************/ +const double gravity = 9.81; //Needed for the strength scaling used + //by Anderson et al. + +const char* DataSet::names[] = { "Anderson2007", + "Gymnast"}; + +const char* GenderSet::names[] = {"Male", + "Female"}; + +const char* AgeGroupSet::names[] = {"Young18To25", + "Middle55To65", + "SeniorOver65"}; + +const char* JointTorqueSet::names[] = { "HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension" , + "AnkleFlexion" , + "ElbowExtension" , + "ElbowFlexion" , + "ShoulderExtension" , + "ShoulderFlexion" , + "WristExtension" , + "WristFlexion" , + "ShoulderHorizontalAdduction", + "ShoulderHorizontalAbduction", + "ShoulderInternalRotation" , + "ShoulderExternalRotation" , + "WristUlnarDeviation" , + "WristRadialDeviation" , + "WristPronation" , + "WristSupination" , + "LumbarExtension" , + "LumbarFlexion" }; + +const char* Anderson2007::GenderNames[] = {"Male","Female"}; + +const char* Anderson2007::AgeGroupNames[] = { "Young18To25", + "Middle55To65", + "SeniorOver65"}; + +const char* Anderson2007::JointTorqueNames[] = {"HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension", + "AnkleFlexion" }; + +const char* Gymnast::GenderNames[] = {"Male"}; +const char* Gymnast::AgeGroupNames[] = {"Young18To25"}; +const char* Gymnast::JointTorqueNames[] = + { "HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension" , + "AnkleFlexion" , + "ElbowExtension" , + "ElbowFlexion" , + "ShoulderExtension" , + "ShoulderFlexion" , + "WristExtension" , + "WristFlexion" , + "ShoulderHorizontalAdduction", + "ShoulderHorizontalAbduction", + "ShoulderInternalRotation" , + "ShoulderExternalRotation" , + "WristUlnarDeviation" , + "WristRadialDeviation" , + "WristPronation" , + "WristSupination" , + "LumbarExtension" , + "LumbarFlexion"}; + +/************************************************************* + Coefficient Tables +*************************************************************/ + +/* +This data is taken from Table 3 of + +Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). +Maximum voluntary joint torque as a function of joint angle +and angular velocity: model development and application to +the lower limb. Journal of biomechanics, 40(14), 3105-3113. + +Each row contains the coefficients for the active and +passive torque characteristics for a specific joint, +direction, gender and age group. Each row corresponds +to a single block taken from Table 3, as read from +left to right top to bottom. The first 4 columns have +been added to describe the joint, direction, gender +and age group. + +Column labels: +Parameter Set Meta Data + 0: joint: hip0_knee1_ankle2, + 1: direction: ext0_flex1, + 2: gender: male0_female1, + 3: age: age18to25_0_55to65_1_g65_2, + +Active Torque-Angle and Torque-Velocity Curves + 4: c1, + 5: c2, + 6: c3, + 7: c4, + 8: c5, + 9: c6, +Passive Torque-Angle Curves + 10: b1, + 11: k1, + 12: b2, + 13: k2, +*/ +double const Millard2016TorqueMuscle::Anderson2007Table3Mean[36][14] = { +{0,0,0,0,0.161,0.958,0.932,1.578,3.19,0.242,-1.21,-6.351,0.476,5.91 }, +{0,0,1,0,0.181,0.697,1.242,1.567,3.164,0.164,-1.753,-6.358,0.239,3.872 }, +{0,0,0,1,0.171,0.922,1.176,1.601,3.236,0.32,-2.16,-8.073,0.108,4.593 }, +{0,0,1,1,0.14,0.83,1.241,1.444,2.919,0.317,-1.361,-7.128,0.013,6.479 }, +{0,0,0,2,0.144,0.896,1.125,1.561,3.152,0.477,-2.671,-7.85,0.092,5.192 }, +{0,0,1,2,0.138,0.707,1.542,1.613,3.256,0.36,-0.758,-7.545,0.018,6.061 }, +{0,1,0,0,0.113,0.738,-0.214,2.095,4.267,0.218,1.21,-6.351,-0.476,5.91 }, +{0,1,1,0,0.127,0.65,-0.35,2.136,4.349,0.156,1.753,-6.358,-0.239,3.872 }, +{0,1,0,1,0.107,0.712,-0.192,2.038,4.145,0.206,2.16,-8.073,-0.108,4.593 }, +{0,1,1,1,0.091,0.812,-0.196,2.145,4.366,0.186,1.361,-7.128,-0.013,6.479 }, +{0,1,0,2,0.101,0.762,-0.269,1.875,3.819,0.296,2.671,-7.85,-0.092,5.192 }, +{0,1,1,2,0.081,0.625,-0.422,2.084,4.245,0.196,0.758,-7.545,-0.018,6.061 }, +{1,0,0,0,0.163,1.258,1.133,1.517,3.952,0.095,0,0,-6.25,-4.521 }, +{1,0,1,0,0.159,1.187,1.274,1.393,3.623,0.173,0,0,-8.033,-5.25 }, +{1,0,0,1,0.156,1.225,1.173,1.518,3.954,0.266,0,0,-12.83,-5.127 }, +{1,0,1,1,0.128,1.286,1.141,1.332,3.469,0.233,0,0,-6.576,-4.466 }, +{1,0,0,2,0.137,1.31,1.067,1.141,3.152,0.386,0,0,-10.519,-5.662 }, +{1,0,1,2,0.124,1.347,1.14,1.066,2.855,0.464,0,0,-8.8,-6.763 }, +{1,1,0,0,0.087,0.869,0.522,2.008,5.233,0.304,0,0,6.25,-4.521 }, +{1,1,1,0,0.08,0.873,0.635,1.698,4.412,0.175,0,0,8.033,-5.25 }, +{1,1,0,1,0.081,0.986,0.523,1.83,4.777,0.23,0,0,12.83,-5.127 }, +{1,1,1,1,0.06,0.967,0.402,1.693,4.41,0.349,0,0,6.576,-4.466 }, +{1,1,0,2,0.069,0.838,0.437,1.718,4.476,0.414,0,0,10.519,-5.662 }, +{1,1,1,2,0.06,0.897,0.445,1.121,2.922,0.389,0,0,8.8,-6.763 }, +{2,0,0,0,0.095,1.391,0.408,0.987,3.558,0.295,-0.0005781,-5.819,0.967,6.09 }, +{2,0,1,0,0.104,1.399,0.424,0.862,3.109,0.189,-0.005218,-4.875,0.47,6.425 }, +{2,0,0,1,0.114,1.444,0.551,0.593,2.128,0.35,-0.001311,-10.943,0.377,8.916 }, +{2,0,1,1,0.093,1.504,0.381,0.86,3.126,0.349,-2.888e-05,-17.189,0.523,7.888 }, +{2,0,0,2,0.106,1.465,0.498,0.49,1.767,0.571,-5.693e-05,-21.088,0.488,7.309 }, +{2,0,1,2,0.125,1.299,0.58,0.587,1.819,0.348,-2.35e-05,-12.567,0.331,6.629 }, +{2,1,0,0,0.033,1.51,-0.187,0.699,1.94,0.828,0.0005781,-5.819,-0.967,6.09 }, +{2,1,1,0,0.027,1.079,-0.302,0.864,2.399,0.771,0.005218,-4.875,-0.47,6.425 }, +{2,1,0,1,0.028,1.293,-0.284,0.634,1.759,0.999,0.001311,-10.943,-0.377,8.916 }, +{2,1,1,1,0.024,1.308,-0.254,0.596,1.654,1.006,2.888e-05,-17.189,-0.523,7.888}, +{2,1,0,2,0.029,1.419,-0.174,0.561,1.558,1.198,5.693e-05,-21.088,-0.488,7.309}, +{2,1,1,2,0.022,1.096,-0.369,0.458,1.242,1.401,2.35e-05,-12.567,-0.331,6.629 }}; + + +//See the description for the mean data. This table constains the +//parameter standard deviations +double const Millard2016TorqueMuscle::Anderson2007Table3Std[36][14] = { +{0,0,0,0,0.049,0.201,0.358,0.286,0.586,0.272,0.66,0.97,0.547,4.955 }, +{0,0,1,0,0.047,0.13,0.418,0.268,0.542,0.175,1.93,2.828,0.292,1.895 }, +{0,0,0,1,0.043,0.155,0.195,0.306,0.622,0.189,1.297,2.701,0.091,0.854 }, +{0,0,1,1,0.032,0.246,0.365,0.223,0.45,0.14,1.294,2.541,0.02,2.924 }, +{0,0,0,2,0.039,0.124,0.077,0.184,0.372,0.368,0.271,3.402,0.111,1.691 }, +{0,0,1,2,0.003,0.173,0.279,0.135,0.273,0.237,0.613,0.741,0.031,2.265 }, +{0,1,0,0,0.025,0.217,0.245,0.489,0.995,0.225,0.66,0.97,0.547,4.955 }, +{0,1,1,0,0.033,0.178,0.232,0.345,0.702,0.179,1.93,2.828,0.292,1.895 }, +{0,1,0,1,0.02,0.248,0.274,0.318,0.652,0.088,1.297,2.701,0.091,0.854 }, +{0,1,1,1,0.016,0.244,0.209,0.375,0.765,0.262,1.294,2.541,0.02,2.924 }, +{0,1,0,2,0.025,0.151,0.234,0.164,0.335,0.102,0.271,3.402,0.111,1.691 }, +{0,1,1,2,0.008,0.062,0.214,0.321,0.654,0.28,0.613,0.741,0.031,2.265 }, +{1,0,0,0,0.04,0.073,0.073,0.593,1.546,0.171,0,0,2.617,0.553 }, +{1,0,1,0,0.028,0.084,0.181,0.38,0.989,0.27,0,0,3.696,1.512 }, +{1,0,0,1,0.031,0.063,0.048,0.363,0.947,0.06,0,0,2.541,2.148 }, +{1,0,1,1,0.016,0.094,0.077,0.319,0.832,0.133,0,0,1.958,1.63 }, +{1,0,0,2,0.017,0.127,0.024,0.046,0.04,0.124,0,0,1.896,1.517 }, +{1,0,1,2,0.018,0.044,0.124,0.128,0.221,0.129,0,0,6.141,0.742 }, +{1,1,0,0,0.015,0.163,0.317,1.364,3.554,0.598,0,0,2.617,0.553 }, +{1,1,1,0,0.015,0.191,0.287,0.825,2.139,0.319,0,0,3.696,1.512 }, +{1,1,0,1,0.017,0.138,0.212,0.795,2.067,0.094,0,0,2.541,2.148 }, +{1,1,1,1,0.015,0.21,0.273,0.718,1.871,0.143,0,0,1.958,1.63 }, +{1,1,0,2,0.022,0.084,0.357,0.716,1.866,0.201,0,0,1.896,1.517 }, +{1,1,1,2,0.005,0.145,0.21,0.052,0.135,0.078,0,0,6.141,0.742 }, +{2,0,0,0,0.022,0.089,0.083,0.595,2.144,0.214,0.001193,7.384,0.323,1.196 }, +{2,0,1,0,0.034,0.19,0.186,0.487,1.76,0.213,0.01135,6.77,0.328,1.177 }, +{2,0,0,1,0.029,0.136,0.103,0.165,0.578,0.133,0.003331,10.291,0.403,3.119 }, +{2,0,1,1,0.026,0.235,0.143,0.448,1.613,0.27,3.562e-05,7.848,0.394,1.141 }, +{2,0,0,2,0.035,0.136,0.132,0.262,0.944,0.313,3.164e-05,1.786,0.258,0.902 }, +{2,0,1,2,0.006,0.095,0.115,0.258,0.423,0.158,2.535e-05,10.885,0.247,2.186}, +{2,1,0,0,0.005,0.19,0.067,0.108,0.301,0.134,0.001193,7.384,0.323,1.196 }, +{2,1,1,0,0.006,0.271,0.171,0.446,1.236,0.206,0.01135,6.77,0.328,1.177 }, +{2,1,0,1,0.005,0.479,0.178,0.216,0.601,0.214,0.003331,10.291,0.403,3.119 }, +{2,1,1,1,0.002,0.339,0.133,0.148,0.41,0.284,3.562e-05,7.848,0.394,1.141 }, +{2,1,0,2,0.002,0.195,0.056,0.188,0.521,0.29,3.164e-05,1.786,0.258,0.902 }, +{2,1,1,2,0.003,0.297,0.109,0.089,0.213,0.427,2.535e-05,10.885,0.247,2.186}}; + +/* + This table contains parameters for the newly made torque muscle curves: + + 1. maxIsometricTorque Nm + 2. maxAngularVelocity rad/s + 3. angleAtOneActiveNormTorque rad + 4. angularWidthActiveTorque rad + 5. tvAtMaxEccentricVelocity Nm/Nm + 6. tvAtHalfMaxConcentricVelocity Nm/Nm + 7. angleAtZeroPassiveTorque rad + 8. angleAtOneNormPassiveTorque rad + +*/ + + + +double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] = +{ {0,0,0,0,175.746, 9.02335, 1.06465, 1.05941, 1.1, 0.163849, 0.79158, 1.5708 }, + {0,1,0,0,157.293, 9.18043, 0.733038, 1.21999, 1.11905, 0.25, -0.0888019,-0.515207 }, + {1,0,0,0,285.619, 19.2161, 0.942478, 0.509636, 1.13292, 0.115304, 2.00713, 2.70526 }, + {1,1,0,0,98.7579, 16.633, 1.02974, 1.11003, 1.12, 0.19746, 0, -0.174533 }, + {2,0,0,0,127.561, 11.7646, 0.408, 0.660752, 1.159, 0.410591, 0.0292126, 0.785398 }, + {2,1,0,0,44.3106, 17.2746, -0.187, 0.60868, 1.2656, 0.112303, -0.523599, -1.0472 }, + {3,0,0,0,127.401, 16.249, 2.14675, 1.83085, 1.1, 0.250134, 2.8291, 3.55835 }, + {3,1,0,0,91.1388, 19.0764, 0.890118, 1.2898, 1.23011, 0.249656, -0.523599, -1.0472 }, + {5,0,0,0,15.5653, 36.5472, 1.55334, 1.38928, 1.16875, 0.115356, 1.0472, 1.5708 }, + {5,1,0,0,39.2252, 36.3901, 0.663225, 1.71042, 1.14, 0.115456, -0.793739, -1.49714 }, + {3,2,0,0,128.496, 18.05, 0.839204, 1.28041, 1.25856, 0.214179, 1.5708, 2.26893 }, + {3,3,0,0,94.6839, 18 , -0.277611, 2.37086, 1.23042, 0.224227, -0.523599, -1.0472 }, + {3,5,0,0,50.522 , 19.47, -1.18761, 2.80524, 1.27634, 0.285399, 1.39626, 1.91986 }, + {3,4,0,0,43.5837, 18, -0.670796, 1.98361, 1.35664, 0.229104, -1.0472, -1.5708 }, + {4,1,0,0,101.384, 18.1, 0.33, 3.62155, 1.37223, 0.189909, 0, -0.174533 }, + {4,0,0,0,69.8728, 18.45, 1.64319, 1.30795, 1.31709, 0.189676, 2.0944, 2.61799 }, + {5,6,0,0,13.5361, 35.45, -0.209204, 1.33735, 1.23945, 0.250544, -0.785398, -1.5708 }, + {5,7,0,0,12.976 , 27.88, -0.212389, 0.991803, 1.3877, 0.207506, 0.785398, 1.5708 }, + {5,9,0,0,31.4217, 18.02, 0.43, 1.47849, 1.34817, 0.196913, 0, -0.523599}, + {5,8,0,0,23.8345, 21.77, -1.14319, 2.56082, 1.31466, 0.2092, 0.349066, 0.872665 }, + {6,0,0,0,687.864, 1.0472, 1.5506, 1.14543, 1.1, 0.45, 0.306223, 1.35342 }, + {6,1,0,0,211.65 , 0.523599, 0, 6.28319, 1.1, 0.45, 0, -0.785398 }}; + +/************************************************************* + Map that goes from a single joint-torque-direction index to + the pair of joint and direction indicies used in the tables +*************************************************************/ + +const static struct JointSet{ + enum item { Hip = 0, + Knee, + Ankle, + Shoulder, + Elbow, + Wrist, + Lumbar, + Last}; + JointSet(){} +} JointSet; + + +struct DirectionSet{ + enum item { + Extension = 0, + Flexion, + HorizontalAdduction, + HorizontalAbduction, + ExternalRotation, + InternalRotation, + Supination, + Pronation, + RadialDeviation, + UlnarDeviation, + Last + }; + DirectionSet(){} +} DirectionSet; + + +const static int JointTorqueMap[22][3] = { + {(int)JointTorqueSet::HipExtension , (int)JointSet::Hip , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::HipFlexion , (int)JointSet::Hip , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::KneeExtension , (int)JointSet::Knee , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::KneeFlexion , (int)JointSet::Knee , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::AnkleExtension , (int)JointSet::Ankle , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::AnkleFlexion , (int)JointSet::Ankle , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ElbowExtension , (int)JointSet::Elbow , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ElbowFlexion , (int)JointSet::Elbow , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderExtension , (int)JointSet::Shoulder , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ShoulderFlexion , (int)JointSet::Shoulder , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::WristExtension , (int)JointSet::Wrist , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::WristFlexion , (int)JointSet::Wrist , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderHorizontalAdduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAdduction}, + {(int)JointTorqueSet::ShoulderHorizontalAbduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAbduction}, + {(int)JointTorqueSet::ShoulderInternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::InternalRotation }, + {(int)JointTorqueSet::ShoulderExternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::ExternalRotation }, + {(int)JointTorqueSet::WristUlnarDeviation , (int)JointSet::Wrist , (int)DirectionSet::UlnarDeviation }, + {(int)JointTorqueSet::WristRadialDeviation , (int)JointSet::Wrist , (int)DirectionSet::RadialDeviation }, + {(int)JointTorqueSet::WristPronation , (int)JointSet::Wrist , (int)DirectionSet::Pronation }, + {(int)JointTorqueSet::WristSupination , (int)JointSet::Wrist , (int)DirectionSet::Supination }, + {(int)JointTorqueSet::LumbarExtension , (int)JointSet::Lumbar , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::LumbarFlexion , (int)JointSet::Lumbar , (int)DirectionSet::Flexion }}; + + +/************************************************************* + Constructors +*************************************************************/ + +Millard2016TorqueMuscle:: + Millard2016TorqueMuscle( ) + :angleOffset(1.0), + signOfJointAngle(1.0), + signOfJointTorque(1.0), + signOfConcentricAnglularVelocity(1.0), + muscleName("empty") +{ + muscleCurvesAreDirty = true; +} + +Millard2016TorqueMuscle::Millard2016TorqueMuscle( + DataSet::item dataSet, + const SubjectInformation &subjectInfo, + int jointTorque, + double jointAngleOffsetRelativeToDoxygenFigures, + double signOfJointAngleRelativeToDoxygenFigures, + double signOfJointTorque, + const std::string& name + ):angleOffset(jointAngleOffsetRelativeToDoxygenFigures), + signOfJointAngle(signOfJointAngleRelativeToDoxygenFigures), + signOfJointTorque(signOfJointTorque), + signOfConcentricAnglularVelocity(signOfJointTorque), + muscleName(name), + dataSet(dataSet) +{ + + subjectHeightInMeters = subjectInfo.heightInMeters; + subjectMassInKg = subjectInfo.massInKg; + passiveCurveAngleOffset = 0.; + beta = 0.1; + + int gender = (int) subjectInfo.gender; + int ageGroup = (int) subjectInfo.ageGroup; + + int joint = -1; + int jointDirection = -1; + + for(int i=0; i < JointTorqueSet::Last; ++i){ + if(JointTorqueMap[i][0] == jointTorque){ + joint = JointTorqueMap[i][1]; + jointDirection = JointTorqueMap[i][2]; + } + } + + if(joint == -1 || jointDirection == -1){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": A jointTorqueDirection of " << jointTorque + << " does not exist."; + assert(0); + abort(); + } + + if( abs(abs(signOfJointAngle)-1) > EPSILON){ + + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not " + << signOfJointAngle; + assert(0); + abort(); + } + + if( abs(abs(signOfConcentricAnglularVelocity)-1) > EPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": signOfJointAngularVelocityDuringConcentricContraction " + << "must be [-1,1] not " + << signOfConcentricAnglularVelocity; + assert(0); + abort(); + } + + if( abs(abs(signOfJointTorque)-1) > EPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": signOfJointTorque must be [-1,1] not " + << signOfJointTorque; + assert(0); + abort(); + } + + + if(subjectHeightInMeters <= 0){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": subjectHeightInMeters > 0, but it's " + << subjectHeightInMeters; + assert(0); + abort(); + } + + if(subjectMassInKg <= 0){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << ": subjectMassInKg > 0, but it's " + << subjectMassInKg; + assert(0); + abort(); + } + + + + int idx = -1; + int jointIdx = 0; + int dirIdx = 1; + int genderIdx = 2; + int ageIdx = 3; + + switch(dataSet){ + case DataSet::Anderson2007: + { + c1c2c3c4c5c6Anderson2007.resize(6); + b1k1b2k2Anderson2007.resize(4); + + for(int i=0; i<36;++i){ + + if( abs(Anderson2007Table3Mean[i][jointIdx] + -(double)joint) < EPSILON + && abs(Anderson2007Table3Mean[i][dirIdx] + -(double)jointDirection) < EPSILON + && abs(Anderson2007Table3Mean[i][genderIdx] + -(double)gender) < EPSILON + && abs(Anderson2007Table3Mean[i][ageIdx] + -(double)ageGroup) < EPSILON){ + idx = i; + } + } + + if(idx != -1){ + for(int i=0; i<6; ++i){ + c1c2c3c4c5c6Anderson2007[i] = Anderson2007Table3Mean[idx][i+4]; + } + for(int i=0; i<4; ++i){ + b1k1b2k2Anderson2007[i] = Anderson2007Table3Mean[idx][i+10]; + } + } + + } break; + + case DataSet::Gymnast: + { + gymnastParams.resize(8); + for(int i=0; i(this); + mutableThis->updateTorqueMuscleCurves(); + } + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double ta = taCurve.calcValue(fiberAngle); + double tp = tpCurve.calcValue(fiberAngle); + double fiberVelocityNorm = fiberVelocity/omegaMax; + double tv = tvCurve.calcValue(fiberVelocityNorm); + + + double jointTorque = maxActiveIsometricTorque*( + activation*ta*tv + + passiveTorqueScale*tp + - beta*fiberVelocityNorm); + if(jointTorque < 0){ + jointTorque = 0; + } + + return jointTorque*signOfJointTorque; +} + + + +void Millard2016TorqueMuscle:: + calcTorqueMuscleInfo(double jointAngle, + double jointAngularVelocity, + double activation, + TorqueMuscleInfo& tmi) const +{ + if(muscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double ta = taCurve.calcValue(fiberAngle); + double tp = passiveTorqueScale*tpCurve.calcValue(fiberAngle); + + double omegaNorm = fiberAngularVelocity/omegaMax; + double D_wn_w = 1.0/omegaMax; + double tv = tvCurve.calcValue(omegaNorm); + + double betaUpd = beta; + double tb = -betaUpd*omegaNorm; + + //The jointTorque is not allowed to go below 0, as this corresponds + //to the muscle pushing. If the joint torque is going negative, we update + //beta so that it will perfectly zero the joint torque. + if(activation*ta*tv + passiveTorqueScale*tp + tb < 0){ + betaUpd = (activation*ta*tv + passiveTorqueScale*tp)/omegaNorm; + tb = -betaUpd*omegaNorm; + } + + double D_ta_DfiberAngle = taCurve.calcDerivative(fiberAngle,1); + double D_tp_DfiberAngle = passiveTorqueScale*tpCurve.calcDerivative(fiberAngle,1); + double D_tv_DfiberAngularVelocity + = tvCurve.calcDerivative(omegaNorm,1)*D_wn_w; + double D_tb_DfiberAngularVelocity = -betaUpd*D_wn_w; + + double D_fiberAngle_D_jointAngle = signOfJointAngle; + double D_tv_DfiberAngularVelocity_D_jointAngularVelocity = + signOfConcentricAnglularVelocity; + + + tmi.jointAngle = jointAngle; + tmi.jointAngularVelocity = jointAngularVelocity; + tmi.fiberAngle = fiberAngle; + //tmi.tendonAngle = 0.; + tmi.fiberAngularVelocity = fiberAngularVelocity; + //tmi.tendonAngularVelocity = 0.; + + tmi.fiberPassiveTorqueAngleMultiplier = tp; + tmi.fiberActiveTorqueAngleMultiplier = ta; + tmi.fiberActiveTorqueAngularVelocityMultiplier = tv; + //tmi.tendonTorqueAngleMultiplier = activation*ta*tv + tp; + + tmi.activation = activation; + tmi.fiberActiveTorque = maxActiveIsometricTorque*(activation*ta*tv); + tmi.fiberPassiveTorque = maxActiveIsometricTorque*(tb+tp); + tmi.fiberDampingTorque = maxActiveIsometricTorque*(tb); + tmi.fiberNormDampingTorque = tb; + tmi.fiberTorque = tmi.fiberActiveTorque + + tmi.fiberPassiveTorque; + //tmi.tendonTorque = tmi.fiberActiveTorque + // + tmi.fiberPassiveTorque; + + tmi.jointTorque = signOfJointTorque*tmi.fiberTorque; + + tmi.fiberStiffness = maxActiveIsometricTorque*( + activation*D_ta_DfiberAngle*tv + + D_tp_DfiberAngle); + + //tmi.tendonStiffness = std::numeric_limits::infinity(); + tmi.jointStiffness = signOfJointTorque + *tmi.fiberStiffness + *D_fiberAngle_D_jointAngle; + + tmi.fiberActivePower = tmi.fiberActiveTorque + * tmi.fiberAngularVelocity; + tmi.fiberPassivePower = tmi.fiberPassiveTorque + * tmi.fiberAngularVelocity; + tmi.fiberPower = tmi.fiberActivePower + + tmi.fiberPassivePower; + //tmi.tendonPower = 0.; + tmi.jointPower = tmi.jointTorque * jointAngularVelocity; + + + //tau = signTq*tauIso*(a * ta(theta) * tv(thetaDot) + tp(theta) ) + // Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) ) + tmi.DjointTorqueDactivation = + signOfJointTorque + *maxActiveIsometricTorque + *(ta * tv); + + //Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot)* ) + tmi.DjointTorqueDjointAngularVelocity = + signOfJointTorque + * maxActiveIsometricTorque + * ( activation + * ta + * D_tv_DfiberAngularVelocity + * D_tv_DfiberAngularVelocity_D_jointAngularVelocity + + D_tb_DfiberAngularVelocity); + + tmi.DjointTorqueDjointAngle = + signOfJointTorque + * maxActiveIsometricTorque + * ( activation + *D_ta_DfiberAngle + * tv + + D_tp_DfiberAngle + )* D_fiberAngle_D_jointAngle; + +} + + +/************************************************************* + Get / Set Functions +*************************************************************/ + +double Millard2016TorqueMuscle:: + getJointTorqueSign() const +{ + return signOfJointTorque; +} + +double Millard2016TorqueMuscle:: + getJointAngleSign() const +{ + return signOfJointAngle; +} + +double Millard2016TorqueMuscle:: + getJointAngleOffset() const +{ + return angleOffset; +} + + +double Millard2016TorqueMuscle:: + getNormalizedDampingCoefficient() const +{ + return beta; +} + +void Millard2016TorqueMuscle:: + setNormalizedDampingCoefficient(double betaUpd) +{ + if(betaUpd < 0){ + cerr << "Millard2016TorqueMuscle::" + << "setNormalizedDampingCoefficient:" + << muscleName + << "beta is " << betaUpd + << " but beta must be > 0 " + << endl; + assert(0); + abort(); + } + + + beta = betaUpd; +} + + + + +double Millard2016TorqueMuscle:: + getMaximumActiveIsometricTorque() const +{ + return maxActiveIsometricTorque; +} + +double Millard2016TorqueMuscle:: + getMaximumJointAngularVelocity() const +{ + return calcFiberAngularVelocity(omegaMax); +} + +void Millard2016TorqueMuscle:: + setMaximumActiveIsometricTorque(double maxIsoTorque) +{ + muscleCurvesAreDirty = true; + maxActiveIsometricTorque = maxIsoTorque; +} + + +double Millard2016TorqueMuscle:: + getJointAngleAtMaximumActiveIsometricTorque() const +{ + return calcJointAngle(angleAtOneNormActiveTorque); +} + +double Millard2016TorqueMuscle:: + getJointAngleAtOneNormalizedPassiveIsometricTorque() const +{ + if(muscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngle(angleAtOneNormPassiveTorque); +} + + +double Millard2016TorqueMuscle:: + getPassiveTorqueScale() const +{ + return passiveTorqueScale; +} + +void Millard2016TorqueMuscle:: + setPassiveTorqueScale(double passiveTorqueScaling) +{ + muscleCurvesAreDirty = true; + passiveTorqueScale = passiveTorqueScaling; +} + + +double Millard2016TorqueMuscle:: + getPassiveCurveAngleOffset() const +{ + return passiveCurveAngleOffset; +} + +void Millard2016TorqueMuscle:: + setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal) +{ + muscleCurvesAreDirty = true; + passiveCurveAngleOffset = passiveCurveAngleOffsetVal; +} + + +void Millard2016TorqueMuscle:: + fitPassiveCurveAngleOffset(double jointAngleTarget, + double passiveTorqueTarget) +{ + muscleCurvesAreDirty = true; + setPassiveCurveAngleOffset(0.0); + + if(passiveTorqueTarget < SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveTorqueScale:" + << muscleName + << ": passiveTorque " << passiveTorqueTarget + << " but it should be greater than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + //Solve for the fiber angle at which the curve develops + //the desired passiveTorque + double normPassiveTorque = passiveTorqueTarget + /maxActiveIsometricTorque; + //Ge a good initial guess + + VectorNd curveDomain = tpCurve.getCurveDomain(); + double angleRange = abs( curveDomain[1]-curveDomain[0]); + double fiberAngle = 0.5*(curveDomain[0]+curveDomain[1]); + double jointAngleCurr = calcJointAngle(fiberAngle); + + TorqueMuscleInfo tqInfo = TorqueMuscleInfo(); + + calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo); + double err = tqInfo.fiberPassiveTorqueAngleMultiplier-normPassiveTorque; + double jointAngleLeft = 0; + double jointAngleRight = 0; + double errLeft = 0; + double errRight = 0; + + double h = 0.25*angleRange; + + //Get a good initial guess - necessary because these curves + //can be *very* nonlinear. + int iter = 0; + int iterMax = 10; + int tol = sqrt(SQRTEPSILON); + + while(iter < iterMax && abs(err) > tol){ + jointAngleLeft = jointAngleCurr-h; + jointAngleRight = jointAngleCurr+h; + + calcTorqueMuscleInfo(jointAngleLeft,0.,0.,tqInfo); + errLeft = tqInfo.fiberPassiveTorqueAngleMultiplier + - normPassiveTorque; + + calcTorqueMuscleInfo(jointAngleRight,0.,0.,tqInfo); + errRight = tqInfo.fiberPassiveTorqueAngleMultiplier + - normPassiveTorque; + + if(abs(errLeft) SQRTEPSILON && iter < iterMax){ + calcTorqueMuscleInfo(jointAngleCurr,0.,0.,tqInfo); + err = tqInfo.fiberPassiveTorqueAngleMultiplier + -normPassiveTorque; + derr = signOfJointTorque*tqInfo.DjointTorqueDjointAngle + / maxActiveIsometricTorque ; + delta = -err/derr; + jointAngleCurr += delta; + ++iter; + } + + if(abs(err)>SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveCurveAngleOffset:" + << muscleName + << ": failed to fit the passive curve offset " + << " such that the curve develops the desired " + << " passive torque at the given joint angle. This" + << " should not be possible - contact the maintainer " + << " of this addon." + << endl; + assert(0); + abort(); + } + double fiberAngleOffset = calcFiberAngle(jointAngleTarget) + - calcFiberAngle(jointAngleCurr); + + setPassiveCurveAngleOffset(fiberAngleOffset); + +} + +void Millard2016TorqueMuscle:: + fitPassiveTorqueScale(double jointAngleTarget, + double passiveTorqueTarget) +{ + muscleCurvesAreDirty = true; + setPassiveTorqueScale(1.0); + + VectorNd curveDomain = tpCurve.getCurveDomain(); + VectorNd curveRange = VectorNd(2); + curveRange[0] = tpCurve.calcValue(curveDomain[0]); + curveRange[1] = tpCurve.calcValue(curveDomain[1]); + double fiberAngle = calcFiberAngle(jointAngleTarget); + + if( (fiberAngle < curveDomain[0] && (curveRange[0] < curveRange[1])) + || (fiberAngle > curveDomain[1] && (curveRange[1] < curveRange[0])) ){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveTorqueScale:" + << muscleName + << ": joint angle is " << jointAngleTarget + << " but it should be between " + << calcJointAngle(curveDomain[0]) << " and " + << calcJointAngle(curveDomain[1]) + << endl; + assert(0); + abort(); + } + + if(passiveTorqueTarget < SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveTorqueScale:" + << muscleName + << ": passiveTorque " << passiveTorqueTarget + << " but it should be greater than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + double normPassiveTorque = passiveTorqueTarget/maxActiveIsometricTorque; + int iter = 0; + int iterMax = 10; + double err = SQRTEPSILON*2; + double derr = 0; + double delta= 0; + double scale = 1.0; + setPassiveTorqueScale(scale); + TorqueMuscleInfo tqInfo = TorqueMuscleInfo(); + + while(abs(err) > SQRTEPSILON && iter < iterMax){ + setPassiveTorqueScale(scale); + calcTorqueMuscleInfo(jointAngleTarget,0,0,tqInfo); + err = tqInfo.fiberPassiveTorqueAngleMultiplier - normPassiveTorque; + derr= tqInfo.fiberPassiveTorqueAngleMultiplier/scale; + delta = -err/derr; + scale += delta; + + if(scale < SQRTEPSILON){ + scale = SQRTEPSILON; + } + iter++; + } + + if(abs(err) > SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveTorqueScale:" + << muscleName + << ": passiveTorqueScale could not be fit to" + << " the data given. See the maintainer of this" + << " addon for help." + << endl; + assert(0); + abort(); + } +} + +/* +const RigidBodyDynamics::Math::VectorNd& + Millard2016TorqueMuscle::getParametersc1c2c3c4c5c6() +{ + return c1c2c3c4c5c6Anderson2007; +} + +const RigidBodyDynamics::Math::VectorNd& + Millard2016TorqueMuscle::getParametersb1k1b2k2() +{ + return b1k1b2k2Anderson2007; +} +*/ + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: + getActiveTorqueAngleCurve() const +{ + //This must be updated if the parameters have changed + if(muscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return taCurve; +} + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: +getPassiveTorqueAngleCurve() const +{ + //This must be updated if the parameters have changed + if(muscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return tpCurve; +} + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: +getTorqueAngularVelocityCurve() const +{ + //This must be updated if the parameters have changed + if(muscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return tvCurve; +} + + +string Millard2016TorqueMuscle::getName(){ + return muscleName; +} + +void Millard2016TorqueMuscle::setName(string &name){ + muscleCurvesAreDirty = true; + muscleName = name; +} + + + +/************************************************************* + Utilities +*************************************************************/ +//fa = signJointAngle*(ja - jaO) +// ja = signJointAngle*fa + ja0 +//dja = signJointAngle*dfa + +double Millard2016TorqueMuscle:: + calcFiberAngle(double jointAngle) const +{ + return signOfJointAngle*(jointAngle-angleOffset); +} + +double Millard2016TorqueMuscle:: + calcJointAngle(double fiberAngle) const +{ + return fiberAngle*signOfJointAngle + angleOffset; +} + + +double Millard2016TorqueMuscle:: + calcFiberAngularVelocity(double jointAngularVelocity) const +{ + return signOfConcentricAnglularVelocity*jointAngularVelocity; +} + +void Millard2016TorqueMuscle::updateTorqueMuscleCurves() +{ + std::string tempName = muscleName; + + switch(dataSet){ + case DataSet::Anderson2007: + { + double c4 = c1c2c3c4c5c6Anderson2007[3]; + double c5 = c1c2c3c4c5c6Anderson2007[4]; + omegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) ); + + scaleFactorAnderson2007 = subjectHeightInMeters + *subjectMassInKg + *gravity; + maxActiveIsometricTorque = scaleFactorAnderson2007 + *c1c2c3c4c5c6Anderson2007[0]; + + angleAtOneNormActiveTorque = c1c2c3c4c5c6Anderson2007[2]; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueAngleCurve( + c1c2c3c4c5c6Anderson2007[1], + c1c2c3c4c5c6Anderson2007[2], + tempName.append("_taCurve"), + taCurve); + + tempName = muscleName; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueVelocityCurve( + c1c2c3c4c5c6Anderson2007[3], + c1c2c3c4c5c6Anderson2007[4], + c1c2c3c4c5c6Anderson2007[5], + 1.1, + 1.4, + tempName.append("_tvCurve"), + tvCurve); + + tempName = muscleName; + + double normMaxActiveIsometricTorque = maxActiveIsometricTorque + /scaleFactorAnderson2007; + + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + scaleFactorAnderson2007, + normMaxActiveIsometricTorque, + b1k1b2k2Anderson2007[0], + b1k1b2k2Anderson2007[1], + b1k1b2k2Anderson2007[2], + b1k1b2k2Anderson2007[3], + tempName.append("_tpCurve"), + tpCurve); + + tpCurve.shift(passiveCurveAngleOffset,0); + + double k = 0; + double b = 0; + + if(b1k1b2k2Anderson2007[0] > 0){ + b = b1k1b2k2Anderson2007[0]; + k = b1k1b2k2Anderson2007[1]; + }else if(b1k1b2k2Anderson2007[2] > 0){ + b = b1k1b2k2Anderson2007[2]; + k = b1k1b2k2Anderson2007[3]; + } + + if(abs(b) > 0 && passiveTorqueScale > SQRTEPSILON){ + angleAtOneNormPassiveTorque = + (1/k)*log(abs(maxActiveIsometricTorque/b)) + + passiveCurveAngleOffset; + }else{ + angleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + + + //angleAtOneNormPassiveTorque + //gymnastParams[Gymnast::PassiveAngleAtOneNormTorque] + + + } break; + case DataSet::Gymnast: + { + omegaMax = gymnastParams[ + Gymnast::OmegaMax]; + maxActiveIsometricTorque = gymnastParams[ + Gymnast::TauMax]; + angleAtOneNormActiveTorque = gymnastParams[ + Gymnast::ActiveAngleAtOneNormTorque]; + + TorqueMuscleFunctionFactory:: + createGaussianShapedActiveTorqueAngleCurve( + gymnastParams[Gymnast::ActiveAngleAtOneNormTorque], + gymnastParams[Gymnast::ActiveAngularStandardDeviation], + tempName.append("_taCurve"), + taCurve); + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + gymnastParams[Gymnast::PassiveAngleAtZeroTorque], + gymnastParams[Gymnast::PassiveAngleAtOneNormTorque], + tempName.append("_tpCurve"), + tpCurve); + + tpCurve.shift(passiveCurveAngleOffset,0); + + TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + gymnastParams[Gymnast::TvAtMaxEccentricVelocity], + gymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity], + tempName.append("_tvCurve"), + tvCurve); + + if(passiveTorqueScale > 0){ + angleAtOneNormPassiveTorque = + gymnastParams[Gymnast::PassiveAngleAtOneNormTorque] + + passiveCurveAngleOffset; + }else{ + angleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + + } break; + default: + { + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << "dataSet has a value of " << dataSet + << " which is not a valid choice"; + assert(0); + abort(); + } + }; + + + + //If the passiveScale is < 1 and > 0, then we must iterate to + //find the true value of angleAtOneNormPassiveTorque; + + if(isfinite(angleAtOneNormPassiveTorque) + && (passiveTorqueScale > 0 && passiveTorqueScale != 1.0) ){ + int iter = 1; + int iterMax =100; + double err = 10*SQRTEPSILON; + double derr = 0; + double delta = 0; + + while(abs(err) > SQRTEPSILON && iter < iterMax){ + + err = passiveTorqueScale*tpCurve.calcValue(angleAtOneNormPassiveTorque) + - 1.0; + derr = passiveTorqueScale + *tpCurve.calcDerivative(angleAtOneNormPassiveTorque,1); + + delta = -err/derr; + angleAtOneNormPassiveTorque += delta; + ++iterMax; + } + + if(abs(err) > SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << muscleName + << "Internal Error: failed to solve for " + <<"angleAtOneNormPassiveTorque. Consult the maintainer of this " + <<"addon"; + assert(0); + abort(); + } + + } + + + + + muscleCurvesAreDirty = false; +} + + + +void Millard2016TorqueMuscle::printJointTorqueProfileToFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + int numberOfSamplePoints) +{ + if(muscleCurvesAreDirty){ + updateTorqueMuscleCurves(); + } + + VectorNd activeDomain = taCurve.getCurveDomain(); + VectorNd passiveDomain = tpCurve.getCurveDomain(); + VectorNd velocityDomain= tvCurve.getCurveDomain(); + + double angleMin = activeDomain[0]; + double angleMax = activeDomain[1]; + + if(tpCurve.calcValue(passiveDomain[0]) >= 0.99){ + angleMin = passiveDomain[0]; + } + + if(tpCurve.calcValue(passiveDomain[1]) >= 0.99){ + angleMax = passiveDomain[1]; + } + + double jointMin = signOfJointAngle*angleMin + angleOffset; + double jointMax = signOfJointAngle*angleMax + angleOffset; + + if(jointMin > jointMax){ + double tmp = jointMin; + jointMin=jointMax; + jointMax=tmp; + } + double range = jointMax-jointMin; + jointMin = jointMin -range*0.1; + jointMax = jointMax +range*0.1; + double jointDelta = (jointMax-jointMin) + /((double)numberOfSamplePoints-1.); + + double velMin = omegaMax*signOfConcentricAnglularVelocity*velocityDomain[0]; + double velMax = omegaMax*signOfConcentricAnglularVelocity*velocityDomain[1]; + + if(velMin > velMax){ + double tmp = velMin; + velMin = velMax; + velMax = tmp; + } + double velRange = velMax-velMin; + velMin = velMin-0.1*velRange; + velMax = velMax+0.1*velRange; + double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0); + + double angleAtMaxIsoTorque = angleAtOneNormActiveTorque; + + std::vector< std::vector < double > > matrix; + std::vector < double > row(21); + std::string header("jointAngle," + "jointVelocity," + "activation," + "fiberAngle," + "fiberAngularVelocity," + "passiveTorqueAngleMultiplier," + "activeTorqueAngleMultiplier," + "torqueVelocityMultiplier," + "activeTorque," + "passiveTorque," + "fiberTorque," + "jointTorque," + "fiberStiffness," + "jointStiffness," + "fiberActivePower," + "fiberPassivePower," + "fiberPower," + "jointPower," + "DjointTorqueDactivation," + "DjointTorqueDjointAngularVelocity," + "DjointTorqueDjointAngle"); + + double activation =1.0; + double jointAngle = 0.; + double jointVelocity = 0.; + + + for(int i=0; i + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include "../geometry/SmoothSegmentedFunction.h" + + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + + /** + This struct contains an enumerated list of the data sets which + constain torque muscles. Thus far this list includes + + -Anderson2007: Table 3 from Anderson et al. + -Gymnast: A set of torque muscles for the whole body (in progress) + + For details of these data sets please refer to the main description + of this class. + + */ + const static struct DataSet{ + enum item{ + Anderson2007 = 0, + Gymnast, + Last + }; + const static char* names[]; + DataSet(){} + } DataSet; + + + /** + This struct contains an enumerated list of the genders for which + data torque data has been reported. + */ + const static struct GenderSet{ + enum item { + Male = 0, + Female, + Last + }; + const static char* names[]; + GenderSet(){} + } GenderSet; + + /** + This struct contains an enumerated list of the age groups for which + data torque data has been reported. + */ + const static struct AgeGroupSet{ + enum item { + Young18To25 = 0, + Middle55To65, + SeniorOver65, + Last + }; + const static char* names[]; + AgeGroupSet(){} + } AgeGroupSet; + + /** + This struct contains an enumerated list of the joint-torque-directions + for which data torque data has been reported. + */ + const static struct JointTorqueSet{ + enum item{ + HipExtension = 0, + HipFlexion = 1, + KneeExtension = 2, + KneeFlexion = 3, + AnkleExtension = 4, + AnkleFlexion = 5, + ElbowExtension = 6, + ElbowFlexion = 7, + ShoulderExtension = 8, + ShoulderFlexion = 9, + WristExtension = 10, + WristFlexion = 11, + ShoulderHorizontalAdduction = 12, + ShoulderHorizontalAbduction = 13, + ShoulderInternalRotation = 14, + ShoulderExternalRotation = 15, + WristUlnarDeviation = 16, + WristRadialDeviation = 17, + WristPronation = 18, + WristSupination = 19, + LumbarExtension = 20, + LumbarFlexion = 21, + Last = 22 + }; + const static char* names[]; + JointTorqueSet(){} + } JointTorqueSet; + + /** + This struct contains 3 enumerated lists (Gender, AgeGroup, + JointTorque) that lists the genders, age groups, and + joint-torque-directions for which the Anderson2007 data set contains + data. Please refer to the class description for more details about + this data set and how to use it. + */ + const static struct Anderson2007{ + enum Gender { + Male = GenderSet::Male, + Female = GenderSet::Female, + LastGender + }; + enum AgeGroup { + Young18To25 = AgeGroupSet::Young18To25, + Middle55To65 = AgeGroupSet::Middle55To65, + SeniorOver65 = AgeGroupSet::SeniorOver65, + LastAgeGroup + }; + enum JointTorque{ + HipExtension = JointTorqueSet::HipExtension , + HipFlexion = JointTorqueSet::HipFlexion , + KneeExtension = JointTorqueSet::KneeExtension , + KneeFlexion = JointTorqueSet::KneeFlexion , + AnkleExtension = JointTorqueSet::AnkleExtension, + AnkleFlexion = JointTorqueSet::AnkleFlexion, + LastJointTorque + }; + const static char* GenderNames[]; + const static char* AgeGroupNames[]; + const static char* JointTorqueNames[]; + Anderson2007(){} + } Anderson2007; + + /** + This struct contains 3 enumerated lists (Gender, AgeGroup, + JointTorque) that lists the genders, age groups, and + joint-torque-directions for which the Gymnast data set contains + data. At the present time Gymnast data set only contains data + appropriate for a young (18-25) elite male gymnast. Please refer + to the main class description for details on this data set and how + to use it. + */ + const static struct Gymnast{ + enum Gender { + Male = GenderSet::Male, + LastGender + }; + enum AgeGroup { + Young18To25 = AgeGroupSet::Young18To25, + LastAgeGroup + }; + + enum TableIndex { + TauMax = 0, + OmegaMax, + ActiveAngleAtOneNormTorque, + ActiveAngularStandardDeviation, + TvAtMaxEccentricVelocity, + TvAtHalfMaxConcentricVelocity, + PassiveAngleAtZeroTorque, + PassiveAngleAtOneNormTorque, + LastTableIndex + }; + + enum JointTorque { + HipExtension = JointTorqueSet::HipExtension , + HipFlexion = JointTorqueSet::HipFlexion , + KneeExtension = JointTorqueSet::KneeExtension , + KneeFlexion = JointTorqueSet::KneeFlexion , + AnkleExtension = JointTorqueSet::AnkleExtension , + AnkleFlexion = JointTorqueSet::AnkleFlexion , + ElbowExtension = JointTorqueSet::ElbowExtension , + ElbowFlexion = JointTorqueSet::ElbowFlexion , + ShoulderExtension = JointTorqueSet::ShoulderExtension , + ShoulderFlexion = JointTorqueSet::ShoulderFlexion , + WristExtension = JointTorqueSet::WristExtension , + WristFlexion = JointTorqueSet::WristFlexion , + ShoulderHorizontalAdduction = + JointTorqueSet::ShoulderHorizontalAdduction, + ShoulderHorizontalAbduction = + JointTorqueSet::ShoulderHorizontalAbduction, + ShoulderInternalRotation = + JointTorqueSet::ShoulderInternalRotation , + ShoulderExternalRotation = + JointTorqueSet::ShoulderExternalRotation , + WristUlnarDeviation = JointTorqueSet::WristUlnarDeviation , + WristRadialDeviation = JointTorqueSet::WristRadialDeviation, + WristPronation = JointTorqueSet::WristPronation , + WristSupination = JointTorqueSet::WristSupination , + LumbarExtension = JointTorqueSet::LumbarExtension, + LumbarFlexion = JointTorqueSet::LumbarFlexion, + LastJointTorque + }; + const static char* GenderNames[]; + const static char* AgeGroupNames[]; + const static char* JointTorqueNames[]; + Gymnast(){} + } Gymnast; + + + + /** + This is a struct that contains subject-specific information that + does not change for a given subject. + + @param gender + Male/Female. Selecting a gender that + is not present in a data set will result in + the program aborting and an error message + being printed to the terminal. + + @param ageGroup: + Presently the age group options include Young (18-25), + middle aged (55-65), and senior (>65). Selecting an + age group that is not present in a data set will result + in the program aborting and an error message + being printed to the terminal. + + @param subjectHeightInMeters + This parameter is used to scale from the normalized + curves reported by Anderson et al. + See the class description for details. + + @param subjectMassInKg + This parameter is used to scale from the normalized + curves reported by Anderson et al. + See the class description for details. + */ + struct SubjectInformation{ + GenderSet::item gender; + AgeGroupSet::item ageGroup; + double heightInMeters; + double massInKg; + }; + + + + struct TorqueMuscleInfo{ + + /**The angle of the joint (radians)*/ + double jointAngle; + + /**The angular velocity of the joint, where the sign convention + is chosen by the user at the time the torque muscle is created + (radians/sec)*/ + double jointAngularVelocity; + + /**The angle that the muscle fiber spans (radians)*/ + double fiberAngle; + + //The angle that the tendon spans (radians) + //double tendonAngle; + + /**The rate-of-angular-lengthening of the fiber. + A positive sign is for a concentric contraction, + that is where the fibers are shortening. (radians/sec)*/ + double fiberAngularVelocity; + + /*The rate-of-angular-lengthening of the tendon. + A positive sign is for a concentric contraction, + that is where the tendon is shortening. (radians/sec)*/ + //double tendonAngularVelocity; + + /** The normalized value of the passive-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberPassiveTorqueAngleMultiplier; + + /**The normalized value of the active-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberActiveTorqueAngleMultiplier; + + /**The normalized value of the torque-angular-velocity curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberActiveTorqueAngularVelocityMultiplier; + + /*The normalized value of the tendon-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + //double tendonTorqueAngleMultiplier; + + /**The activation of the muscle*/ + double activation; + + /**The torque generated by the active element of the muscle + fiber (Nm)*/ + double fiberActiveTorque; + + /**The torque generated by the passive element of the muscle + fiber (Nm)*/ + double fiberPassiveTorque; + + /**The torque generated by the damping element (Nm)*/ + double fiberDampingTorque; + + /**The torque generated by the damping element (Nm)*/ + double fiberNormDampingTorque; + + /**The torque generated by the entire fiber (Nm)*/ + double fiberTorque; + + /*The torque transmitted through the tendon across the joint (Nm)*/ + //double tendonTorque; + + /**The joint torque developed by the muscle. This is signed so + that it is consistent with the sign convention of the joint + chosen by the user. (Nm)*/ + double jointTorque; + + /**The stiffness of the fiber (Nm/rad)*/ + double fiberStiffness; + + /*The stiffness of the tendon (Nm/rad)*/ + //double tendonStiffness; + + /** The stiffness of the joint. This is signed so + that it is consistent with the sign convention of the joint + chosen by the user. (Nm/rad)*/ + double jointStiffness; + + /**The power output of the active fiber element. A positive + power means that the fiber is contracting concentrically. + (Watts - Nm/s)*/ + double fiberActivePower; + + /**The power output of the passive fiber element. A positive + power means that the passive element is recoiling concentrically + (Watts - Nm/s)*/ + double fiberPassivePower; + + /**The total power output of the fiber element.(Watts - Nm/s)*/ + double fiberPower; + + /**The partial derivative of joint torque w.r.t the joint angle*/ + double DjointTorqueDjointAngle; + + /**The partial derivative of joint torque w.r.t the joint + angular velocity*/ + double DjointTorqueDjointAngularVelocity; + + /**The partial derivative of joint torque w.r.t activation*/ + double DjointTorqueDactivation; + + /*The power output of the tendon. A positive power means that + the tendon is physically shortening. (Watts - Nm/s)*/ + //double tendonPower; + + /** The power output by this muscle at the joint. This is + signed so that it is consistent with the sign convention of the + joint chosen by the user. (Nm/rad)*/ + double jointPower; + + TorqueMuscleInfo(): + jointAngle(nan("1")), + jointAngularVelocity(nan("1")), + fiberAngle(nan("1")), + fiberAngularVelocity(nan("1")), + fiberPassiveTorqueAngleMultiplier(nan("1")), + fiberActiveTorqueAngleMultiplier(nan("1")), + fiberActiveTorqueAngularVelocityMultiplier(nan("1")), + activation(nan("1")), + fiberActiveTorque(nan("1")), + fiberPassiveTorque(nan("1")), + fiberTorque(nan("1")), + jointTorque(nan("1")), + fiberStiffness(nan("1")), + jointStiffness(nan("1")), + fiberActivePower(nan("1")), + fiberPassivePower(nan("1")), + fiberPower(nan("1")), + jointPower(nan("1")){} + + }; + + /** + This class implements a rigid-tendon torque muscle for a growing + list of joints and torque-directions. This rigid-tendon torque + muscle model provides modeling support for 3 phenomena + + -torque-angle curve (\f$\mathbf{t}_A(\theta)\f$): the variation of active isometric torque in one direction as a function of joint angle + -torque-velocity curve (\f$\mathbf{t}_V(\dot{\theta})\f$): the variation of torque as a function of angular velocity + -passive-torque-angle curve (\f$s_P\mathbf{t}_P(\theta-\theta_S)\f$): the variation of passive torque as a function of joint angle. Here \f$s_P\f$ and \f$\theta_S\f$ are user-defined scaling and shift parameters. + + each of which are represented as smooth normalized curves that + vary between 0 and 1. These three phenomena are used to compute the + torque developed \f$\tau\f$ given the angle of the joint + \f$\theta\f$, the angular-velocity of the joint \f$\dot{\theta}\f$, + and the activation of the muscle \f$\mathbf{a}\f$ (a 0-1 quantity + that defines how much the muscle is turned-on, or activated), and + the maximum-isometric torque \f$\tau_{ISO}\f$ + \f[ + \tau (\mathbf{a}, \theta,\dot{\theta}) = + \begin{cases} + \tau_{ISO} ( \mathbf{a} \, \mathbf{t}_A(\theta) \mathbf{t}_V(\dot{\theta}/\dot{\theta}_{MAX}) + - \beta \dot{\theta}/\dot{\theta}_{MAX} + + s_P \mathbf{t}_P(\theta-\theta_S) \, ) & \text{ if } \,\, \tau (\mathbf{a}, \theta,\dot{\theta}) > 0 \\ + 0 & \text{otherwise} \end{cases} + \f] + The + This model does not yet provide support for the following phenomena + but will in the future. + + -activation dynamics: this is to be decided by the modeler. + -tendon-elasticity + -muscle short-range-stiffness + + All of these characteristic curves are represented using \f$C_2\f$ + continuous \f$5^{th}\f$ order Bezier curves that have been fitted to + the data from data in the literature. In many cases these + curves have been carefully edited so that they fit the curves of + the original papers, but have more desireable numerical properties + for optimal control work. The characterisic curves provided by this + class have been fitted to a growing list of data sets: + -Anderson Data Set: from Anderson et al. 2007 + -Whole-body Gymnast Data Set: from Jackson, Kentel et al., Anderson et al., Dolan et al. and Raschke et al. + + Data Set: Anderson2007 + + This data set uses the mean value of the coefficients published + in Anderson et al. The standard deviation table has also been + entered. However, since it is unclear how to use the standard + deviation in a consistent way across all joints/parameters this + table is not yet accessible through the constructor. This data + set includes coefficients for the following + + -Number of subjects: 34 + -Gender: male and female + -Age: young (18-25, 14 subjects), middle-aged (55-65, 14 subjects), senior (> 65, 6 subjects) + -Joint: hip/knee/ankle + -Direction: extension/flexion + + Notes + -# Angles are plotted using units of degrees for readability. The + actual curves are described in units of radians + -# See Anderson et al. for further details. + + \image html fig_MuscleAddon_Anderson2007AllPositiveSigns.png "Characteristic from Anderson et al. 2007 [1]" + + + Data Set: Gymnast + + This data set is an attempt at making enough torque muscles for a + whole body. Since no single source in the literature comes close to + measuring the characteristics of all of the joints, data from + Jackson et al., Kentel et al., Anderson et al., Dolan et al, and + Raschke et al. + have been combined. Since the subjects used in these various studies + are wildly different (Jackson et al. measured an elite male gymnast; + Kentel et al. measured an elite tennis player; Anderson et al. measured, + in the category of young male, a selection of active undergraduate + students, Dolan et al from 126 women and 23 men, and Raschke from 5 + male subjects) scaling has been used to make the strength of the subject + consistent. Scaling coefficients for the lower body, shoulders and + elbow, and forearm/wrist using measurements that overlapped between + datasets. Presently this data set includes curves for 22 joint and + direction specific torque muscles. + + - Number of subjects: 1 elite gymnast (69.6 kg, 1.732 m) + - Gender: male + - Age: 21 years old + - Joint and Directions available + -# Ankle: flexion/extension (scaled from Anderson) + -# Knee: flexion/extension (from Jackson) + -# Hip: flexion/extension (from Jackson) + -# Lumbar: active extension curves (\f$\mathbf{t}_A\f$ and \f$\mathbf{t}_P\f$) from Raschke et al. passive extension from Dolan et al. + -# Lumbar: active flexion \f$\tau_{ISO}\f$ from Beimborn et al. + -# Shoulder: flexion/extension (from Jackson) + -# Shoulder: horizontal adduction/abduction (from Kentel, scaled to Jackson's subject) + -# Shoulder: internal rotation/external rotation (from Kentel, scaled to Jackson's subject) + -# Elbow: flexion/extension (from Kentel, scaled to Jackson's subject) + -# Wrist: pronation/supination (from Kentel, scaled to Jackson's subject) + -# Wrist: extension/flextion (from Jackson) + -# Wrist: ulnar/radial deviation (from Kentel, scaled to Jackson's subject) + - Missing Joint and directions + -# Ankle inversion/eversion + -# Hip adduction/abduction + -# Hip internal rotation/external rotation + -# Lumbar extension/flexion + -# Lumbar bending + -# Lumbar twisting + -# Shoulder Adduction + -# Shoulder Abduction + -# Scapular elevation/depression + -# Scapular adduction/abduction + -# Scapular upward/downward rotation + + In all cases the curves have been fitted to Bezier curves that + are constructed using functions in TorqueMuscleFunctionFactory. + + Notes + -# Angles are plotted using units of degrees for readability. The actual curves are described in units of radians + -# Hip and Knee characteristics taken from Jackson. Ankle extension is from Anderson et al., scaled using Jackson-to-Anderson hip/knee strength ratios from Jackson ratios + -# Shoulder horizontal adduction/abduction and internal/external rotation is a scaled version of the Kentel. Strength was scaled using the Jackson-to-Kentel shoulder flex/ext ratios. + -# Elbow extension/flexion and forearm pronation/supination. Elbow strength scaled from Kentel using the ratio of maximum isometric shoulder ext/flextion between Kentel and Jackson. Forearm pronation/supination scaled using the maximum torque strength ratio of wrist extension/flextion between Kentel and Jackson + -# Wrist ext/flexion directly from Jackson, while the curves for ulnar and radial deviation have been scaled (using the maximum isometric torque ratios of wrist extension and flexion from both models) from Kentel et al. + -# Lumbar-extension active-torque-angle-curve, and torque-velocity-curve in extension comes from Raschke et al. + -# Lumbar-flexion \f$\tau_{ISO}\f$ comes from Beimborn et al.'s observation that the strength ratio of back extensors to flextors at a flextion angle of zero is most often repored as 1.3:1. The torque velocity curve is a guess (slower than back extesors due to the larger moment arm). + -# Any passive curve that is not accompanied by a curve from the literature (see the plots for details) is an educated guess. + + + \image html fig_MuscleAddon_Gymnast_HipKneeAnkle.png " Hip/Knee/Ankle: from Jackson and Anderson et al. " + \image html fig_MuscleAddon_Gymnast_Lumbar.png " Lumbar Extension/Flexion: from Dolan et al.,Raschke et al., and Beimborn et al." + \image html fig_MuscleAddon_Gymnast_Shoulder3Dof.png " Shoulder 3 DoF torques: from Jackson and Kentel et al. " + \image html fig_MuscleAddon_Gymnast_ElbowForearm.png " Elbow flexion/extension: from Kentel et al." + \image html fig_MuscleAddon_Gymnast_Wrist3Dof.png " Wrist 3 DoF torques: from Jackson and Kentel et al." + + + Parameterized Curves used here vs. Literature + + The curves used in this implementation are 2nd order 2-dimensional + Bezier curves. The curves described in Anderson et al., Jackson, + Kentel were not directly used because they are not continuous to the + second derivative (a requirement for most gradient based optimization + routines). There are some other detailed differences that might be + of interest: + + -# Anderson et al.'s torque-velocity curve tends to large + negative values for fast eccentric contractions. This is + in contrast to the literature which says that at large + eccentric contractions the torque-velocity curve (or the + force-velocity-curve) tends to a value between 1.0 and 1.4. + -# Anderson et al.'s torque-velcity curve for ankle extension + did not cross the x-axis on the concentric side of the curve. + This would endow the plantar flexors with super-human abilities. + This error has been corrected by fitting a Bezier curve to a + Hill-type curve that passes through the point where + \f$\dot{\theta}= \frac{1}{2} \dot{\theta}_{MAX}\f$ + -# Anderson et al.'s, Jackson, and Kentel et al. had discintinuities + in the first derivative of the force velocity curve at + \f$\dot{\theta}=0\f$. While this follows Huxley's famous observations + that the slope does discontinuously change at at \f$\dot{\theta}=0\f$. + This is not a phenomena that is not compatible with most optimal + control formulations and thus this discontinuity is not present in + the force velocity curves used in this model. + -# Anderson et al. and Kentel et al.'s active-torque-angle curves + can achieve negative values - this is obviously undesirable as it + will allow a muscle to push. + -# Kentel et al.'s activation inhibiition function does not always + cross 1.0 for \f$\dot{\theta}=0\f$, which means that \f$\tau_{ISO}\f$ + is not reached. This makes for a confusing model to use. + + + Coordinate Mapping + + Every author chose a particular convention for measuring + the angles of the hip, knee, ankle joint, shoulder, elbow, wrist and + lumbar --- see the figure for details. These conventions have all + been mapped to the one used in the illustrations. You will need to + use the figure, your model, and the constructors appropriately + so that + + -# the joint angle of your model is correctly mapped to the + fiber angle of the Millard2016TorqueMuscle; + -# the sign of the muscle's output torque matches the + sign associated with your model. + + To map from your model's joint coordinates to the joint coordines + used in this model (see the figure in the description) + the followinq equation is used at the torque level + + \f$ jointTorque = signOfJointTorque*fiberTorque \f$ + + where fiberTorque is the torque produced by Anderson et al.'s curves, + which is always positive. At the position level, the angles from your + models joint angle to Anderson et al.'s joint angle (called fiberAngle) + are mapped using + + \f$ fiberAngle = signOfJointAngleRelativeToAnderson2007*(jointAngle-jointAngleOffsetRelativeToAnderson2007). \f$ + + Internally the sign of the fiber velocity follows signOfJointTorque so + that the signs of joint power and muscle power are consistent. + + Strength Scaling + + The leg strength (here we mean \f$\tau_{ISO}\f$) + predicted by Anderson et al.'s curves should be taken + as a good first approximation. While Anderson et al.'s data set is + the most comprehensive in the literature, they only measured torques + from active people: they did not include people at the extremes + (both very weak, and very strong), nor did they include children. + Finally, the torques produced by each subject were normalized by + subjectMassInKg*subjectHeightInM*accelerationDueToGravity. Strength + is a strange phenomena which is not nicely normalized by just these + quantites, and so the strength predicted by Anderson et al.'s curves + might not fit your subject even if they are represented in Anderson + et al.'s data set. + + The strength used in the Gymnast data set is fitted to an elite + male gymnast. It goes without saying that an elite gymnast has + strength proportions, and an absolute strength that are not typical. + In the future it would be nice to have a function that could provide + an educated guess about how to map Gymnast's strengths to that of + another subject. For the moment I have no idea how to do this, nor + am I aware of any works in the literature that can provide insight + of how to do this. For now the whole-body Gymnast model should be + viewed as being a representation of what is possible for a human, + but not a typical human. At the present time the default strength + settings of the Gymnast are not scaled by subject height, nor + weight. + + If you happen to know the maximum-isometric-active-torque (note this + does not include the passive component) that your subject can + produce,you can update the strength of the torque-muscle using the + functions getMaximumActiveIsometricTorque(), and + setMaximumActiveIsometricTorque(). + + Limitations + + This rigid-tendon torque muscle has some limitations that you should + be aware of: + + -# There are no elastic tendons. That means that the mapping between + the mechanical work done by this torque actuator will greatly differ + from the positive mechanical work done by a torque actuator that + includes an elastic tendon. This difference is greatest for those + muscles with long tendons - namely the Achilles tendon. If you are + interested in fiber kinematics, fiber work, or metabolic energy + consumption you cannot use this model especially for muscles that + have long tendons. + -# This model formulation predicts torque well, but does a poor job + of predicting joint stiffness. In this model stiffness is given + by the partial derivative of torque w.r.t. joint angle. Since the + active-torque-angle curve fits a cosine function, it is possible + to construct a torque muscle that has a region of physically + impossible negative stiffness. Real muscle, in constrast, always + has a positive stiffness even on the descending limb of the + active-torque-angle curve (see Rassier et al. for details). + -# Muscles that cross 2 joints (e.g. the hamstrings) produce coupled + torques at both of those joints. In this model there is no coupling + between joints. Furthermore, because of the lack of coupling the + curves used here are only valid for the posture that Anderson et al., + Jackson, and Kentel et al. + used when they made their data collection. If you are interested + in simulating postures that are very different from those described + in by these authors then the results produced by this model should + be treated as very rough. + -# Because this is a joint-torque muscle, none of the joint contact + forces predicted will come close to matching what is produced by + line-type muscles. If you are interested in joint-contact forces you + cannot use this model. + + This simple model is a fast approximate means to constrain the joint + torque developed in the body to something that is physiologically + possible. That is it. + + Units + Although the figure in this description has angles in units + of degrees, this is only to help intuitition: when using + the model, use radians. This model uses MKS: + + -Distance: m + -Angles: radians + -Angular velocity: radians/s + -Mass: kg + -Torque: Nm + -Time: second + -Power: Nm/second + + + References + + -# Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). + Maximum voluntary joint torque as a function of joint angle + and angular velocity: model development and application to + the lower limb. Journal of biomechanics, 40(14), 3105-3113. + + -# Beimborn, D. S., & Morrissey, M. C. (1988). A review of the + literature related to trunk muscle performance. Spine, 13(6), 655-660. + + + -# Dolan, P., A. F. Mannion, and M. A. Adams. Passive tissues help + the back muscles to generate extensor moments during lifting. + Journal of Biomechanics 27, no. 8 (1994): 1077-1085. + + + -# Jackson, M.I. (2010). The mechanics of the Table Contact + Phase of Gymnastics Vaulting. Doctoral Thesis, Loughborough + University. + + -# Kentel, B.B., King, M.A., & Mitchell, S.R. (2011). + Evaluation of a subject-specific torque-driven computer simulation + model of one-handed tennis backhand ground strokes. Journal of + Applied Biomechanics, 27(4),345-354. + + -# Millard, M., Uchida, T., Seth, A., & Delp, S. L. (2013). + Flexing computational muscle: modeling and simulation of + musculotendon dynamics. Journal of biomechanical engineering, + 135(2), 021005. + + -# Raschke, U., & Chaffin, D. B. (1996). Support for a linear + length-tension relation of the torso extensor muscles: an + investigation of the length and velocity EMG-force relationships. + Journal of biomechanics, 29(12), 1597-1604. + + -# Rassier, D. E., Herzog, W., Wakeling, J., & Syme, D. A. (2003). + Stretch-induced, steady-state force enhancement in single + skeletal muscle fibers exceeds the isometric force at optimum + fiber length. Journal of biomechanics, 36(9), 1309-1316. + + + + */ + class Millard2016TorqueMuscle { + + public: + /** + Default constructor, which for the moment does nothing. + Calling any of the models functions after the default + construction will result in a runtime error. + */ + Millard2016TorqueMuscle(); + + + /** + This constructor allows you to easily access the large + table of built-in torque muscle coefficients to create + a torque muscle that best represents the joint of interest. + + Note: directions + This constructs a single joint-torque muscle: + it can only generate torque in one direction. If you want + to generate a torque in two directions, you need 2 torque + muscles. + + Note: signs and offsets + + All of the angles in these models are defined anatomically. + You will need to set a series of variables to correctly + map from your model's joint coordinates and sign conventions + to that of the models: + jointAngleOffsetRelativeToDoxygenFigures, + signOfJointAngleRelativeToDoxygenFigures, + signOfJointTorqueToDoxygenFigures. Also note that due to + the anatomical angle definitions some left and right handed + joints will require different signs. This will be true + for internal/external rotation at the shoulder, + horizontal adduction/abduction at the shoulder, ulnar/radial + deviation at the wrist, pronation/supination of the wrist, + and others as the list of directions grows. + + + @param dataSet + The desired source of joint torque + coefficients. Use the DataSet structure to choose + the desired data set (e.g. DataSet::Anderson2007, + or DataSet::Gymnast) + + @param subjectInfo + A struct that contains metadata about the subject + which is used to scale the maximum torque of the + torque muscle. + + @param jointTorque + Select the joint and torque direction of interest. + Use the struct for each data set to choose a + joint-torque-direction that is in the set (e.g. + Anderson2007::HipExtension, or + Gymnast::ShoulderHorizontalAdduction) + + @param jointAngleOffsetRelativeToDoxygenFigures + Offset angle between your model's joints and the + reference figures in class description. + + @param signOfJointAngleRelativeToDoxygenFigures + The sign convention that converts your model's joint + angles to the angles used in the reference figures. + + @param signOfJointTorqueToDoxygenFigures + The sign that maps fiberTorque from Anderson's model + (which is always positive) to the correctly signed + joint torque for your model. + + @param name + The name of the muscle. This is needed to do useful + things like provide error messages that are human + readable. + + @throws abort() when + -# The combination of dataSet, gender, joint, and jointDirection does not correspond to a valid entry + -# subjectHeightInMeters <= 0 + -# subjectMassInKg <= 0 + -# abs(signOfJointTorque)-1 > epsilon + + */ + Millard2016TorqueMuscle( + DataSet::item dataSet, + const SubjectInformation &subjectInfo, + int jointTorque, + double jointAngleOffsetRelativeToDoxygenFigures, + double signOfJointAngleRelativeToDoxygenFigures, + double signOfJointTorqueToDoxygenFigures, + const std::string& name + ); + + + /** + Calculates the joint torque developed by the + muscle. + + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param activation: the percentage of the muscle that is + turned on [0-1]. This function allows activations to be + outside [0,1], because this is useful during the + intermediate solutions of an optimization run. However, + you must ensure after the fact that your activations + fall within a bound of [0,1]. + + @returns torque developed by the musce in (Nm). + */ + double calcJointTorque( + double jointAngle, + double jointAngularVelocity, + double activation) const; + + + /** + Calculates a large number of internal quantities of the + torque muscle ranging from the values of the muscle's + components, the stiffness of the muscle, and its power output. + + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param activation: the percentage of the muscle that is + turned on [0-1]. This function allows activations to be + outside [0,1], because this is useful during the + intermediate solutions of an optimization run. However, + you must ensure after the fact that your activations + fall within a bound of [0,1]. + + @param torqueMuscleInfoStruct: A torque muscle struct + */ + void calcTorqueMuscleInfo( + double jointAngle, + double jointAngularVelocity, + double activation, + TorqueMuscleInfo& torqueMuscleInfoStruct) const; + + /** + @return the sign of the joint torque (+/- 1) + */ + double getJointTorqueSign() const; + + /** + @return the sign of the angle sign relative to the + figures in the class description (+/- 1) + */ + double getJointAngleSign() const; + + /** + @return the offset angle between the model's joint + and the figures in the class description (rad) + */ + double getJointAngleOffset() const; + + + /** + @return the maximum-active-isometric torque that this muscle + can produce in Nm. + */ + double getMaximumActiveIsometricTorque() const; + + /** + @return the joint angle at which the normalized + active-torque-angle curve peaks at its + maximum value of 1.0. Angle is in radians + */ + double getJointAngleAtMaximumActiveIsometricTorque() const; + + /** + @return the joint angle at which the normalized + passive-torque-angle curve reaches a value + of 1.0. If this curve never reaches a value + of 1.0 (because it is flat, or the + passiveTorqueScale has been set to 0) a value + of std::numeric_limits::signaling_NaN() + is returned. Use the std function isfinite to + test if a signaling_NaN has been returned. + Angle is in radians + */ + double getJointAngleAtOneNormalizedPassiveIsometricTorque() const; + + /** + @return the maximum concentric joint angular velocity + in radians. + */ + double getMaximumJointAngularVelocity() const; + + /** + @return the passive-torque-scale \f$s_P\f$ that is applied + to the passive-torque-curve. + */ + double getPassiveTorqueScale() const; + + /** + @return the angle \f$\theta_S\f$ that the passive curve has + been shifted (radians). + */ + double getPassiveCurveAngleOffset() const; + + /** + @return The normalized damping term \f$\beta\f$ term + in the torque muscle model. See class description + for details. + */ + double getNormalizedDampingCoefficient() const; + + /** + @param beta -t normalized damping term \f$\beta\f$ term + in the torque muscle model. See class description + for details. + @throw abort() if beta < 0 + */ + void setNormalizedDampingCoefficient(double beta); + /** + Sets the scaling of the passive-joint-torques. By default + this scale + is one. + + @param passiveTorqueScale + The scale \f$s_P\f$ applied to the + passive-joint-torque curve (unitless) + */ + void setPassiveTorqueScale(double passiveTorqueScale); + + /** + @param passiveCurveAngleOffsetVal the angle \f$\theta_S\f$ + that the passive curve should be shifted. Angles in radians + */ + void setPassiveCurveAngleOffset( + double passiveCurveAngleOffsetVal); + + + /** + This function iteratively solves for the passiveTorqueScale + so that at the specified jointAngle the passive curve + develops the specified passiveTorque + + @param jointAngle the target joint angle in radians + @param passiveTorque the target passive joint torque in Nm. + @throws abort if jointAngle is not in the domain of the curve + @throws abort if passiveTorque < sqrt(eps) + */ + void fitPassiveTorqueScale(double jointAngle, + double passiveTorque); + + /** + This function solves for the passive curve angle offset + so that at the specified jointAngle the passive curve + develops the specified passiveTorque + + @param jointAngle the target joint angle in radians + @param passiveTorque the target passive joint torque in Nm. + @throws abort if passiveTorque < sqrt(eps) + */ + void fitPassiveCurveAngleOffset(double jointAngle, + double passiveTorque); + + /** + Sets the strength of the muscle to match a desired value. + + @param maxIsometricTorque + The desired maximum-active-isometric torque of the + muscle (Nm) + + */ + void setMaximumActiveIsometricTorque( + double maxIsometricTorque); + + /** + @return the SmoothSegmentedFunction the has been fitted to + Anderson et al.'s passive torque angle curve. + */ + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getActiveTorqueAngleCurve() const; + + /** + @return the SmoothSegmentedFunction the has been fitted to + Anderson et al.'s active torque angle curve. + */ + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getPassiveTorqueAngleCurve() const; + + /** + @return the SmoothSegmentedFunction the has been fitted to + Anderson et al.'s torque velocity curve. + */ + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getTorqueAngularVelocityCurve() const; + + + /** + Prints 2 csv files: + -# 'fileName' + '_variableLengthfixedVelocity': All of the + fields in TorqueMuscleInfo are recorded to file as the + jointAngle varies but the jointAngularVelocity is zero. + -#'fileName' + '_fixedLengthVariableVelocity': All of the fields + in TorqueMuscleInfo are recorded to file as the jointAngle is + fixed but the jointAngularVelocity varies. + + Each column has a header, so that you can tell what each + piece of data means. + + @param path: the path to the destination folder. Don't put + an '\' on the end. + @param fileNameWithoutExtension: the name of the file, but + without an extension. + @param numberOfSamplePoints: the number of sample points to + use in the files. + */ + void printJointTorqueProfileToFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + int numberOfSamplePoints); + + std::string getName(); + void setName(std::string& name); + + private: + /** + @return the parameters c1,...,c6 that desribe the + active-torque-angle and torque-velocity curves of this + torque muscle model. See the Anderson et al. paper metioned + in the class description for detail. + */ + //const RigidBodyDynamics::Math::VectorNd& + //getParametersC1C2C3C4C5C6(); + + /** + @return the parameters b1,k1,b2,k2 that desribe the + passive-torque-angle curves of this model. See the Anderson + et al. paper metioned in the class description for detail. + */ + //const RigidBodyDynamics::Math::VectorNd& + //getParametersB1K1B2K2(); + + bool muscleCurvesAreDirty; + void updateTorqueMuscleCurves(); + TorqueMuscleInfo tmInfo; + + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction taCurve; + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction tpCurve; + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction tvCurve; + + RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6Anderson2007; + RigidBodyDynamics::Math::VectorNd b1k1b2k2Anderson2007; + RigidBodyDynamics::Math::VectorNd gymnastParams; + + DataSet::item dataSet; + + double maxActiveIsometricTorque; + double angleAtOneNormActiveTorque; + double omegaMax; + double angleAtOneNormPassiveTorque; + double passiveTorqueScale; + double passiveCurveAngleOffset; + + double beta; //passive damping coefficient + + double subjectHeightInMeters; + double subjectMassInKg; + double scaleFactorAnderson2007; + + double signOfJointAngle; + double signOfConcentricAnglularVelocity; + double signOfJointTorque; + double angleOffset; + + std::string muscleName; + + double calcJointAngle(double fiberAngle) const; + double calcFiberAngle(double jointAngle) const; + double calcFiberAngularVelocity( + double jointAngularVelocity) const; + + + + //const static RigidBodyDynamics::Math::MatrixNd& + //getAnderson2007ParameterMatrix(); + static double const Anderson2007Table3Mean[36][14]; + static double const Anderson2007Table3Std[36][14]; + static double const GymnastWholeBody[22][12]; + + }; + + + +} +} +} + +#endif diff --git a/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc new file mode 100644 index 0000000..8c09b36 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc @@ -0,0 +1,927 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + /* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= + +#include "MuscleFunctionFactory.h" +#include +#include +#include +#include +#include + +using namespace std; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +//============================================================================= +// STATICS +//============================================================================= +//using namespace std; + + +static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample + //each Bezier corner section + +static double SMOOTHING = 0; //The amount of smoothing to use when fitting + //3rd order splines to the quintic Bezier + //functions +static bool DEBUG = true; //When this is set to true, each function's debug + //routine will be called, which ususally results + //in a text file of its output being produced + +static double UTOL = (double)std::numeric_limits::epsilon()*1e2; + +static double INTTOL = (double)std::numeric_limits::epsilon()*1e4; + +static int MAXITER = 20; +//============================================================================= +// UTILITY FUNCTIONS +//============================================================================= + +//============================================================================= +// MUSCLE CURVE FITTING FUNCTIONS +//============================================================================= +void MuscleFunctionFactory::createFiberActiveForceLengthCurve( + double x0, + double x1, + double x2, + double x3, + double ylow, + double dydx, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + double rootEPS = sqrt(std::numeric_limits::epsilon()); + + if( (!(x0>=0 && x1>x0+rootEPS && x2>x1+rootEPS && x3>x2+rootEPS) ) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve: " + << curveName + << ": This must be true: 0 < lce0 < lce1 < lce2 < lce3" + << endl; + assert(0); + abort(); + + } + + + if( !(ylow >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + <<": shoulderVal must be greater than, or equal to 0" + << endl; + assert(0); + abort(); + + } + + double dydxUpperBound = (1-ylow)/(x2-x1); + + + if( !(dydx >= 0 && dydx < dydxUpperBound) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": plateauSlope must be greater than 0 and less than " + << dydxUpperBound + << endl; + assert(0); + abort(); + } + + if( !(curviness >= 0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": curviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberActiveForceLengthCurve"); + + + + //Translate the users parameters into Bezier curves + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //The active force length curve is made up of 5 elbow shaped sections. + //Compute the locations of the joining point of each elbow section. + + //Calculate the location of the shoulder + double xDelta = 0.05*x2; //half the width of the sarcomere 0.0259, + //but TM.Winter's data has a wider shoulder than + //this + + double xs = (x2-xDelta);//x1 + 0.75*(x2-x1); + + //Calculate the intermediate points located on the ascending limb + double y0 = 0; + double dydx0 = 0; + + double y1 = 1 - dydx*(xs-x1); + double dydx01= 1.25*(y1-y0)/(x1-x0);//(y1-y0)/(x1-(x0+xDelta)); + + double x01 = x0 + 0.5*(x1-x0); //x0 + xDelta + 0.5*(x1-(x0+xDelta)); + double y01 = y0 + 0.5*(y1-y0); + + //Calculate the intermediate points of the shallow ascending plateau + double x1s = x1 + 0.5*(xs-x1); + double y1s = y1 + 0.5*(1-y1); + double dydx1s= dydx; + + //double dydx01c0 = 0.5*(y1s-y01)/(x1s-x01) + 0.5*(y01-y0)/(x01-x0); + //double dydx01c1 = 2*( (y1-y0)/(x1-x0)); + //double dydx01(1-c)*dydx01c0 + c*dydx01c1; + + //x2 entered + double y2 = 1; + double dydx2 = 0; + + //Descending limb + //x3 entered + double y3 = 0; + double dydx3 = 0; + + double x23 = (x2+xDelta) + 0.5*(x3-(x2+xDelta)); //x2 + 0.5*(x3-x2); + double y23 = y2 + 0.5*(y3-y2); + + //double dydx23c0 = 0.5*((y23-y2)/(x23-x2)) + 0.5*((y3-y23)/(x3-x23)); + //double dydx23c1 = 2*(y3-y2)/(x3-x2); + double dydx23 = (y3-y2)/((x3-xDelta)-(x2+xDelta)); + //(1-c)*dydx23c0 + c*dydx23c1; + + //Compute the locations of the control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,ylow,dydx0,x01,y01,dydx01,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x01,y01,dydx01,x1s,y1s,dydx1s,c); + RigidBodyDynamics::Math::MatrixNd p2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1s,y1s,dydx1s,x2, y2, dydx2,c); + RigidBodyDynamics::Math::MatrixNd p3 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2, y2, dydx2,x23,y23,dydx23,c); + RigidBodyDynamics::Math::MatrixNd p4 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x23,y23,dydx23,x3,ylow,dydx3,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,5), mY(6,5); + mX.col(0) = p0.col(0); + mX.col(1) = p1.col(0); + mX.col(2) = p2.col(0); + mX.col(3) = p3.col(0); + mX.col(4) = p4.col(0); + + mY.col(0) = p0.col(1); + mY.col(1) = p1.col(1); + mY.col(2) = p2.col(1); + mY.col(3) = p3.col(1); + mY.col(4) = p4.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x3,ylow,ylow,0,0,curveName); +} + +void MuscleFunctionFactory::createFiberForceVelocityCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + + if( !(fmaxE > 1.0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": fmaxE must be greater than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxC >= 0.0 && dydxC < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxC must be greater than or equal to 0 " + <<" and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxNearC > dydxC && dydxNearC <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxNearC must be greater than or equal to 0 " + << "and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxIso > 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxIso must be greater than (fmaxE-1)/1 (" + << ((fmaxE-1.0)/1.0) + << ")" + << endl; + assert(0); + abort(); + } + + if( !(dydxE >= 0.0 && dydxE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": dydxE must be greater than or equal to 0 " + << "and less than fmaxE-1 (" + << (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(!(dydxNearE >= dydxE && dydxNearE < (fmaxE-1))){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve" + << curveName + << ": dydxNearE must be greater than or equal to dydxE " + << "and less than fmaxE-1 (" << (fmaxE-1) + << ")" + << endl; + assert(0); + abort(); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberForceVelocityCurve"); + + //Translate the users parameters into Bezier point locations + double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); + double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); + + //Compute the concentric control point locations + double xC = -1; + double yC = 0; + + double xNearC = -0.9; + double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); + + double xIso = 0; + double yIso = 1; + + double xE = 1; + double yE = fmaxE; + + double xNearE = 0.9; + double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE); + + + RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); + RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); + RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); + RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); + + RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); + mX.col(0) = concPts1.col(0); + mX.col(1) = concPts2.col(0); + mX.col(2) = eccPts1.col(0); + mX.col(3) = eccPts2.col(0); + + mY.col(0) = concPts1.col(1); + mY.col(1) = concPts2.col(1); + mY.col(2) = eccPts1.col(1); + mY.col(3) = eccPts2.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,xC,xE,yC,yE,dydxC,dydxE,curveName); +} + + +void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + if(! (fmaxE > 1.0 )){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve: " + << curveName + << ": fmaxE must be greater than 1" + << endl; + assert(0); + abort(); + } + + double SimTKSignificantReal = + pow((double)std::numeric_limits::epsilon(), 7.0/8.0); + + if(! (dydxC > SimTKSignificantReal && dydxC < 1 )){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxC must be greater than 0" + << "and less than 1" + << endl; + assert(0); + abort(); + + } + + if(! (dydxNearC > dydxC && dydxNearC < 1 )){ + std::stringstream errMsg; + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << ": dydxNearC must be greater than 0 " + << curveName + << " and less than 1" + << endl; + assert(0); + abort(); + } + + if(! (dydxIso > 1)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxIso must be greater than or equal to 1" + << endl; + assert(0); + abort(); + } + + //double SimTKSignificantReal = + // pow(std::numeric_limits::epsilon(), 7.0/8.0); + + if(! (dydxE > SimTKSignificantReal && dydxE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxE must be greater than or equal to 0" + << " and less than fmaxE-1 (" << (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(! (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxNearE must be greater than or equal to dydxE" + << "and less than fmaxE-1 ("<< (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberForceVelocityInverseCurve"); + + //Translate the users parameters into Bezier point locations + double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); + double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); + + //Compute the concentric control point locations + double xC = -1; + double yC = 0; + + double xNearC = -0.9; + double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); + + double xIso = 0; + double yIso = 1; + + double xE = 1; + double yE = fmaxE; + + double xNearE = 0.9; + double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE); + + + RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); + RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); + RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); + RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); + + RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); + mX.col(0) = concPts1.col(0); + mX.col(1) = concPts2.col(0); + mX.col(2) = eccPts1.col(0); + mX.col(3) = eccPts2.col(0); + + mY.col(0) = concPts1.col(1); + mY.col(1) = concPts2.col(1); + mY.col(2) = eccPts1.col(1); + mY.col(3) = eccPts2.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mY,mX,yC,yE,xC,xE,1/dydxC,1/dydxE, curveName); + +} + +void MuscleFunctionFactory::createFiberCompressiveForcePennationCurve( + double phi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(phi0>0 && phi0<(M_PI/2.0)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": phi0 must be greater than 0, and less than Pi/2" + << endl; + assert(0); + abort(); + } + + if( !(k > (1.0/(M_PI/2.0-phi0))) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": k must be greater than " << (1.0/(M_PI/2.0-phi0)) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string name=curveName; + name.append(".createFiberCompressiveForcePennationCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = phi0; + double y0 = 0; + double dydx0 = 0; + double x1 = M_PI/2.0; + double y1 = 1; + double dydx1 = k; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + +void MuscleFunctionFactory:: + createFiberCompressiveForceCosPennationCurve( + double cosPhi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(cosPhi0>0 && cosPhi0 < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": cosPhi0 must be greater than 0, and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(k < 1/cosPhi0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": k must be less than 0" + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve" + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string name=curveName; + name.append(".createFiberCompressiveForceCosPennationCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0; + double y0 = 1; + double dydx0 = k; + double x1 = cosPhi0; + double y1 = 0; + double dydx1 = 0; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + +void MuscleFunctionFactory::createFiberCompressiveForceLengthCurve( + double lmax, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(lmax>0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": l0 must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(k < -(1.0/lmax)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": k must be less than " + << -(1.0/lmax) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string caller = curveName; + caller.append(".createFiberCompressiveForceLength"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0.0; + double y0 = 1; + double dydx0 = k; + double x1 = lmax; + double y1 = 0; + double dydx1 = 0; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + + +void MuscleFunctionFactory::createFiberForceLengthCurve( + double eZero, + double eIso, + double kLow, + double kIso, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(eIso > eZero) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": The following must hold: eIso > eZero" + << endl; + assert(0); + abort(); + } + + if( !(kIso > (1.0/(eIso-eZero))) ){ + std::stringstream errMsg; + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kiso must be greater than 1/(eIso-eZero) (" + << (1.0/(eIso-eZero)) << ")" + << endl; + assert(0); + abort(); + } + + if( !(kLow > 0.0 && kLow < 1/(eIso-eZero)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kLow must be greater than 0 and less than" + << 1.0/(eIso-eZero) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string callerName = curveName; + callerName.append(".createFiberForceLength"); + + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double xZero = 1+eZero; + double yZero = 0; + + double xIso = 1 + eIso; + double yIso = 1; + + double deltaX = std::min(0.1*(1.0/kIso), 0.1*(xIso-xZero)); + + double xLow = xZero + deltaX; + double xfoot = xZero + 0.5*(xLow-xZero); + double yfoot = 0; + double yLow = yfoot + kLow*(xLow-xfoot); + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xZero, yZero, 0, + xLow, yLow, kLow,c); + + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xLow, yLow, kLow, + xIso, yIso, kIso, c); + RigidBodyDynamics::Math::MatrixNd mX(6,2); + RigidBodyDynamics::Math::MatrixNd mY(6,2); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, xZero, xIso, yZero, yIso, 0.0, kIso, curveName); +} + + + + +void MuscleFunctionFactory:: + createTendonForceLengthCurve( double eIso, double kIso, + double fToe, double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(eIso>0) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": eIso must be greater than 0, but " + << eIso << " was entered" + << endl; + assert(0); + abort(); + + } + + if( !(fToe>0 && fToe < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": fToe must be greater than 0 and less than 1, but " + << fToe + << " was entered" + << endl; + assert(0); + abort(); + } + + if( !(kIso > (1/eIso)) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": kIso must be greater than 1/eIso, (" + << (1/eIso) << "), but kIso (" + << kIso << ") was entered" + << endl; + assert(0); + abort(); + } + + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << " : curviness must be between 0.0 and 1.0, but " + << curviness << " was entered" + << endl; + assert(0); + abort(); + } + + std::string callerName = curveName; + callerName.append(".createTendonForceLengthCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 1.0; + double y0 = 0; + double dydx0 = 0; + + double xIso = 1.0 + eIso; + double yIso = 1; + double dydxIso = kIso; + + //Location where the curved section becomes linear + double yToe = fToe; + double xToe = (yToe-1)/kIso + xIso; + + + //To limit the 2nd derivative of the toe region the line it tends to + //has to intersect the x axis to the right of the origin + double xFoot = 1.0+(xToe-1.0)/10.0; + double yFoot = 0; + double dydxToe = (yToe-yFoot)/(xToe-xFoot); + + //Compute the location of the corner formed by the average slope of the + //toe and the slope of the linear section + double yToeMid = yToe*0.5; + double xToeMid = (yToeMid-yIso)/kIso + xIso; + double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot); + + //Compute the location of the control point to the left of the corner + double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); + double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot); + + + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + xToeCtrl,yToeCtrl,dydxToeMid,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); + RigidBodyDynamics::Math::MatrixNd mX(6,2); + RigidBodyDynamics::Math::MatrixNd mY(6,2); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, x0, xToe, y0, yToe, dydx0, dydxIso, curveName); + +} diff --git a/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h new file mode 100644 index 0000000..ece6344 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h @@ -0,0 +1,832 @@ +#ifndef MUSCLEFUNCTIONFACTORY_H_ +#define MUSCLEFUNCTIONFACTORY_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunctionFactory.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +#include "../geometry/SmoothSegmentedFunction.h" +#include "../geometry/SegmentedQuinticBezierToolkit.h" + +#include +#include +#include +#include + + + +/** +This is a class that acts as a user friendly wrapper to QuinticBezerCurveSet +to build specific kinds of physiologically plausible muscle curves using C2 +continuous sets of quintic Bezier curves. This class has been written there did +not exist a set of curves describing muscle characteristics that was: + +1. Physiologically Accurate +2. Continuous to the second derivative +3. Parameterized in a physically meaningful manner + +For example, the curves employed by Thelen (Thelen DG(2003). Adjustment of +Muscle Mechanics Model Parameters to Simulate Dynamic Contractions in Older +Adults. ASME Journal of Biomechanical Engineering (125).) are parameterized in a +physically meaningful manner, making them easy to use. However there are +many shortcomings of these curves: + +1. The tendon and parallel element are not C2 continuous, making them slow to + simulate and likely not physiologically accurate. +2. The active force length curve approaches does not achieve its minimum value + at a normalized fiber length of 0.5, and 1.5. +3. The force velocity curve is not C2 continuous at the origin. As it is + written in the paper the curve is impossible to use with an equilibrium model + because it is not invertible. In addition the force-velocity curve actually + increases in stiffness as activation drops - a very undesirable property given + that many muscles are inactive at any one time. + +The muscle curves used in the literature until 2012 have been hugely influenced +by Thelen's work, and thus similar comments can easily be applied to just about +every other musculoskeletal simulation. + +Another example is from Miller (Miller,RH(2011).Optimal Control of +Human Running. PhD Thesis). On pg 149 a physiolgically plausible force velocity +curve is specified that gives the user the ability to change the concentric +curvature to be consistent with a slow or a fast twitch musle. This curve is +not C2 continuous at the origin, but even worse, it contains singularities in +its parameter space. Since these parameters do not have a physical +interpretation this model is difficult to use without accidentically creating a +curve with a singularity. + +With this motivation I set out to develop a class that could generate muscle +characteristic curves with the following properties: + +1. Physiologically Accurate +2. Continuous to the second derivative +3. Parameterized in a physically meaningful manner +4. Monotonicity for monotonic curves +5. Computationally efficient + +These goals were surprisingly time consuming achieve, but these goals have been +achieved using sets of C2 continuous quintic Bezier curves. The resulting +muscle curve functions in this class take parameters that would be intuitive to +biomechanists who simulate human musculoskeletal systems, and returns a +SmoothSegmentedFunction which is capable of evaluating the value, and +derivatives of the desired function (or actually relation as +the case may be). + +Each curve is made up of one or more C2 quintic Bezier curves x(u), +and y(u), with linearily extrapolated ends as shown in the figure below. These +quintic curves span 2 points, and achieve the desired derivative at its end +points. The degree of curviness can be varied from 0 to 1 (0, 0.75 and 1.0 are +shown in the figure in grey, blue and black respectively), and will make the +curve approximate a line when set to 0 (grey), and approximate a curve that +hugs the intersection of the lines that are defined by the end points locations +and the slopes at the end of each curve segment (red lines). Although you do +not need to set all of this information directly, for some of the curves it is +useful to know that both the slope and the curviness parameter may need to be +altered to achieve the desired shape. + + +\image html fig_GeometryAddon_quinticCornerSections.png + + + +Computational Cost Details +All computational costs assume the following operation costs: + +\verbatim +Operation Type : #flops +*,+,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 +\endverbatim + +These relative weightings will vary processor to processor, and so any of +the quoted computational costs are approximate. + + RBDL Port Notes +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + + + +@author Matt Millard +@version 0.0 + +*/ +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + +class MuscleFunctionFactory +{ + + + public: + + // friend class SmoothSegmentedFunction; + + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active force length curve. + + + @param lce0 Normalized fiber length at the left-most shoulder of the + active force-length curve. The value of the active force + length curve for lce < lce0 will be equal to the value + set in shoulderVal. Normally lce0 is approximately 0.5 + + @param lce1 Normalized fiber length at the transition point between + the ascending limb and the plateau region of the active + force length curve. + + @param lce2 Normalized fiber length at the maximum active force length + curve value of 1. Normally lce2 is by definition 1. + + @param lce3 Normalized fiber length of the at the right most shoulder + of the active-force length curve. The value of the active + force length curve for lce > lce2 will be equal to the + value of shoulderVal. Normally lce3 is approximately 1.5 + + @param minActiveForceLengthValue + The minimum value of the active force length + curve. A physiological non-equibrium muscle model + would have this value set to 0. An equilibrium + muscle model would have a non-zero lower bound on + this value of 0.1 typically. shoulderVal must be + greater than, or equal to 0. + + @param plateauSlope The slope of the plateau of the active force + length curve between lce1 and lce2. This parameter + can vary depending on the muscle model, but a + value of 0.8616 is a good place to start. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0 will yield an active + force length curve that is composed of slightly curved + line segments. A value of 1 will yield an active force + length curve that is smoothly rounded. + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberActiveForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + if these conditions aren't met + -0 < lce0 < lce1 < lce2 < lce3 + -shoulderVal >= 0 + -0 <= plateauSlope < (1/(lce3-lce2)) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_falCurve.png + + + Conditions: + + Computational Costs + \verbatim + ~20,500 flops + \endverbatim + + Example: + @code + double lce0 = 0.5; + double lce1 = 0.75; + double lce2 = 1; + double lce3 = 1.5; + double shoulderVal = 0.1; + double plateauSlope = 0.75; + double curviness = 0.9; + + SmoothSegmentedFunction fiberfalCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberActiveForceLengthCurve(lce0, lce1, lce2, lce3, + shoulderVal, plateauSlope, curviness,"test", fiberfalCurve); + fiberfalCurve.printCurveToFile(); + @endcode + + + */ + static void createFiberActiveForceLengthCurve( + double lce0, + double lce1, + double lce2, + double lce3, + double minActiveForceLengthValue, + double plateauSlope, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry + ::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continous (continuous to the second + derivative) force velocity curve of a single muscle fiber. The main + function of this element is to model the amount the force enhancement or + attenuation that is associated with contracting at a particular velocity. + + @param fmaxE The normalized maximum force the fiber can generate when + is being stretched. This value is reported to range + between 1.1 and 1.8 in the literature, though all values + are above 1. + + @param dydxC The slope of the fv(dlce(t)/dt) curve at the maximum + normalized concentric contraction velocity. Although + physiologically the value of dydxC at the maximum + concentric contracton velocity is by definition 0, a value + of 0 is often used. If you are using an equilbrium type + model this term must be positive and greater than zero so + that the fv curve can be inverted. +

+ Minimum Value: 0 + Maximum Value: dydxC < 1 +

+ + @param dydxNearC The slope of the force velocity curve as it approaches + the maximum concentric (shortening) contraction velocity. +

+ Minimum Value: > dydxC + Maximum Value: dydxNearC < 1 +

+ + + @param dydxIso The slope of the fv curve when dlce(t)/dt = 0. +

+ Minimim Value: dydxIso > 1.0 + Maximum Value: dydxIso < Inf + + @param dydxE The analogous term of dydxC parameter but for the + eccentric portion of the force-velocity curve. As with + the dydxC term, the physiologically accurate value for + this parameter is 0, though a value of 0 is rarely used + in muscle models. If you are using an equilbrium type + model this term must be positive and greater than zero + so that the fv curve can be inverted. +

+ Minimum Value: 0 + Maximum Value: dydxC < (fmaxE-1). +

+ As with the dydxC term, + the size of this term also affects the stiffness of the + integration problem for equilibrium-type muscle models: + the closer to zero this term is, the stiffer the model + will be (but only when (dlce(t)/dt)/vmax approaches 1. + + @param dydxNearE The slope of the force velocity curve as it approaches + the maximum eccentric (lengthening) contraction velocity. +

+ Minimum Value: > dydxE + Maximum Value: dydxNearE < (fmaxE-1) +

+ + + @param concCurviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). This parameter affects only + the concentric side of the fv curve. + + @param eccCurviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). This parameter affects only + the eccentric side of the fv curve. + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberForceVelocityCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless these conditions are met + -0 <= dydxC < 1 + -dydxC < dydxNearC < 1 + -1 < dydxIso + -dydxE < (fmaxE-1) + -dydxE < dydxNearC < (fmaxE-1) + -0<= concCurviness <=0 + -0 <= eccCurviness <= 0 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png + + + + Computational Costs + \verbatim + ~8,200 flops + \endverbatim + + Example: + @code + double fmaxE = 1.8; + double dydxC = 0.1; + double dydxNearC = 0.25; + double dydxE = 0.1; + double dydxNearE = 0.15; + double dydxIso= 5; + double concCurviness = 0.1; + double eccCurviness = 0.75; + + SmoothSegmentedFunction fiberFVCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberForceVelocityCurve(fmaxE, + dydxC, dydxNearC, dydxIso, dydxE, dydxNearE, + concCurviness, eccCurviness,"test", fiberFVCurve); + fiberFVCurve.printCurveToFile(); + @endcode + */ + static void createFiberForceVelocityCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continuous (continuous to the 2nd + derivative) inverse curve that the function + createFiberForceVelocityCurve generates. The inverse force velocity + curve is required by every equilibrium muscle model in order to compute + the derivative of fiber velocity. To generate the inverse force velocity + curve simply call this function with EXACTLY the same parameter values + that you used to generate the force velocity curve. See the parameter + descriptions for createFiberForceVelocityCurve, as the parameters for + the inverse function are identical. The curve name should be different, + however, because this is an inverse curve + (e.g. "bicep_fiberForceVelocityInverseCurve") + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png + + */ + static void createFiberForceVelocityInverseCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& muscleName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This element will generate a C2 continuous (continuous to the 2nd + derivative) compressive force profile curve as a function of pennation. + A muscle model with this element usually places this element parallel to + the fiber.The main function of this element is to prevent the fiber from + achieving a pennation angle of pi/2 radians. This type of element is + necessary for a parallelogram pennated equilibrium muscle models because + without it, the muscle model can deform to the point where a pennation + angle of pi/2 radians is reached, which causes a singularity in the + model. + + + @param phi0 The pennation angle at which the compressive force element + starts to engage . When the pennation angle is greater than + phi0, the compressive element is generating a force. When the + pennation angle is less than phi0, the compressive element + generates no force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when the fiber is pennated by 90 degrees + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForcePennationCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -0 < phi0 < SimTK::Pi/2 + -kiso > 1/(SimTK::Pi/2-phi0) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double phi0 = (SimTK::Pi/2)*(8.0/9.0); + double kiso = 8.389863790885878; + double c = 0.0; + + SmoothSegmentedFunction fiberCEPhiCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberCompressiveForcePennationCurve(phi0,kiso,c,"test",fiberCEPhiCurve); + fiberCEPhiCurve.printCurveToFile(); + @endcode + */ + static void createFiberCompressiveForcePennationCurve( + double phi0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This element will generate a C2 continuous (continuous to the 2nd + derivative) compressive force profile curve as a function of + cos(pennation). + + A muscle model with this element usually places this element in line + with the tendon. The main function of this element is to prevent the + fiber from achieving a pennation angle of pi/2 radians. This type of + element is necessary for a parallelogram pennated muscle models because + without it, the muscle model can deform to the point where a pennation + angle of pi/2 radians is reached, which causes a singularity in the + model. + + + @param cosPhi0 The cosine of the pennation angle at which the + compressive force element starts to engage. When the + cos of the pennation angle is greater than cosPhi0, the + compressive element generates no force. When cos of the + pennation angle is less than cosPhi0, the compressive + element generates a compressive force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when cosPhi is zero. This parameter must be + negative + cos + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForceCosPennationCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met: + -0 < cosPhi0 + -kiso > 1/(cosPhi0) + -0 <= curviness <= 1 + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double cosPhi0 = cos( (80.0/90.0)*SimTK::Pi/2); + double kiso = -1.2/(cosPhi0); + double c = 0.5; + + SmoothSegmentedFunction fiberCECosPhiCurve = MuscleFunctionFactory:: + createFiberCompressiveForceCosPennationCurve(cosPhi0,kiso,c,"test"); + fiberCEPhiCurve.printCurveToFile(); + @endcode + + + + */ + static void createFiberCompressiveForceCosPennationCurve( + double cosPhi0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + /** + This element will generate a C2 continous (continuous to the second + derivative) curve that models a compressive force profile that is a + function of fiber length. The main function of + this element is to prevent the fiber from achieving an unrealistically + short length. This type of element is necessary for equilibrium-type + muscle models because of the editing that is done to the active force + length curve that endows an equilibrium model fiber with the ability to + to generate force when a physiological fiber cannot. + + + + @param l0 The normalized fiber length at which the compressive element + starts to engage. When the fiber is shorter than l0, the + compressive element is generating a force. When the fiber + length is longer than l0, the compressive element generates + no force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when the fiber has a length of 0, under a load + of 1 maximum isometric unit of force. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -e0 > 0 + -kiso > 1/(e0) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double lmax = 0.6; + double kiso = -8.389863790885878; + double c = 0.1;//0.0; + + SmoothSegmentedFunction fiberCECurve = MuscleFunctionFactory:: + createFiberCompressiveForceLengthCurve(lmax,kiso,c,"test"); + fiberCECurve.printCurveToFile(); + @endcode + + */ + static void createFiberCompressiveForceLengthCurve( + double l0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continuous curve that fits a fiber's + tensile force length curve. + + @param eZero The fiber strain at which the fiber begins to develop force. + Thus an e0 of 0.0 means that the fiber will start to develop + passive force when it has a normalized length of 1.0. Note + that e0 can be postive or negative. + + @param eIso The fiber strain at which the fiber develops 1 unit of + normalized force (1 maximum isometric force). Note that the + '1' is left off. Thus an e0 of 0.6 means that the fiber + will develop an 1 normalized force unit when it is strained + by 60% of its resting length, or to a normalized length of + 1.6 + + @param kLow The normalized stiffness (or slope) of the fiber curve + close to the location where the force-length curve + approaches a normalized force of 0. This is usually + chosen to be a small, but non-zero fraction of kIso + (kLow = 0.025 kIso is typical). + + @param kIso The normalized stiffness (or slope) of the fiber curve + when the fiber is strained by eIso (or has a length of + 1+eIso) under a load of 1 maximum isometric unit of force. + + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -eIso > eZero + -kIso > 1/(eIso-eZero) + -0 < kLow < kIso + -0 <= curviness <= 1 + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double eIso = 0.6; + double eZero = 0.0; + double kIso = 4.0/(eIso-eZero); + double kNearZero = 0.025*kIso + double c = 0.5; + + SmoothSegmentedFunction fiberFLCurve + = MuscleFunctionFactory:: + createFiberForceLengthCurve(eZero, eIso, + kLow, kIso, c,"test"); + fiberFLCurve.printCurveToFile(); + @endcode + + */ + static void createFiberForceLengthCurve( + double eZero, + double eIso, + double kLow, + double kIso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + Will generate a C2 continous (continuous to the second derivative) + curve in a MuscleFunctionObject object that fits a tendon's tensile + force length curve. + + + + @param eIso The tendon strain at which the tendon develops 1 unit + of normalized force (1 maximum isometric force). Note that + the'1' is left off. Thus an e0 of 0.04 means that the tendon + will develop an 1 normalized force unit when it is strained + by 4% of its resting length, at a normalized length of + 1.04 + + @param kIso The normalized stiffness (or slope) of the tendon + curve when the tendon is strained by e0 + (or has a length of 1+e0) under a load of 1 maximum + isometric unit of force. + + @param fToe The normalized force at which the tendon smoothly + transitions from the curved low stiffness region to + the linear stiffness region. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_tendonForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met: + -0 < fToe < 1 + -e0 > 0 + -kiso > 1/e0 + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double e0 = 0.04; + double kiso = 42.79679348815859; + double fToe = 1.0/3.0 + double c = 0.75; + + SmoothSegmentedFunction* tendonCurve = MuscleFunctionFactory:: + createTendonForceLengthCurve( + e0,kiso,fToe,c,"test"); + tendonCurve.printCurveToFile(); + @endcode + + + */ + static void createTendonForceLengthCurve(double eIso, + double kIso, + double fToe, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + + +}; + +} +} +} + +#endif //MUSCLEFUNCTIONFACTORY_H_ diff --git a/3rdparty/rbdl/addons/muscle/NOTICE b/3rdparty/rbdl/addons/muscle/NOTICE new file mode 100644 index 0000000..e12de15 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/NOTICE @@ -0,0 +1,22 @@ +Author: + Matthew Millard + +Date: + July 2016 + + +Notice: these files + + -MuscleFunctionFactory.cc + -MuscleFunctionFactory.h + -csvtools.h + -csvtools.cc + -tests/testMuscleFunctionFactory.cc + +were originally part of OpenSim and have been ported +over to RBDL with some modification. These files are licenced under the +APACHE 2.0 license which, like the zlib license, is quite liberal. The +full licence can be found in this folder in the file "LICENSE_APACHE-2.0.txt" +and online here: + +http://www.apache.org/licenses/LICENSE-2.0.txt diff --git a/3rdparty/rbdl/addons/muscle/README.md b/3rdparty/rbdl/addons/muscle/README.md new file mode 100644 index 0000000..5905145 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/README.md @@ -0,0 +1,55 @@ +@brief muscle - a set of functions and classes for simulation musculotendon + dynamics. This addon is maintained by Matthew Millard, so if + you have problems with it email him. + +@author Matthew Millard + +\copyright 2016 Matthew Millard + +\b Requirements +This addon depends on the geometry addon + +\b Description + This addon currently contains an optimized library for creating specialized + curves that describe phenomenological curves for line-type muscles + -fiber active-force-length curve + -fiber force-velocity curve + -fiber passive-force-length curve + -tendon force-length curve + and torque-type muscles + -active torque-angle curve + -active torque-velocity curve + -passive torque-angle curve + In addition, there is a class that can be used to package the memory and + functions required to model torque muscles: Millard2016TorqueMuscle. + +\b Future Development +In the near future this library will also contain + +1. Torque-type muscle models with + a. Elastic tendons + b. Short-range-stiffness + +2. An implementation of the line-type muscle Millard2012Equilibrium muscle model + which features the option of using rigid and elastic tendons, and a damped/ + undamped formulation. + +3. An novel implemenation of 2D muscle wrapping using an obstacle set of + smooth convex shapes described using Pythagorean hodographs. + +\b Licensing +The following files have been ported over from OpenSim and Simbody and as such +are licenced under the Apache 2.0 Licence: + +SmoothSegmentedFunctionFactory.h +SmoothSegmentedFunctionFactory.cc + +The Apache License is very similar to the zlib license and is quite liberal. +Licensed under the Apache License, Version 2.0 (the "License"); you may +not use this file except in compliance with the License. You may obtain a +copy of the License at http://www.apache.org/licenses/LICENSE-2.0. + +The remaining code has been written from scratch and is licenced under the +zlib license. See the LICENSE file for details. + + diff --git a/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc new file mode 100644 index 0000000..89b9f95 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc @@ -0,0 +1,1434 @@ +/*------------------------------------------------------------------------- + OpenSim: SmoothSegmentedFunctionFactory.cpp + -------------------------------------------------------------------------- + The OpenSim API is a toolkit for musculoskeletal modeling and simulation. + See http:%opensim.stanford.edu and the NOTICE file for more information. + OpenSim is developed at Stanford University and supported by the US + National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA + through the Warrior Web program. + + Copyright (c) 2005-2012 Stanford University and the Authors + Author(s): Matthew Millard + + Licensed under the Apache License, Version 2.0 (the 'License'); you may + not use this file except in compliance with the License. You may obtain a + copy of the License at http:%www.apache.org/licenses/LICENSE-2.0. + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an 'AS IS' BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + -------------------------------------------------------------------------- + + Derivative work + Date : September 2016 + Authors(s): Millard + Updates : Made active torque-angle, passive-torque-angle, torque-velocity + and tendon-torque-angle curves based on the equivalent line-type + curves in OpenSim. +*/ + +#include "TorqueMuscleFunctionFactory.h" +#include +#include +#include +#include +#include + +using namespace std; +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; + +static double SQRTEPS = sqrt( (double)std::numeric_limits::epsilon()); + +//============================================================================= +// Anderson 2007 Active Torque Angle Curve +//============================================================================= + +void TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueAngleCurve( + double c2, + double c3, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(c2 > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007ActiveTorqueAngleCurve " + << curveName + << ": c2 must be greater than 0" + << endl; + assert(0); + abort(); + } + + + std::string name=curveName; + name.append(".createAnderson2007ActiveTorqueAngleCurve"); + + //For now these advanced paramters are hidden. They will only be + //uncovered if absolutely necessary. + double minValueAtShoulders = 0; + double minShoulderSlopeMagnitude = 0; + + double curviness = 0.5; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //Translate the user parameters to quintic Bezier points + double x0 = c3 - 1.05*(0.5*(M_PI/c2)); + double x1 = c3 - 0.95*(0.5*(M_PI/c2)); + double x2 = c3; + double x3 = c3 + 0.95*(0.5*(M_PI/c2)); + double x4 = c3 + 1.05*(0.5*(M_PI/c2)); + + double y0 = minValueAtShoulders; + double y1 = cos(c2*(x1-c3)); + double y2 = cos(c2*(x2-c3)); + double y3 = cos(c2*(x3-c3)); + double y4 = minValueAtShoulders; + + double dydx0 = minShoulderSlopeMagnitude; + double dydx1 = -sin(c2*(x1-c3))*c2; + double dydx2 = -sin(c2*(x2-c3))*c2; + double dydx3 = -sin(c2*(x3-c3))*c2; + double dydx4 = -minShoulderSlopeMagnitude; + + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + + RigidBodyDynamics::Math::MatrixNd p3 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,4); + RigidBodyDynamics::Math::MatrixNd mY(6,4); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + mX.col(3) = p3.col(0); + mY.col(3) = p3.col(1); + + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, x0, x4, y0, y4, dydx0, dydx4, curveName); +} + +//============================================================================= +// ANDERSON 2007 Active Torque Angular Velocity Curve +//============================================================================= +void TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueVelocityCurve( + double c4, + double c5, + double c6, + double minEccentricMultiplier, + double maxEccentricMultiplier, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(c4 < c5) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than c5" + << endl; + assert(0); + abort(); + } + + if( !((c4 > 0)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(c6 > 0.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c6 must be greater than 1.0" + << endl; + assert(0); + abort(); + } + + if( !(minEccentricMultiplier > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": minEccentricMultiplier must be greater than 1.0" + << endl; + assert(0); + abort(); + } + + if( !(maxEccentricMultiplier > minEccentricMultiplier) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": maxEccentricMultiplier must be greater than " + << " minEccentricMultiplier" + << endl; + assert(0); + abort(); + } + + //Advanced settings that we'll hide for now + double minShoulderSlopeMagnitude = 0; + double curviness = 0.75; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //Go and get the value of the curve that is closest to + //the maximum contraction velocity by setting rhs of Eqn. 9 + //to 0 and solving + double dthMaxConc = abs( 2.0*c4*c5/(c5-3.0*c4) ); + + //Go and evaluate the concentric side of the Anderson curve + //at 1/2 of omega max - we need this to use the updated + //torque-velocity curve. + double wMid = dthMaxConc*0.50; + double tvMidDen = (2*c4*c5 + wMid*(2*c5-4*c4)); + double tvMid = (2*c4*c5 + wMid*(c5-3*c4))/tvMidDen; + + tvMid = min(tvMid,0.45); + double tvMaxEcc = 1.1 + c6*0.2; + + createTorqueVelocityCurve(tvMaxEcc, + tvMid, + curveName, + smoothSegmentedFunctionToUpdate); + + +} +//============================================================================= +// ANDERSON 2007 Passive Torque Angle Curve +//============================================================================= +void TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + double scale, + double c1, + double b1, + double k1, + double b2, + double k2, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(scale > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": scale must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(c1 > 0) ) { + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": c1 must be greater than 0" + << endl; + assert(0); + abort(); + } + + //Advanced settings that we'll hide for now + double curviness = 0.75; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double minShoulderSlopeMagnitude = 0; + + //Zero out the coefficients associated with a + //the side of the curve that goes negative. + bool flag_oneSided = true; + + if(flag_oneSided){ + if(fabs(b1) > 0){ + if(b1 > 0){ + b2 = 0; + k2 = 0; + }else{ + b1 = 0; + k1 = 0; + } + + }else if(fabs(b2) > 0){ + if(b2 > 0){ + b1 = 0; + k1 = 0; + }else{ + b2 = 0; + k2 = 0; + } + } + } + //Divide up the curve into a left half + //and a right half, rather than 1 and 2. + //Why? These two different halves require different + // Bezier curves. + + double c1Scale = c1*scale; + double thL = 0.; //left + double thR = 0.; //right + double DtauDthL = 0.; + double DtauDthR = 0.; + double bL = 0.; + double kL = 0.; + double bR = 0.; + double kR = 0.; + + int curveType = 0; //flat curve + int flag_thL = 0; + int flag_thR = 0; + + if(fabs(k1)>0 && fabs(b1)>0){ + //The value of theta where the passive force generated by the + //muscle is equal to 1 maximum isometric contraction. + thL = (1/k1)*log(fabs( c1Scale/b1 )); + DtauDthL = b1*k1*exp(thL*k1); + bL = b1; + kL = k1; + flag_thL = 1; + } + + if(fabs(k2)>0 && fabs(b2)>0){ + //The value of theta where the passive force generated by the + //muscle is equal to 1 maximum isometric contraction. + thR = (1/k2)*log(fabs( c1Scale/b2 )); + DtauDthR = b2*k2*exp(thR*k2); + bR = b2; + kR = k2; + flag_thR = 1; + } + + //A 'left' curve has a negative slope, + //A 'right' curve has a positive slope. + if(DtauDthL > DtauDthR){ + double tmpD = thL; + thL = thR; + thR = tmpD; + + tmpD = bR; + bR = bL; + bL = tmpD; + + tmpD = kR; + kR = kL; + kL = tmpD; + + tmpD = DtauDthL; + DtauDthL = DtauDthR; + DtauDthR = tmpD; + + int tmpI = flag_thL; + flag_thL = flag_thR; + flag_thR = tmpI; + } + + + if(flag_thL){ + curveType = curveType + 1; + } + if(flag_thR){ + curveType = curveType + 2; + } + + RigidBodyDynamics::Math::MatrixNd mX(6,1); + RigidBodyDynamics::Math::MatrixNd mY(6,1); + + double xStart = 0; + double xEnd = 0; + double yStart = 0; + double yEnd = 0; + double dydxStart = 0; + double dydxEnd = 0; + + + switch(curveType){ + + case 0: + {//No curve - it's a flat line + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(0.,0.,0., + 1.,0.,0.,c); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + }break; + case 1: + { + //Get a point on the curve that is close to 0. + double x1 = (1/kL)*log(fabs(0.01*c1Scale/bL) ); + double y1 = bL*exp(kL*x1); + double dydx1 = bL*kL*exp(kL*x1); + + //Get a point that is at 1 maximum isometric torque + double x3 = thL; + double y3 = bL*exp(kL*x3); + double dydx3 = bL*kL*exp(kL*x3); + + //Get a mid-point + double x2 = 0.5*(x1+x3); + double y2 = bL*exp(kL*x2); + double dydx2 = bL*kL*exp(kL*x2); + + //Past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + xStart = x3; + xEnd = x0; + yStart = y3; + yEnd = y0; + dydxStart = dydx3; + dydxEnd = dydx0; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x0,y0,dydx0,c); + + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + }break; + case 2: + { + //Get a point on the curve that is close to 0. + double x1 = (1/kR)*log(fabs(0.01*c1Scale/bR) ); + double y1 = bR*exp(kR*x1); + double dydx1 = bR*kR*exp(kR*x1); + + //Go just past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + //Get a point close to 1 maximum isometric torque + double x3 = thR; + double y3 = bR*exp(kR*x3); + double dydx3 = bR*kR*exp(kR*x3); + + //Get a mid point. + double x2 = 0.5*(x1+x3); + double y2 = bR*exp(kR*x2); + double dydx2 = bR*kR*exp(kR*x2); + + xStart = x0; + xEnd = x3; + yStart = y0; + yEnd = y3; + dydxStart = dydx0; + dydxEnd = dydx3; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + }break; + case 3: + { + //Only when flag_oneSided = false; + double x0 = thL; + double x4 = thR; + + double x2 = 0.5*(x0 + x4); + double x1 = 0.5*(x0 + x2); + double x3 = 0.5*(x2 + x4); + + double y0 = b1*exp(k1*x0) + + b2*exp(k2*x0); + double y1 = b1*exp(k1*x1) + + b2*exp(k2*x1); + double y2 = b1*exp(k1*x2) + + b2*exp(k2*x2); + double y3 = b1*exp(k1*x3) + + b2*exp(k2*x3); + double y4 = b1*exp(k1*x4) + + b2*exp(k2*x4); + + double dydx0 = b1*k1*exp(k1*x0) + + b2*k2*exp(k2*x0); + double dydx1 = b1*k1*exp(k1*x1) + + b2*k2*exp(k2*x1); + double dydx2 = b1*k1*exp(k1*x2) + + b2*k2*exp(k2*x2); + double dydx3 = b1*k1*exp(k1*x3) + + b2*k2*exp(k2*x3); + double dydx4 = b1*k1*exp(k1*x4) + + b2*k2*exp(k2*x4); + + xStart = x0; + xEnd = x4; + yStart = y0; + yEnd = y4; + dydxStart = dydx0; + dydxEnd = dydx4; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + RigidBodyDynamics::Math::MatrixNd p3 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); + + mX.resize(6,4); + mY.resize(6,4); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + mX.col(3) = p3.col(0); + mY.col(3) = p3.col(1); + + }break; + default: + { + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": undefined curveType" + << endl; + assert(0); + abort(); + } + + }; + + //Normalize the y values. + mY = mY*(1/c1Scale); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, + xStart, xEnd, + yStart/c1Scale, yEnd/c1Scale, + dydxStart/c1Scale, dydxEnd/c1Scale, + curveName); +} + +//============================================================================= +// +// New torque-muscle characteristic curves +// +//============================================================================= + + +//============================================================================= +// torque-velocity curves +//============================================================================= + +void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) + { + + double slopeAtConcentricOmegaMax = 0.0; + double slopeNearEccentricOmegaMax = -0.025; + double slopeAtEccentricOmegaMax = 0.0; + double eccentricCurviness = 0.75; + + createTorqueVelocityCurve( + tvAtEccentricOmegaMax, + tvAtHalfConcentricOmegaMax, + slopeAtConcentricOmegaMax, + slopeNearEccentricOmegaMax, + slopeAtEccentricOmegaMax, + eccentricCurviness, + curveName, + smoothSegmentedFunctionToUpdate); + + } + +void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + double slopeAtConcentricOmegaMax, + double slopeNearEccentricOmegaMax, + double slopeAtEccentricOmegaMax, + double eccentricCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + if( (tvAtEccentricOmegaMax < 1.05) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtEccentricOmegaMax must be greater than 1.05" + << endl; + assert(0); + abort(); + } + + if( (tvAtHalfConcentricOmegaMax < 0.05 + || tvAtHalfConcentricOmegaMax > 0.45) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtHalfOmegaMax must be in the interval [0.05,0.45]" + << endl; + assert(0); + abort(); + } + + if( (slopeAtConcentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtConcentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (slopeNearEccentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeNearEccentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (slopeAtEccentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtEccentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (eccentricCurviness < 0 || eccentricCurviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": eccentricCurviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + + double omegaMax = 1.0; + double wmaxC = omegaMax; //In biomechanics the concentric side gets a + //a +'ve signed velocity + double wmaxE = -omegaMax; + + //----------------------------------------------------------------------- + // 1. Fit concentric Bezier curves to a Hill-type hyperbolic curve + //----------------------------------------------------------------------- + + /* + First we compute the terms that are consistent with a Hill-type concentric + contraction. Starting from Hill's hyperbolic equation + + f(w) = (fiso*b - a*w) / (b+w) + + + Taking a derivative w.r.t omega yields + + df(w)/dw = [ (fiso*b - a*w + a*(b+w))] / (b+w)^2 + + at w = wmaxC the numerator goes to 0 + + (fiso*b - a*wmaxC) / (b + wmaxC) = 0 + (fiso*b - a*wmaxC) = 0; + b = a*wmaxC/fiso; + + Subtituting this expression for b into the expression when + f(wHalf) = tvAtHalfOmegaMax yields this expression for parameter a + + a = tvAtHalfOmegaMax*w*fiso ... + / (wmaxC*tvAtHalfOmegaMax + fiso*wmaxC - fiso*w); + + */ + + double fiso = 1.0; + double w = 0.5*wmaxC; + double a = -tvAtHalfConcentricOmegaMax*w*fiso + / (wmaxC*tvAtHalfConcentricOmegaMax - fiso*wmaxC + fiso*w); + + double b = a*wmaxC/fiso; + double yCheck = (b*fiso-a*w)/(b+w); + + if( abs(yCheck-tvAtHalfConcentricOmegaMax) > SQRTEPS ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << "abs(yCheck-tvAtHalfOmegaMax) > sqrt(eps)" + << "Consult the maintainers of this addon." + << endl; + assert(0); + abort(); + } + + + w = 0*wmaxC; + double dydxIso = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w)); + + + w = 0.9*wmaxC; + double dydxNearC = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w)); + + + if( dydxNearC > slopeAtConcentricOmegaMax || abs(dydxNearC) > abs(1/wmaxC) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << " dydxNearC < dydxC || dydxNearC > abs(1/wmaxC)" + << "Consult the maintainers of this addon." + << endl; + assert(0); + abort(); + } + + //---------------------------------------------------------------------------- + // Iterate over the curviness value to get a good match between Hill's + // hyperbolic curve and the Bezier curve + //---------------------------------------------------------------------------- + + double xNearC = 0.9*wmaxC; + double yNearC = (b*fiso-a*w)/(b+w); + double xIso = 0; + double yIso = 1.0; + + double cC = 0.5; + + MatrixNd pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso, + xNearC,yNearC, dydxNearC, + cC); + MatrixNd xpts(6,1); + xpts.col(0) = pts.col(0); + MatrixNd ypts(6,1); + ypts.col(0) = pts.col(1); + + SmoothSegmentedFunction fvCFcn = SmoothSegmentedFunction( + xpts,ypts, + xIso,xNearC, + yIso,yNearC, + dydxIso,dydxNearC, + "tvFcn"); + SmoothSegmentedFunction fvCFcnLeft = SmoothSegmentedFunction(); + SmoothSegmentedFunction fvCFcnRight = SmoothSegmentedFunction(); + + int nSample = 10; + double f = 0; + double yHill = 0; + + //Calculate the error of the Bezier curve with the starting curviness value + //of 0.5 + for(int j=0; j (0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque has a magnitude that is too " + << "large, it exceeds 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( abs(stiffnessAtOneNormTorque) + < (1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque has a magnitude that is too " + << "small, it is less than 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque*stiffnessAtLowTorque < 0.0 ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque and stiffnessAtOneNormTorque must have the" + << " same sign." + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque + *(angleAtOneNormTorque-angleAtZeroTorque) < 0.0){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque must have the same sign as " + << "(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + MatrixNd xM(6,2); + MatrixNd yM(6,2); + MatrixNd pts(6,2); + + double x0,x1,y0,y1,dydx0,dydx1; + if(angleAtZeroTorque < angleAtOneNormTorque){ + x0 = angleAtZeroTorque; + x1 = angleAtOneNormTorque; + y0 = 0.; + y1 = 1.; + dydx0 = 0.0; + dydx1 = stiffnessAtOneNormTorque; + }else{ + x0 = angleAtOneNormTorque; + x1 = angleAtZeroTorque; + y0 = 1.0; + y1 = 0.0; + dydx0 = stiffnessAtOneNormTorque; + dydx1 = 0.0; + } + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + if(abs(stiffnessAtOneNormTorque) > SQRTEPS){ + + double delta = min( 0.1*(1.0-abs(1.0/stiffnessAtOneNormTorque)), + 0.05*abs(x1-x0)); + if(stiffnessAtOneNormTorque < 0.){ + delta *= -1.0; + } + + double xLow = angleAtZeroTorque + delta; + double xFoot = angleAtZeroTorque + 0.5*(xLow-angleAtZeroTorque); + double yFoot = 0.0; + double yLow = yFoot + stiffnessAtLowTorque*(xLow-xFoot); + + pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( + x0, y0, dydx0, + xLow,yLow,stiffnessAtLowTorque, c); + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( + xLow,yLow,stiffnessAtLowTorque, + x1, y1, dydx1, c); + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + }else{ + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + angleAtZeroTorque,0, 0, + angleAtOneNormTorque,0, 0, + c); + xM.col(0) = pts.col(0); + yM.col(1) = pts.col(1); + } + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + xM, yM, + x0, x1, + y0, y1, + dydx0, dydx1, + curveName); + + +} + +//============================================================================= +// active-torque angle curve +//============================================================================= + + +void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + 0.0, + 0.0, + 0.5, + curveName, + smoothSegmentedFunctionToUpdate ); + +} + + +void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + double minSlopeAtShoulders, + double minValueAtShoulders, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + if( (angularStandardDeviation < SQRTEPS) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": angularStandardDeviation is less than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + if( (minValueAtShoulders < 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minValueAtShoulders is less than 0" + << endl; + assert(0); + abort(); + } + + + if( (minSlopeAtShoulders < 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minSlopeAtShoulders is less than 0" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + double taCutoff = 1e-3; + if(taCutoff < minValueAtShoulders ){ + taCutoff = minValueAtShoulders; + } + double angularStandardDeviationSq = + angularStandardDeviation*angularStandardDeviation; + double thetaWidth = sqrt(-log(taCutoff)*2*angularStandardDeviationSq); + double thetaMin = -thetaWidth + angleAtOneNormTorque; + double thetaMax = thetaWidth + angleAtOneNormTorque; + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + double x0 = thetaMin; + + double x1 = angleAtOneNormTorque - angularStandardDeviation; + double x2 = angleAtOneNormTorque; + double x3 = angleAtOneNormTorque + angularStandardDeviation; + + if( (angleAtOneNormTorque-thetaMin) < + (angleAtOneNormTorque-angularStandardDeviation)){ + x1 = angleAtOneNormTorque - 0.5*thetaMin; + x3 = angleAtOneNormTorque + 0.5*thetaMin; + } + + double x4 = thetaMax; + + double y0 = minValueAtShoulders; + double y1 = exp(-(x1-angleAtOneNormTorque)*(x1-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y2 = exp(-(x2-angleAtOneNormTorque)*(x2-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y3 = exp(-(x3-angleAtOneNormTorque)*(x3-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y4 = minValueAtShoulders; + + + double dydx1 = -(2*(x1-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y1; + double dydx2 = -(2*(x2-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y2; + double dydx3 = -(2*(x3-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y3; + + double dydx0 = minSlopeAtShoulders; + double dydx4 = minSlopeAtShoulders; + + if(dydx1 < 0){ + dydx0 *= -1.0; + } + if(dydx3 < 0){ + dydx4 *= -1.0; + } + + + MatrixNd xM(6,4); + MatrixNd yM(6,4); + MatrixNd pts(6,2); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x0,y0, dydx0, + x1,y1, dydx1, c); + + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x1,y1, dydx1, + x2,y2, dydx2, c); + + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x2,y2, dydx2, + x3,y3, dydx3, c); + + xM.col(2) = pts.col(0); + yM.col(2) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x3,y3, dydx3, + x4,y4, dydx4, c); + + xM.col(3) = pts.col(0); + yM.col(3) = pts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + xM, yM, + x0, x4, + y0, y4, + dydx0,dydx4, + curveName); + +} + + +//============================================================================= +// tendon-torque angle curve +//============================================================================= + +void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + 1.5/angularStretchAtOneNormTorque, + 1.0/3.0, + 0.5, + curveName, + smoothSegmentedFunctionToUpdate ); + +} + + +void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + double stiffnessAtOneNormTorque, + double normTorqueAtToeEnd, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + + if( angularStretchAtOneNormTorque < SQRTEPS ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": angularStretchAtOneNormTorque should be greater than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque should be greater " + << " than 1.1/angularStretchAtOneNormTorque" + << endl; + assert(0); + abort(); + } + + if( normTorqueAtToeEnd < SQRTEPS || normTorqueAtToeEnd > 0.99){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": normTorqueAtToeEnd must be in the inteval [sqrt(eps), 0.99]" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0; + double y0 = 0; + double dydx0 = 0; + + double xIso = angularStretchAtOneNormTorque; + double yIso = 1; + double dydxIso = stiffnessAtOneNormTorque; + + //Location where the curved section becomes linear + double yToe = normTorqueAtToeEnd; + double xToe = (yToe-1)/stiffnessAtOneNormTorque + xIso; + + + //To limit the 2nd derivative of the toe region the line it tends to + //has to intersect the x axis to the right of the origin + double xFoot = (xToe)/10.0; + double yFoot = 0; + + + //Compute the location of the corner formed by the average slope of the + //toe and the slope of the linear section + double yToeMid = yToe*0.5; + double xToeMid = (yToeMid-yIso)/stiffnessAtOneNormTorque + xIso; + double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot); + + //Compute the location of the control point to the left of the corner + double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); + double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot); + + MatrixNd xM(6,2); + MatrixNd yM(6,2); + MatrixNd pts(6,2); + + //Compute the Quintic Bezier control points + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0, y0, dydx0, + xToeCtrl,yToeCtrl,dydxToeMid, c); + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM, + x0,xToe, + y0,yToe, + dydx0,dydxIso, + curveName); + +} diff --git a/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h new file mode 100644 index 0000000..44f92ef --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h @@ -0,0 +1,663 @@ +#ifndef TORQUEMUSCLEFUNCTIONFACTORY_H_ +#define TORQUEMUSCLEFUNCTIONFACTORY_H_ +/*------------------------------------------------------------------------- + OpenSim: SmoothSegmentedFunctionFactory.cpp + -------------------------------------------------------------------------- + The OpenSim API is a toolkit for musculoskeletal modeling and simulation. + See http:%opensim.stanford.edu and the NOTICE file for more information. + OpenSim is developed at Stanford University and supported by the US + National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA + through the Warrior Web program. + + Copyright (c) 2005-2012 Stanford University and the Authors + Author(s): Matthew Millard + + Licensed under the Apache License, Version 2.0 (the 'License'); you may + not use this file except in compliance with the License. You may obtain a + copy of the License at http:%www.apache.org/licenses/LICENSE-2.0. + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an 'AS IS' BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + -------------------------------------------------------------------------- + + Derivative work + Date : September 2016 + Authors(s): Millard + Updates : Made active torque-angle, passive-torque-angle, torque-velocity + and tendon-torque-angle curves based on the equivalent line-type + curves in OpenSim. +*/ + +#include "../geometry/SmoothSegmentedFunction.h" +#include "../geometry/SegmentedQuinticBezierToolkit.h" + +#include +#include +#include +#include + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + +class TorqueMuscleFunctionFactory +{ + public: + + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active torque angle curve. This Bezier curve has been + fitted to match the active-torque-angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + but note that its range is normalized to [0,1]. + + @param c2 (radians) + The active-torque-angle width parameter. The parameter c2 + is defined by Anderson et al. as + + c2 = pi/(theta_max - theta_min). + + @param c3 : (radians) + Then angle which has the largest active-torque. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + +*/ +static void createAnderson2007ActiveTorqueAngleCurve( + double c2, + double c3, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active torque (angular) velocity curve. This Bezier curve + has been fitted to match the active-torque-angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + While the concentric side of the Bezier curve and the original + formulation match, the eccentric side does not: the equations + Anderson et al. chose decrease down to 0 rapidly. Since Anderson + et al. did not collect data at the higher eccentric velocities the + oddities in their chosen curves are likely due to the parameterization + they chose. The eccentric side of the Bezier curve will be fitted + so that, if possible, it passes close to the value of the original + curves for theta = -60 deg/s within the limits imposed by + minEccentricMultiplier and maxEccentricMultiplier. + + @param c4 (rads/s) + Angular velocity when the torque is 75% of the maximum + isometric torque. + + @param c5 (rads/s) + Angular velocity when the torque is 50% of the maximum + isometric torque. + + @param c6 + Multiplier that Anderson et al. uses to describe the + change in slope of the curve as the contraction velocity + changes sign from + to -. + + @param minEccentricMultiplier + The minimum value of the torque-(angular)-velocity curve + tends to at large eccentric contraction velocities. Note + minEccentricMultiplier > 1.0 + + @param maxEccentricMultiplier + The value of the torque-(angular)-velocity curve tends + to at large eccentric contraction velocities. Note + maxEccentricMultiplier > minEccentricMultiplier. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + */ + static void createAnderson2007ActiveTorqueVelocityCurve( + double c4, + double c5, + double c6, + double minEccentricMultiplier, + double maxEccentricMultiplier, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This is a function that will produce a C2 (continuous to the second + derivative) passive torque angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + Note the following differences between this implementation and + the original equations presented in Anderson et al.: + + 1. This function will return a curve that is fitted to the + positive side of the curve defined by the coefficients + b1, k1, b2, and k2. Because of the sign convention employed by + Anderson et al. the positive side of the curve corresponds to + the passive curve generated by the torque actuator associated + with the rest of the coefficients. + + 2. This function has been normalized so that a value of 1.0 + corresponds to one-maximum-isometric-active-contraction + torque, or c1*subjectWeightInNewtons*subjectHeightInMeters. + + @param scale + The scaling factor used on the c1 column in Table 3 of + Anderson et al.: + + scale = subjectWeightInNewtons * subjectHeightInMeters + + @param c1 + The normalized c1 parameter listed in Tabel 3 of + Anderson et al. + + @param b1 + The passive torque angle curve parameter used in + Anderson et al.'s Eqn. 1: + + torquePassive = b1*exp(k1*theta) + b2*exp(k2*theta) + + @param k1 + The term k1 in Anderson et al.'s Eqn. 1. + + @param b2 + The term b2 in Anderson et al.'s Eqn. 1. + + @param k2 + The term k2 in Anderson et al.'s Eqn. 1. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + */ + static void createAnderson2007PassiveTorqueAngleCurve( + double scale, + double c1, + double b1, + double k1, + double b2, + double k2, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + /** + This function creates a normalized torque-velocity curve. The concentric + side of the curve is fitted to Hill's hyperbola that passes through + the value of tv-at-half-of-the-maximum-concentric-velocity (a parameter + supplied by the user). The eccentric side of the curve rapidly, but smoothly + approaches a terminal value of + tv-at-the-maximum-eccentric-contraction-velocity. Outside of the normalized + velocities of -1 to 1 the curve takes the values of 0, and + tvAtEccentricOmegaMax respectively with a slope of 0. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png + + @param tvAtEccentricOmegaMax + The value of the torque-velocity-multiplier at the maximum + eccentric contraction velocity. This value must be + + @param tvAtHalfConcentricOmegaMax + The value of the torque-velocity- + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + -tvAtEccentricOmegaMax < 1.05 + -tvAtHalfOmegaMax >= 0.45 + -tvAtHalfOmegaMax <= 0.05 + + References + Hill, A. V. (1938). The heat of shortening and the dynamic constants of + muscle. Proceedings of the Royal Society of London B: Biological Sciences, + 126(843), 136-195. + + */ + + static void createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + /** + This function creates a normalized torque-velocity curve. The concentric + side of the curve is fitted to Hill's hyperbola that passes through + the value of tv-at-half-of-the-maximum-concentric-velocity (a parameter + supplied by the user). The eccentric side of the curve rapidly, but smoothly + approaches a terminal value of + tv-at-the-maximum-eccentric-contraction-velocity. Shape of the eccentric + side of the curve can be changed using the slopeNearEccentricOmegaMax + and curviness variables. Outside of the normalized + velocities of -1 to 1 the curve takes the values of + slopeAtConcentricOmegaMax and slopeAtEccentricOmegaMax respectively. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png + + @param tvAtEccentricOmegaMax + The value of the torque-velocity-multiplier at the maximum + eccentric contraction velocity. This value must be + + @param tvAtHalfConcentricOmegaMax + The value of the torque-velocity- + + @param slopeAtConcentricOmegaMax + The slope of the curve at a normalized angular velocity of -1. This slope + is used to extrapolate \f$\mathbf{t}_V\f$ for normalized angular velocities + of less than -1. + + @param slopeNearEccentricOmegaMax + The slope of the eccentric side of the curve as the normalized angular + velocity approaches 1. + + @param slopeAtEccentricOmegaMax + The slope of the curve at a normalized angular velocity of 1. This slope + is used to extrapolate \f$\mathbf{t}_V\f$ for normalized angular velocities + of greater than 1. + + @param eccentricCurviness + This parameter controls the shape of the curve between the normalized + angular velocities of 0 and 1. An eccentricCurviness of 0 will flatten + the elbow so that the curve closely follows a line that begins at (0,1) + and ends at (1,tvAtEccentricOmegaMax). An eccentricCurviness of 1 will + give the curve a strong elbow so that it quickly approaches the line that + passes through the point (1,tvAtEccentricOmegaMax) and has a slope of + slopeNearEccentricOmegaMax. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + -tvAtEccentricOmegaMax < 1.05 + -tvAtHalfOmegaMax > 0.45 or tvAtHalfOmegaMax < 0.05 + -slopeAtConcentricOmegaMax < 0 + -slopeNearEccentricOmegaMax < 0 + -slopeAtEccentricOmegaMax < 0 + -eccentricCurviness < 0 or eccentricCurviness > 1 + + References + Hill, A. V. (1938). The heat of shortening and the dynamic constants of + muscle. Proceedings of the Royal Society of London B: Biological Sciences, + 126(843), 136-195. + + */ + static void createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + double slopeAtConcentricOmegaMax, + double slopeNearEccentricOmegaMax, + double slopeAtEccentricOmegaMax, + double eccentricCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + /** + This function creates a Bezier spline that closely follows the + exponential curves that are typically used to model the passive-torque-angle + characteristic of muscles. This curve has a value and a slope of zero + for angles that are less than abs(angleAtZeroTorque). For angles that + have an absolute magnitude larger than abs(angleAtOneNormTorque) the curve + is simply linearly extrapolated. + + Note that curves can be represented that increase left-to-right, or + decrease left-to-right by setting the variables angleAtOneNormTorque and + angleAtZeroTorque correctly. For example using (0,1) for + angleAtOneNormTorque and angleAtZeroTorque produces a curve that increases + left-to-right while using (-1,0) produces a curve that decreases left to + right. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png + + @param angleAtZeroTorque is the angle at which the curve transitions from + a flat line and begins curving upwards. (radians) + + @param angleAtOneNormTorque is the angle at which this curve achieves a + value of 1.0. (radians) + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - abs(angleAtOneNormTorque-angleAtZeroTorque) < sqrt(eps) + */ + static void createPassiveTorqueAngleCurve( + double angleAtZeroTorque, + double angleAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + +/** + This function creates a Bezier spline that closely follows the + exponential curves that are typically used to model the passive-torque-angle + characteristic of muscles. This curve has a value and a slope of zero + for angles that are less than abs(angleAtZeroTorque). For angles that + have an absolute magnitude larger than abs(angleAtOneNormTorque) the curve + is simply linearly extrapolated. + + Note that curves can be represented that increase left-to-right, or + decrease left-to-right by setting the variables angleAtOneNormTorque and + angleAtZeroTorque correctly. For example using (0,1) for + angleAtOneNormTorque and angleAtZeroTorque produces a curve that increases + left-to-right while using (-1,0) produces a curve that decreases left to + right. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png + + @param angleAtZeroTorque is the angle at which the curve transitions from + a flat line and begins curving upwards. (radians) + + @param angleAtOneNormTorque is the angle at which this curve achieves a + value of 1.0. (radians) + + @param stiffnessAtLowTorque + The normalized stiffness (or slope) of the curve achieves as it begins + to increase. This is usually chosen to be a small, but non-zero fraction + of stiffnessAtOneNormTorque + (stiffnessAtLowTorque = 0.025 stiffnessAtOneNormTorque is typical). + The sign of stiffnessAtLowTorque must be positive if + angleAtOneNormTorque > angleAtZeroPassiveTorque. The sign + of stiffnessAtLowTorque must be negative if + angleAtOneNormTorque < angleAtZeroPassiveTorque. + (Norm.Torque/radians) + + @param stiffnessAtOneNormTorque + The normalized stiffness (or slope) of the fiber curve + when the fiber is stretched by + angleAtOneNormTorque - angleAtZeroPassiveTorque. The sign + of stiffnessAtOneNormTorque must agree with stiffnessAtLowTorque. + (Norm.Torque/radians) + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - abs(angleAtOneNormTorque-angleAtZeroTorque) < sqrt(eps) + - sign(stiffnessAtLowTorque) != sign(angleAtOneNormTorque-angleAtLowTorque) + - sign(stiffnessAtOneNormTorque) != sign(stiffnessAtLowTorque) + - abs(stiffnessAtLowTorque) > 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque) + - abs(stiffnessAtOneTorque) <= 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque) + - curviness < 0 or curviness > 1 + + */ + static void createPassiveTorqueAngleCurve( + double angleAtZeroTorque, + double angleAtOneNormTorque, + double stiffnessAtLowTorque, + double stiffnessAtOneNormTorque, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + + /** + This function produces a Bezier curve fitted to a Gaussian function. As the + tails of the Gaussian curve become small this curve is simply extrapolated + as a line with a y-value of zero and a slope of zero. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png + + @param angleAtOneNormTorque The angle at which the Gaussian curve develops + a value of 1. + + @param angularStandardDeviation The angular deviation from + the mean at which the Gaussian curve reaches a value of \f$e^{-1/2}\f$. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - angularWidthOfOneStandardDeviation < sqrt(eps) + */ + static void createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + +/** + This function produces a C2 continuous Bezier curve fitted to a Gaussian + function. As the tails of the Gaussian curve become less than + minValueAtShoulders, the curve is linearly extrapolated at a sloe of + minSlopeOfShoulders. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png + + @param angleAtOneNormTorque The angle at which the Gaussian curve develops + a value of 1. + + @param angularStandardDeviation The angular deviation from + the mean at which the Gaussian curve reaches a value of \f$e^{-1/2}\f$. + + @param minSlopeAtShoulders The y-value at which the Bezier curve transitions + from having a shape like a Gaussian curve to being linearly extrapolated. + + @param minValueAtShoulders The slope of the linear extrapolation of the + Bezier curve for y-values that are less than minSlopeAtShoulders. The + sign of minValueAtShoulders is automatically set so that it matches the + curve near it (see the figure). + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - angularWidthOfOneStandardDeviation < sqrt(eps) + - minSlopeAtShoulders < 0 + - minValueAtShoulders < 0 + - curviness > 1 or curviness < 0 + + */ + static void createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + double minSlopeAtShoulders, + double minValueAtShoulders, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + + /** + This function produces a normalized tendon-torque-angle curve with a + toe region that is in the range of \f$\mathbf{t}_V\f$ [0,1./3,] after which + the curve is linearly extrapolated. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png + + @param angularStretchAtOneNormTorque The amount of angular stretch of the + joint as the tendon goes from developing zero torque at its slack length + to developing one maximum isometric torque. (radians) + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - angularWidthOfOneStandardDeviation < sqrt(eps) + + */ + static void createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + /** + This function produces a normalized tendon-torque-angle curve with a + toe region, final stiffness, and shape that can be controlled. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png + + @param angularStretchAtOneNormTorque The amount of angular stretch of the + joint as the tendon goes from developing zero torque at its slack length + to developing one maximum isometric torque. (radians) + + @param stiffnessAtOneNormTorque The linear stiffness value of the tendon + that is used for all y-values greater than the toe region. + (Norm. Torque/rad) + + @param normTorqueAtToeEnd The normalized torque value which defines the + end of the nonlinear-stiffness region of the tendon and the beginning of + the linear stiffness region of the tendon. + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + - angularStretchAtOneNormTorque < sqrt(eps) + - stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque + - normTorqueAtToeEnd < sqrt(eps) or normTorqueAtToeEnd > 0.99 + - curviness < 0 or curviness > 1 + + */ + static void createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + double stiffnessAtOneNormTorque, + double normTorqueAtToeEnd, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + +}; +} +} +} +#endif //TORQUEMUSCLEFUNCTIONFACTORY_H_ diff --git a/3rdparty/rbdl/addons/muscle/csvtools.cc b/3rdparty/rbdl/addons/muscle/csvtools.cc new file mode 100644 index 0000000..b8b5517 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/csvtools.cc @@ -0,0 +1,160 @@ +/* -------------------------------------------------------------------------- * + * csvtools.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +#include "csvtools.h" + + +std::vector > readMatrixFromFile( + const std::string& filename, + int startingRow) +{ + std::ifstream datafile; + datafile.open(filename.c_str()); + + + //SimTK::Matrix data; + std::vector > dataMatrix; + std::vector rowVector; + + if(datafile.is_open()) + { + std::string line; + std::string entry; + int row = 0; + int col = 0; + //int matrixRowNum = 0; + int matrixColNum = 1; + + //1. Size the matrix + + + getline(datafile,line); //get a line of text + + while(row < startingRow){ + getline(datafile,line); + row++; + } + + //parse it for all of the comma spots + unsigned pos1 = 0; + unsigned pos2 = 0; + do{ + pos2 = line.find(",",pos1); + //if this is the first time running this loop, count + //the number of columns + if(pos2 != std::string::npos && row == 0) + matrixColNum++; + + pos1 = pos2+1; + }while(pos2 != std::string::npos); + + //Initial matrix sizing + if(row == 0){ + //matrixRowNum = max(matrixColNum,20); + rowVector.resize(matrixColNum); + //dataMatrix.resize(matrixRowNum); + //data.resizeKeep(matrixRowNum, matrixColNum); + } + + + while(datafile.good()) + { + pos1 = 0; + pos2 = 0; + for(int i=0; i < matrixColNum; i++){ + pos2 = line.find(",",pos1); + if(pos2 == std::string::npos) + pos2 = line.length(); + entry = line.substr(pos1,pos2-pos1); + pos1 = pos2+1; + //data(row,i) = atof(entry.c_str()); + rowVector[i] = atof(entry.c_str()); + } + + //Resize the matrix if its too small for the next line + //if(row == matrixRowNum-1){ + // matrixRowNum = matrixRowNum*2; + // data.resizeKeep(matrixRowNum,matrixColNum); + //} + dataMatrix.push_back(rowVector); + + row++; + getline(datafile,line); + } + //data.resizeKeep(row,matrixColNum); + + } + datafile.close(); + return dataMatrix; +} + +void printMatrixToFile( + const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename) +{ + + std::ofstream datafile; + datafile.open(filename.c_str()); + datafile << std::scientific; + datafile.precision(16); + if(header.length() > 1) + datafile << header << "\n"; + + for(int i = 0; i < dataMatrix.size(); i++){ + for(int j = 0; j < dataMatrix[0].size(); j++){ + if(j >& dataMatrix, + const std::string& header, + const std::string& filename) +{ + + std::ofstream datafile; + datafile.open(filename.c_str()); + //datafile << std::scientific; + //datafile.precision(16); + if(header.length() > 1) + datafile << header << "\n"; + + for(int i = 0; i < dataMatrix.size(); i++){ + for(int j = 0; j < dataMatrix[0].size(); j++){ + if(j +#include +#include +#include +#include +#include + +/** +This function will print cvs file of the matrix + data + +@params data: A vector of state vectors +@params filename: The name of the file to print +*/ +void printMatrixToFile( const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename); + +void printMatrixToFile( const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename); +/** +This function will read in a cvs file assuming that all entries are numbers. + +@params filename: The name of the file to print +@params startingRow: the index of first row that contains numeric data. + +@return: A matrix of data +*/ +std::vector > readMatrixFromFile( + const std::string& filename, + int startingRow); diff --git a/3rdparty/rbdl/addons/muscle/muscle.h b/3rdparty/rbdl/addons/muscle/muscle.h new file mode 100644 index 0000000..5df7f1e --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/muscle.h @@ -0,0 +1,12 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : forceElements + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ +#ifndef MUSCLE_H_ +#define MUSCLE_H_ + +#include "Millard2016TorqueMuscle.h" +#endif diff --git a/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt b/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt new file mode 100644 index 0000000..00865b0 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt @@ -0,0 +1,78 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) +CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_MUSCLE_VERSION + ${RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH} +) + + +PROJECT (RBDL_MUSCLE_TESTS VERSION ${RBDL_ADDON_MUSCLE_VERSION}) +#SET (PROJECT_VERSION ${RBDL_ADDON_MUSCLE_TESTS_VERSION}) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) +INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) + +SET ( MUSCLE_TESTS_SRCS + testMillard2016TorqueMuscle.cc + testMuscleFunctionFactory.cc + testTorqueMuscleFunctionFactory.cc + ../muscle.h + ../MuscleFunctionFactory.h + ../MuscleFunctionFactory.cc + ../TorqueMuscleFunctionFactory.h + ../TorqueMuscleFunctionFactory.cc + ../Millard2016TorqueMuscle.h + ../Millard2016TorqueMuscle.cc + ../csvtools.h + ../csvtools.cc + ../../geometry/geometry.h + ../../geometry/SegmentedQuinticBezierToolkit.h + ../../geometry/SmoothSegmentedFunction.h + ../../geometry/SegmentedQuinticBezierToolkit.cc + ../../geometry/SmoothSegmentedFunction.cc + ../../geometry/tests/numericalTestFunctions.cc + ../../geometry/tests/numericalTestFunctions.h + ) + +INCLUDE_DIRECTORIES ( ../../../geometry ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +ADD_EXECUTABLE ( rbdl_muscle_tests ${MUSCLE_TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_muscle_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runMuscleTests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +TARGET_LINK_LIBRARIES ( rbdl_muscle_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) +ADD_CUSTOM_COMMAND (TARGET rbdl_muscle_tests + POST_BUILD + COMMAND ./runMuscleTests + COMMENT "Running automated addon muscle tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc b/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc new file mode 100644 index 0000000..86e4316 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc @@ -0,0 +1,725 @@ +/* * + * TorqueMuscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + + +//============================================================================== +// INCLUDES +//============================================================================== + + + +#include "../Millard2016TorqueMuscle.h" +#include "../csvtools.h" +#include "../../geometry/tests/numericalTestFunctions.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; +/* + Constructor tests: + 1. Coefficients are copied over correctly. + 2. Curves are made correctly + + calcTorqueMuscleInfo test + stiffness calculation + power calculation + +*/ + +TEST(ConstructorRegularCallCheck) +{ + + + //Check that the 3 constructors when called properly + //do not abort + Millard2016TorqueMuscle test0 = Millard2016TorqueMuscle(); + + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + + Millard2016TorqueMuscle test2 = + Millard2016TorqueMuscle( + DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + 0.0, + 1.0, + 1.0, + "test_easyConstructor"); + + CHECK(abs( test2.getPassiveTorqueScale()-1.0) < TOL); + +} + + + +TEST(calcJointTorqueCorrectnessTests){ + + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double err = 0.0; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + + bool flagMakeTestVector = false; + if(flagMakeTestVector){ + Millard2016TorqueMuscle tqG = + Millard2016TorqueMuscle(DataSet::Gymnast, + subjectInfo, + Gymnast::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + TorqueMuscleInfo tmiG; + tqG.calcTorqueMuscleInfo(M_PI/3.0,0.1,0.77,tmiG); + + printf("%f\n",tmiG.fiberAngle); + printf("%f\n",tmiG.fiberAngularVelocity); + printf("%f\n",tmiG.activation); + printf("%f\n",tmiG.fiberTorque); + printf("%f\n",tmiG.fiberStiffness); + printf("%f\n",tmiG.fiberPassiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberActiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberActiveTorqueAngularVelocityMultiplier); + printf("%f\n",tmiG.fiberPassiveTorque); + printf("%f\n",tmiG.fiberActiveTorque); + printf("%f\n",tmiG.fiberDampingTorque); + printf("%f\n",tmiG.fiberNormDampingTorque); + printf("%f\n",tmiG.fiberActivePower); + printf("%f\n",tmiG.fiberPassivePower); + printf("%f\n",tmiG.fiberPower); + printf("%f\n",tmiG.DjointTorqueDjointAngle); + printf("%f\n",tmiG.DjointTorqueDjointAngularVelocity); + printf("%f\n",tmiG.DjointTorqueDactivation); + + } + + + //Zero out the passive forces so that calcMuscleTorque reports + //just the active force - this allows us to test its correctness. + tq.setPassiveTorqueScale(0.0); + + //Test that the get and set functions work for + //maximum isometric torque + double tauMaxOld = tq.getMaximumActiveIsometricTorque(); + double tauMax = tauMaxOld*10.0; + tq.setMaximumActiveIsometricTorque(tauMax); + CHECK(abs( tq.getMaximumActiveIsometricTorque()-tauMax) + < TOL ); + + //getParametersC1C2C3C4C5C6() has been removed and so this + //test is no longer possible. It is the responsibility of + //the underlying active-torque-angle curve to ensure that + //it peaks at 1.0 + /* + RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6 = + tq.getParametersC1C2C3C4C5C6(); + double thetaAtTauMax = c1c2c3c4c5c6[2]; + */ + //TorqueMuscleInfo tqInfo; + + //getParametersC1C2C3C4C5C6() has been removed. It is the + //responsibility of the underlying curve test code to + //check for correctness. + //double torque = tq.calcJointTorque(thetaAtTauMax,0.0,1.0); + //err = abs(torque -tauMax); + //CHECK(err< TOL); + + //These tests have been updated because Anderson + //torque velocity curve had to be replaced because it + //can behave badly (e.g. the concentric curve of the ankle + //extensors of a young man does not interest the x axis.) + // + //The new curves do not pass exactly through the points + //0.5*tauMax and 0.75*tauMax, but within 5% of this + //value. + + //getParametersC1C2C3C4C5C6() has been removed. It is the + //responsibility of the underlying curve test code to + //check for correctness. + //double omegaAt75TauMax = c1c2c3c4c5c6[3]; + //torque = tq.calcJointTorque(thetaAtTauMax,omegaAt75TauMax,1.0); + //err = abs(torque - 0.75*tauMax)/tauMax; + //CHECK( err < 0.05); + + //double omegaAt50TauMax = c1c2c3c4c5c6[4]; + //torque = tq.calcJointTorque(thetaAtTauMax,omegaAt50TauMax,1.0); + //err = abs(torque -0.50*tauMax)/tauMax; + //CHECK(err < 0.05); + + +} + +TEST(calcTorqueMuscleInfoCorrectnessTests){ + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double signOfJointVelocity = signOfJointTorque; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Female; + subjectInfo.ageGroup = AgeGroupSet::SeniorOver65; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + double jointAngle = 0.; + double jointVelocity = 0.; + double activation = 1.0; + + tq.setPassiveTorqueScale(0.5); + tq.calcJointTorque(0,0,1.0); + tq.setPassiveTorqueScale(1.0); + tq.calcJointTorque(0,0,1.0); + + double tauMax = tq.getMaximumActiveIsometricTorque(); + //RigidBodyDynamics::Math::VectorNd c1c2c3c4c5c6 = + // tq.getParametersC1C2C3C4C5C6(); + + //RigidBodyDynamics::Math::VectorNd b1k1b2k2 = + // tq.getParametersB1K1B2K2(); + + /* + int idx = -1; + if(b1k1b2k2[0] > 0){ + idx = 0; + }else if(b1k1b2k2[2] > 0){ + idx = 2; + } + */ + + /* + double b = b1k1b2k2[idx]; + double k = b1k1b2k2[idx+1]; + double thetaAtPassiveTauMax = log(tauMax/b)/k; + + double thetaAtTauMax = c1c2c3c4c5c6[2]; + double omegaAt75TauMax = c1c2c3c4c5c6[3]; + double omegaAt50TauMax = c1c2c3c4c5c6[4]; + + double jointAngleAtTauMax = + signOfJointAngle*thetaAtTauMax+jointAngleOffset; + */ + + double jointAngleAtTauMax = tq.getJointAngleAtMaximumActiveIsometricTorque(); + TorqueMuscleInfo tmi; + tq.calcTorqueMuscleInfo(jointAngleAtTauMax, + 0., + activation, + tmi); + + //Keypoint check: active force components + fiber kinematics + + CHECK(abs(tmi.activation-1) < EPSILON); + CHECK(abs(tmi.jointAngle-jointAngleAtTauMax)< TOL); + CHECK(abs(tmi.jointAngularVelocity-0.) < TOL); + + CHECK(abs(tmi.activation-1) < EPSILON); + //CHECK(abs(tmi.fiberAngle-thetaAtTauMax) < TOL); + CHECK(abs(tmi.fiberAngularVelocity-0.) < TOL); + + CHECK(abs(tmi.fiberActiveTorque - tauMax) < TOL); + CHECK(abs(tmi.fiberActiveTorqueAngleMultiplier-1.0) < TOL); + CHECK(abs(tmi.fiberActiveTorqueAngularVelocityMultiplier-1.0) muscleVector; + + bool exception = false; + + double angleTorqueSigns[][2] = {{-1, 1}, + {-1,-1}, + { 1,-1}, + { 1, 1}, + {-1, 1}, + {-1,-1}}; + + Millard2016TorqueMuscle tqMuscle; + std::stringstream tqName; + int tqIdx; + + for(int i=0; i < Anderson2007::LastJointTorque; ++i){ + + tqName.str(""); + tqName << DataSet.names[0] + < +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; +//using namespace OpenSim; +//using namespace SimTK; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + + + +TEST(tendonCurve) +{ + //cout <<"**************************************************"< +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + +TEST(Anderson2007ActiveTorqueAngleCurve) +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + + + //cout < tauMax){ + double tmp = tauMin; + tauMin = tauMax; + tauMax = tmp; + tauMinAngle = angleMax; + } + + CHECK( abs(tauMin) < TOL_SMALL); + CHECK( abs(tauMax - 1.0) < TOL_SMALL); + CHECK( abs(andersonTpCurve.calcDerivative(tauMinAngle,1)) < TOL_SMALL); + + //cout << " passed " << endl; + + //cout << " Continuity and Smoothness Testing " << endl; + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + andersonTpCurve, + andersonTpCurveSample, + TOL_DX); + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous(andersonTpCurve, + andersonTpCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(andersonTpCurveSample); + CHECK(curveIsMonotonic); + //cout << " passed " << endl; + + + } + + SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction(); + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + scale, + c1, + b1, + k1, + b2, + k2, + "test_passiveTorqueAngleCurve", + andersonTpCurve); + + if(FLAG_PLOT_CURVES){ + andersonTpCurve.printCurveToCSVFile( + FILE_PATH, + "anderson2007PassiveTorqueAngleCurve", + andersonTpCurve.getCurveDomain()[0]-0.1, + andersonTpCurve.getCurveDomain()[1]+0.1); + } + +} + +TEST(Anderson2007ActiveTorqueVelocityCurve) +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + //cout <= minEccentricMultiplier); + CHECK(maxTv <= maxEccentricMultiplier); + + CHECK(abs(andersonTvCurve.calcDerivative + (angularVelocityMax/angularVelocityMax,1)) wmaxC){ + CHECK( abs( tv.calcValue(w) ) < TOL_SMALL); + CHECK( abs( tv.calcDerivative(w,1) ) < TOL_SMALL); + CHECK( abs(tvX.calcDerivative(w,1) + - slopeAtConcentricOmegaMax ) < TOL_BIG); + }else if(w > 0 && w <= wmaxC){ + tvHill = (b*fiso-a*w)/(b+w); + errHill = abs(tv.calcValue(w)-tvHill); + //printf("%i. Err %f, ",i,errHill); + CHECK( errHill < 0.02); + errHill = abs(tvX.calcValue(w)-tvHill); + //printf(" Err %f\n",errHill); + CHECK( errHill < 0.02); + }else if(w < 0 & w > wmaxE){ + CHECK(tv.calcValue(w) > 1.0); + }else if(w < wmaxE){ + CHECK(abs( tv.calcValue(w) - tvAtEccentricOmegaMax ) < TOL_SMALL); + CHECK(abs( tv.calcDerivative(w,1) - 0.0 ) < TOL_SMALL); + //CHECK(abs( tvX.calcValue(w) - tvAtEccentricOmegaMax ) ); + CHECK(abs( tvX.calcDerivative(w,1) + - slopeAtEccentricOmegaMax ) < TOL_SMALL ); + } + } + + RigidBodyDynamics::Math::VectorNd curveDomain = tv.getCurveDomain(); + + double angularVelocityMin = curveDomain[0]; + double angularVelocityMax = curveDomain[1]; + + + RigidBodyDynamics::Math::MatrixNd tvCurveSample + = tv.calcSampledCurve( 6, + angularVelocityMin-0.1, + angularVelocityMax+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tv, + tvCurveSample, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous( tv, + tvCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tvCurveSample); + CHECK(curveIsMonotonic); + + if(FLAG_PLOT_CURVES){ + tv.printCurveToCSVFile( + FILE_PATH, + "millard2016TorqueVelocityCurve", + -1.1, + 1.1); + } + tv.printCurveToCSVFile( + "", + "millard2016TorqueVelocityCurve", + -1.1, + 1.1); + +} + +//============================================================================== +TEST(PassiveTorqueAngleCurve) +{ + SmoothSegmentedFunction tp = SmoothSegmentedFunction(); + SmoothSegmentedFunction tpX = SmoothSegmentedFunction(); + double angleAtZeroTorque0 = 0; + double angleAtOneNormTorque0 = -M_PI; + + double angleAtZeroTorque1 = 0; + double angleAtOneNormTorque1 = M_PI; + + double stiffnessAtOneNormTorque1 = + 5.6/(angleAtOneNormTorque1-angleAtZeroTorque1); + double stiffnessAtLowTorque1 = + 0.05*stiffnessAtOneNormTorque1; + double curviness1 = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque0, + angleAtOneNormTorque0, + curveName0, + tp); + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque1, + angleAtOneNormTorque1, + stiffnessAtLowTorque1, + stiffnessAtOneNormTorque1, + curviness1, + curveName1, + tpX); + + CHECK( abs(tp.calcValue(angleAtZeroTorque0)) < TOL_SMALL); + CHECK( abs(tp.calcValue(angleAtOneNormTorque0)-1.0) < TOL_SMALL); + + CHECK( abs(tpX.calcValue(angleAtZeroTorque1)) < TOL_SMALL); + CHECK( abs(tpX.calcValue(angleAtOneNormTorque1) - 1.0) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtZeroTorque1,1)) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtOneNormTorque1,1) + -stiffnessAtOneNormTorque1) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tp.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = tpX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd tpSample0 + = tp.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd tpSample1 + = tpX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tp, + tpSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tpX, + tpSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tp, + tpSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(tpX, + tpSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tpSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(tpSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tp.printCurveToCSVFile( + FILE_PATH, + "millard2016PassiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } +} + +//============================================================================== +TEST(ActiveTorqueAngleCurve) +{ + SmoothSegmentedFunction ta = SmoothSegmentedFunction(); + SmoothSegmentedFunction taX = SmoothSegmentedFunction(); + double angleAtOneNormTorque = 1.5; + double angularStandardDeviation = 1.0; + double angularStandardDeviationSq = + angularStandardDeviation*angularStandardDeviation; + + double minSlopeAtShoulders = 0.2; + double minValueAtShoulders = 0.1; + double xTrans = sqrt(-log(minValueAtShoulders)*2*angularStandardDeviationSq); + double delta = abs(xTrans+angleAtOneNormTorque); + double curviness = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + curveName0, + ta); + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + minSlopeAtShoulders, + minValueAtShoulders, + curviness, + curveName1, + taX); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque + +10.0*angularStandardDeviation)) < TOL_SMALL); + CHECK(abs(ta.calcValue(angleAtOneNormTorque + -10.0*angularStandardDeviation)) < TOL_SMALL); + + CHECK(abs(taX.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + double err = abs(taX.calcDerivative(angleAtOneNormTorque + delta,1) + + minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + err = abs(taX.calcDerivative(angleAtOneNormTorque - delta,1) + - minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + + + RigidBodyDynamics::Math::VectorNd curveDomain0 = ta.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = taX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd taSample0 + = ta.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd taSample1 + = taX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ta, + taSample0, + TOL_DX); + + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + taX, + taSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( ta, + taSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(taX, + taSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + if(FLAG_PLOT_CURVES){ + ta.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } + +} + +//============================================================================== +TEST(TendonTorqueAngleCurve) +{ + SmoothSegmentedFunction tt = SmoothSegmentedFunction(); + SmoothSegmentedFunction ttX = SmoothSegmentedFunction(); + + double angularStretchAtOneNormTorque = M_PI/2.0; + double stiffnessAtOneNormTorque = 2.5/angularStretchAtOneNormTorque; + double normTorqueAtToeEnd = 2.0/3.0; + double curviness = 0.5; + + std::string curveName0("ttTest0"); + std::string curveName1("ttTest1"); + + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + curveName0, + tt); + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + stiffnessAtOneNormTorque, + normTorqueAtToeEnd, + curviness, + curveName1, + ttX); + + CHECK(abs(tt.calcValue(0)) < TOL_SMALL); + CHECK(abs(ttX.calcValue(0)) < TOL_SMALL); + + CHECK(abs(tt.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + CHECK(abs(ttX.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ttX.calcDerivative(angularStretchAtOneNormTorque,1) + - stiffnessAtOneNormTorque) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tt.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = ttX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd ttSample0 + = tt.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd ttSample1 + = ttX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tt, + ttSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ttX, + ttSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tt, + ttSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(ttX, + ttSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(ttSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(ttSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tt.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + angularStretchAtOneNormTorque); + } + +} diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake new file mode 100644 index 0000000..7ce4e85 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake @@ -0,0 +1,88 @@ +# - Try to find URDF +# +# + +SET (URDF_FOUND FALSE) +SET (CONSOLE_BRIDGE_FOUND FALSE) +SET (URDFDOM_HEADERS_FOUND FALSE) +SET (URDFDOM_FOUND FALSE) + +FIND_PATH (CONSOLE_BRIDGE_DIR console_bridge/console.h + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (CONSOLE_BRIDGE_LIBRARY NAMES console_bridge PATHS + /usr/local/include + /usr/include + ) + +FIND_PATH (URDFDOM_HEADERS_DIR urdf_model/model.h + /usr/local/include + /usr/include + ) + +FIND_PATH (URDFDOM_DIR urdf_parser/urdf_parser.h + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (URDFDOM_MODEL_LIBRARY NAMES urdfdom_model PATHS + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (URDFDOM_WORLD_LIBRARY NAMES urdfdom_world PATHS + /usr/local/include + /usr/include + ) + +IF (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + MESSAGE(ERROR "Could not find URDF: console_bridge not found") +ELSE (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + SET (CONSOLE_BRIDGE_FOUND TRUE) +ENDIF (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + +IF (NOT URDFDOM_HEADERS_DIR) + MESSAGE(ERROR "Could not find URDF: urdfdom_headers not found") +ELSE (NOT URDFDOM_HEADERS_DIR) + SET (URDFDOM_HEADERS_FOUND TRUE) +ENDIF (NOT URDFDOM_HEADERS_DIR) + +IF (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + MESSAGE(ERROR "Could not find URDF: urdfdom_model or urdfdom_world library not found") +ELSE (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + SET (URDFDOM_FOUND TRUE) +ENDIF (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + +IF (CONSOLE_BRIDGE_FOUND AND URDFDOM_HEADERS_FOUND AND URDFDOM_FOUND) + SET (URDF_FOUND TRUE) +ENDIF (CONSOLE_BRIDGE_FOUND AND URDFDOM_HEADERS_FOUND AND URDFDOM_FOUND) + +IF (URDF_FOUND) + IF (NOT URDF_FIND_QUIETLY) + MESSAGE(STATUS "Found URDF console_bridge: ${CONSOLE_BRIDGE_LIBRARY}") + MESSAGE(STATUS "Found URDF urdfdom_headers: ${URDFDOM_HEADERS_DIR}") + MESSAGE(STATUS "Found URDF urdfdom: ${URDFDOM_LIBRARY}") + ENDIF (NOT URDF_FIND_QUIETLY) + SET (URDF_INCLUDE_DIRS + ${CONSOLE_BRIDGE_DIR} + ${URDFDOM_HEADERS_DIR} + ${URDFDOM_DIR} + ) + SET (URDF_LIBRARIES + ${CONSOLE_BRIDGE_LIBRARY} + ${URDFDOM_WORLD_LIBRARY} + ${URDFDOM_MODEL_LIBRARY} + ) + MESSAGE (STATUS "URDF Libraries: ${URDF_LIBRARIES}") +ELSE (URDF_FOUND) + IF (URDF_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find URDF") + ENDIF (URDF_FIND_REQUIRED) +ENDIF (URDF_FOUND) + +MARK_AS_ADVANCED ( + ${URDF_INCLUDE_DIRS} + ${URDF_LIBRARIES} + ) diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake new file mode 100644 index 0000000..113ca7b --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake @@ -0,0 +1,369 @@ +# Copyright (C) 2010 Thomas Moulard, JRL, CNRS/AIST. +# +# 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 3 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 program. If not, see . + +INCLUDE(CMake/shared-library.cmake) + +FIND_PACKAGE(PkgConfig) + +# Additional pkg-config variables whose value will be imported +# during the dependency check. +SET(PKG_CONFIG_ADDITIONAL_VARIABLES datarootdir pkgdatarootdir docdir doxygendocdir) + +# _SETUP_PROJECT_PKG_CONFIG +# ------------------------- +# +# Prepare pkg-config pc file generation step. +# +MACRO(_SETUP_PROJECT_PKG_CONFIG) + # Pkg-config related commands. + SET(PKG_CONFIG_PREFIX "${CMAKE_INSTALL_PREFIX}") + SET(PKG_CONFIG_EXEC_PREFIX "${PKG_CONFIG_PREFIX}") + SET(PKG_CONFIG_LIBDIR "${PKG_CONFIG_EXEC_PREFIX}/lib") + SET(PKG_CONFIG_INCLUDEDIR "${PKG_CONFIG_PREFIX}/include") + SET(PKG_CONFIG_DATAROOTDIR "${PKG_CONFIG_PREFIX}/share") + SET(PKG_CONFIG_PKGDATAROOTDIR "${PKG_CONFIG_PREFIX}/share/${PROJECT_NAME}") + SET(PKG_CONFIG_DOCDIR "${PKG_CONFIG_DATAROOTDIR}/doc/${PROJECT_NAME}") + SET(PKG_CONFIG_DOXYGENDOCDIR "${PKG_CONFIG_DOCDIR}/doxygen-html") + + SET(PKG_CONFIG_PROJECT_NAME "${PROJECT_NAME}") + SET(PKG_CONFIG_DESCRIPTION "${PROJECT_DESCRIPTION}") + SET(PKG_CONFIG_URL "${PROJECT_URL}") + SET(PKG_CONFIG_VERSION "${PROJECT_VERSION}") + SET(PKG_CONFIG_REQUIRES "") + SET(PKG_CONFIG_CONFLICTS "") + SET(PKG_CONFIG_LIBS "${LIBDIR_KW}${CMAKE_INSTALL_PREFIX}/lib") + SET(PKG_CONFIG_LIBS_PRIVATE "") + SET(PKG_CONFIG_CFLAGS "-I${CMAKE_INSTALL_PREFIX}/include") + + SET(PKG_CONFIG_EXTRA "") + + # Where to install the pkg-config file? + SET(PKG_CONFIG_DIR "${PKG_CONFIG_LIBDIR}/pkgconfig") + + # Watch variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES + PKG_CONFIG_FOUND + PKG_CONFIG_EXECUTABLE + PKG_CONFIG_PREFIX + PKG_CONFIG_EXEC_PREFIX + PKG_CONFIG_LIBDIR + PKG_CONFIG_INCLUDEDIR + PKG_CONFIG_DATAROOTDIR + PKG_CONFIG_PKGDATAROOTDIR + PKG_CONFIG_DOCDIR + PKG_CONFIG_DOXYGENDOCDIR + PKG_CONFIG_PROJECT_NAME + PKG_CONFIG_DESCRIPTION + PKG_CONFIG_URL + PKG_CONFIG_VERSION + PKG_CONFIG_REQUIRES + PKG_CONFIG_CONFLICTS + PKG_CONFIG_LIBS + PKG_CONFIG_LIBS_PRIVATE + PKG_CONFIG_CFLAGS + PKG_CONFIG_EXTRA + ) + + # Install it. + INSTALL( + FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc" + DESTINATION lib/pkgconfig + PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE) +ENDMACRO(_SETUP_PROJECT_PKG_CONFIG) + + +# _SETUP_PROJECT_PKG_CONFIG_FINALIZE +# ---------------------------------- +# +# Post-processing of the pkg-config step. +# +# The pkg-config file has to be generated at the end to allow end-user +# defined variables replacement. +# +MACRO(_SETUP_PROJECT_PKG_CONFIG_FINALIZE) + # Generate the pkg-config file. + CONFIGURE_FILE( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/pkg-config.pc.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc" + ) +ENDMACRO(_SETUP_PROJECT_PKG_CONFIG_FINALIZE) + + +# ADD_DEPENDENCY(PREFIX P_REQUIRED PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Fail if the package cannot +# be found. +# +# P_REQUIRED : if set to 1 the package is required, otherwise it consider +# as optional. +# WARNING for optional package: +# if the package is detected its compile +# and linking options are still put in the required fields +# of the generated pc file. Indeed from the binary viewpoint +# the package becomes required. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_DEPENDENCY P_REQUIRED PKG_CONFIG_STRING) + # Retrieve the left part of the equation to get package name. + STRING(REGEX MATCH "[^<>= ]+" LIBRARY_NAME "${PKG_CONFIG_STRING}") + # And transform it into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${LIBRARY_NAME}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Force redetection each time CMake is launched. + # Rationale: these values are *NEVER* manually set, so information is never + # lost by overriding them. Moreover, changes in the pkg-config files are + # not seen as long as the cache is not destroyed, even if the .pc file + # is changed. This is a BAD behavior. + SET(${PREFIX}_FOUND 0) + + # Search for the package. + IF(${P_REQUIRED}) + MESSAGE(STATUS "${PKG_CONFIG_STRING} is required.") + PKG_CHECK_MODULES("${PREFIX}" REQUIRED "${PKG_CONFIG_STRING}") + ELSE(${P_REQUIRED}) + MESSAGE(STATUS "${PKG_CONFIG_STRING} is optional.") + PKG_CHECK_MODULES("${PREFIX}" "${PKG_CONFIG_STRING}") + ENDIF(${P_REQUIRED}) + + # Watch variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES + ${PREFIX}_FOUND + ${PREFIX}_LIBRARIES + ${PREFIX}_LIBRARY_DIRS + ${PREFIX}_LDFLAGS + ${PREFIX}_LDFLAGS_OTHER + ${PREFIX}_INCLUDE_DIRS + ${PREFIX}_CFLAGS + ${PREFIX}_CFLAGS_OTHER + ${PREFIX} + ${PREFIX}_STATIC + ${PREFIX}_VERSION + ${PREFIX}_PREFIX + ${PREFIX}_INCLUDEDIR + ${PREFIX}_LIBDIR + ) + + # Get the values of additional variables. + FOREACH(VARIABLE ${PKG_CONFIG_ADDITIONAL_VARIABLES}) + # Upper-case version of the variable for CMake variable generation. + STRING(TOUPPER "${VARIABLE}" "VARIABLE_UC") + EXEC_PROGRAM( + "${PKG_CONFIG_EXECUTABLE}" ARGS + "--variable=${VARIABLE}" "${LIBRARY_NAME}" + OUTPUT_VARIABLE "${PREFIX}_${VARIABLE_UC}") + + # Watch additional variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES ${PREFIX}_${VARIABLE_UC}) + ENDFOREACH(VARIABLE) + + #FIXME: spaces are replaced by semi-colon by mistakes, revert the change. + #I cannot see why CMake is doing that... + STRING(REPLACE ";" " " PKG_CONFIG_STRING "${PKG_CONFIG_STRING}") + + # Add the package to the dependency list if found + IF(${${PREFIX}_FOUND}) + _ADD_TO_LIST(PKG_CONFIG_REQUIRES "${PKG_CONFIG_STRING}" ",") + ENDIF() + + # Add the package to the cmake dependency list + # if cpack has been included. + # This is likely to disappear when Ubuntu 8.04 will + # disappear. + IF(COMMAND ADD_CMAKE_DEPENDENCY) + ADD_CMAKE_DEPENDENCY(${PKG_CONFIG_STRING}) + ENDIF(COMMAND ADD_CMAKE_DEPENDENCY) + + IF(${${PREFIX}_FOUND}) + MESSAGE(STATUS + "Pkg-config module ${LIBRARY_NAME} v${${PREFIX}_VERSION}" + " has been detected with success.") + ENDIF() + +ENDMACRO(ADD_DEPENDENCY) + +# ADD_REQUIRED_DEPENDENCY(PREFIX PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Fail if the package cannot +# be found. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_REQUIRED_DEPENDENCY PKG_CONFIG_STRING) + ADD_DEPENDENCY(1 ${PKG_CONFIG_STRING}) +ENDMACRO(ADD_REQUIRED_DEPENDENCY) + +# ADD_OPTIONAL_DEPENDENCY(PREFIX PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Quiet if the package cannot +# be found. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_OPTIONAL_DEPENDENCY PKG_CONFIG_STRING) + ADD_DEPENDENCY(0 ${PKG_CONFIG_STRING}) +ENDMACRO(ADD_OPTIONAL_DEPENDENCY) + +# PKG_CONFIG_APPEND_LIBRARY_DIR +# ----------------------------- +# +# This macro adds library directories in a portable way +# into the CMake file. +MACRO(PKG_CONFIG_APPEND_LIBRARY_DIR DIRS) + FOREACH(DIR ${DIRS}) + IF(DIR) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIBDIR_KW}${DIR}") + ENDIF(DIR) + ENDFOREACH(DIR ${DIRS}) +ENDMACRO(PKG_CONFIG_APPEND_LIBRARY_DIR DIR) + + +# PKG_CONFIG_APPEND_CFLAGS +# ------------------------ +# +# This macro adds CFLAGS in a portable way into the pkg-config file. +# +MACRO(PKG_CONFIG_APPEND_CFLAGS FLAGS) + FOREACH(FLAG ${FLAGS}) + IF(FLAG) + SET(PKG_CONFIG_CFLAGS "${PKG_CONFIG_CFLAGS} ${FLAG}") + ENDIF(FLAG) + ENDFOREACH(FLAG ${FLAGS}) +ENDMACRO(PKG_CONFIG_APPEND_CFLAGS) + + +# PKG_CONFIG_APPEND_LIBS_RAW +# ---------------------------- +# +# This macro adds raw value in the "Libs:" into the pkg-config file. +# +# Exception for mac OS X: +# In addition to the classical static and dynamic libraries (handled like +# unix does), mac systems can link against frameworks. +# Frameworks are directories gathering headers, libraries, shared resources... +# +# The syntax used to link with a framework is particular, hence a filter is +# added to convert the absolute path to a framework (e.g. /Path/to/Sample.framework) +# into the correct flags (-F/Path/to/ -framework Sample). +# +MACRO(PKG_CONFIG_APPEND_LIBS_RAW LIBS) + FOREACH(LIB ${LIBS}) + IF(LIB) + IF( APPLE AND ${LIB} MATCHES .framework) + GET_FILENAME_COMPONENT(framework_PATH ${LIB} PATH) + GET_FILENAME_COMPONENT(framework_NAME ${LIB} NAME_WE) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} -F${framework_PATH} -framework ${framework_NAME}") + ELSE( APPLE AND ${LIB} MATCHES .framework) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIB}") + ENDIF( APPLE AND ${LIB} MATCHES .framework) + ENDIF(LIB) + ENDFOREACH(LIB ${LIBS}) + STRING(REPLACE "\n" "" PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS}") +ENDMACRO(PKG_CONFIG_APPEND_LIBS_RAW) + + +# PKG_CONFIG_APPEND_LIBS +# ---------------------- +# +# This macro adds libraries in a portable way into the pkg-config +# file. +# +# Library prefix and suffix is automatically added. +# +MACRO(PKG_CONFIG_APPEND_LIBS LIBS) + FOREACH(LIB ${LIBS}) + IF(LIB) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIBINCL_KW}${LIB}${LIB_EXT}") + ENDIF(LIB) + ENDFOREACH(LIB ${LIBS}) +ENDMACRO(PKG_CONFIG_APPEND_LIBS) + + +# PKG_CONFIG_USE_DEPENDENCY(TARGET DEPENDENCY) +# -------------------------------------------- +# +# This macro changes the target properties to properly search for +# headers, libraries and link against the required shared libraries +# when using a dependency detected through pkg-config. +# +# I.e. PKG_CONFIG_USE_DEPENDENCY(my-binary my-package) +# +MACRO(PKG_CONFIG_USE_DEPENDENCY TARGET DEPENDENCY) + # Transform the dependency into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${DEPENDENCY}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Make sure we search for a previously detected package. + IF(NOT DEFINED ${PREFIX}_FOUND) + MESSAGE(FATAL_ERROR + "The package ${DEPENDENCY} has not been detected correctly.\n" + "Have you called ADD_REQUIRED_DEPENDENCY/ADD_OPTIONAL_DEPENDENCY?") + ENDIF() + IF(NOT ${PREFIX}_FOUND) + MESSAGE(FATAL_ERROR + "The package ${DEPENDENCY} has not been found.") + ENDIF() + + # Make sure we do not override previous flags. + GET_TARGET_PROPERTY(CFLAGS ${TARGET} COMPILE_FLAGS) + GET_TARGET_PROPERTY(LDFLAGS ${TARGET} LINK_FLAGS) + + # If there were no previous flags, get rid of the XYFLAGS-NOTFOUND + # in the variables. + IF(NOT CFLAGS) + SET(CFLAGS "") + ENDIF() + IF(NOT LDFLAGS) + SET(LDFLAGS "") + ENDIF() + + # Transform semi-colon seperated list in to space separated list. + FOREACH(FLAG ${${PREFIX}_CFLAGS}) + SET(CFLAGS "${CFLAGS} ${FLAG}") + ENDFOREACH() + + FOREACH(FLAG ${${PREFIX}_LDFLAGS}) + SET(LDFLAGS "${LDFLAGS} ${FLAG}") + ENDFOREACH() + + # Update the flags. + SET_TARGET_PROPERTIES(${TARGET} + PROPERTIES COMPILE_FLAGS "${CFLAGS}" LINK_FLAGS "${LDFLAGS}") + + IF(UNIX AND NOT APPLE) + TARGET_LINK_LIBRARIES(${TARGET} ${${PREFIX}_LDFLAGS}) + TARGET_LINK_LIBRARIES(${TARGET} ${${PREFIX}_LDFLAGS_OTHER}) + ENDIF(UNIX AND NOT APPLE) + + # Include/libraries paths seems to be filtered on Linux, add paths + # again. + INCLUDE_DIRECTORIES(${${PREFIX}_INCLUDE_DIRS}) + LINK_DIRECTORIES(${${PREFIX}_LIBRARY_DIRS}) +ENDMACRO(PKG_CONFIG_USE_DEPENDENCY TARGET DEPENDENCY) + diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake new file mode 100644 index 0000000..2a7c358 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake @@ -0,0 +1,92 @@ +# This file was taken from: +# https://github.com/jrl-umi3218/jrl-cmakemodules/blob/master/ros.cmake + +MACRO(ADD_ROSPACK_DEPENDENCY PKG) + IF(PKG STREQUAL "") + MESSAGE(FATAL_ERROR "ADD_ROS_DEPENDENCY invalid call.") + ENDIF() + + # Transform package name into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${PKG}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + SET(${PREFIX}_FOUND 0) + + FIND_PROGRAM(ROSPACK rospack) + IF(NOT ROSPACK) + MESSAGE(FATAL_ERROR "failed to find the rospack binary. Is ROS installed?") + ENDIF() + + MESSAGE(STATUS "Looking for ${PKG} using rospack...") + EXEC_PROGRAM("${ROSPACK} find ${PKG}" OUTPUT_VARIABLE ${PKG}_ROS_PREFIX) + IF(NOT ${PKG}_ROS_PREFIX) + MESSAGE(FATAL_ERROR "Failed to detect ${PKG}.") + ENDIF() + + SET(${PREFIX}_FOUND 1) + EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=cflags -q ${PKG}" + OUTPUT_VARIABLE "${PREFIX}_CFLAGS") + EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=lflags -q ${PKG}" + OUTPUT_VARIABLE "${PREFIX}_LIBS") + + # Add flags to package pkg-config file. + PKG_CONFIG_APPEND_CFLAGS (${${PREFIX}_CFLAGS}) + PKG_CONFIG_APPEND_LIBS_RAW (${${PREFIX}_LIBS}) +ENDMACRO() + +MACRO(ROSPACK_USE_DEPENDENCY TARGET PKG) + IF(PKG STREQUAL "") + MESSAGE(FATAL_ERROR "ADD_ROS_DEPENDENCY invalid call.") + ENDIF() + + # Transform package name into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${PKG}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Make sure we do not override previous flags. + GET_TARGET_PROPERTY(CFLAGS "${TARGET}" COMPILE_FLAGS) + GET_TARGET_PROPERTY(LDFLAGS "${TARGET}" LINK_FLAGS) + + # If there were no previous flags, get rid of the XYFLAGS-NOTFOUND + # in the variables. + IF(NOT CFLAGS) + SET(CFLAGS "") + ENDIF() + IF(NOT LDFLAGS) + SET(LDFLAGS "") + ENDIF() + + # Transform semi-colon seperated list in to space separated list. + FOREACH(FLAG ${${PREFIX}_CFLAGS}) + SET(CFLAGS "${CFLAGS} ${FLAG}") + ENDFOREACH() + + FOREACH(FLAG ${${PREFIX}_LDFLAGS}) + SET(LDFLAGS "${LDFLAGS} ${FLAG}") + ENDFOREACH() + + # Filter out end of line in new flags. + STRING(REPLACE "\n" "" ${PREFIX}_CFLAGS "${${PREFIX}_CFLAGS}") + STRING(REPLACE "\n" "" ${PREFIX}_LIBS "${${PREFIX}_LIBS}") + + # Append new flags. + SET(CFLAGS "${CFLAGS} ${${PREFIX}_CFLAGS}") + SET(LDFLAGS "${LDFLAGS} ${${PREFIX}_LIBS}") + + # MESSAGE (STATUS "Linkerflags for ${TARGET}: ${LDFLAGS}") + + # Explicitly link against the shared object file + EXEC_PROGRAM("${ROSPACK} export find ${PKG}" + OUTPUT_VARIABLE "${PKG_FULL_PATH}") + + # SET (LDFLAGS "${LDFLAGS} ${PKG_FULL_PATH}/lib/lib${PKG}.so") + + # Update the flags. + SET_TARGET_PROPERTIES(${TARGET} + PROPERTIES COMPILE_FLAGS "${CFLAGS}" LINK_FLAGS "${LDFLAGS}") +ENDMACRO() + diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake new file mode 100644 index 0000000..c8dbbf5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake @@ -0,0 +1,28 @@ +# Copyright (C) 2010 Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST. +# +# 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 3 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 program. If not, see . + +# Shared library related constants +# (used for pkg-config file generation). +# FIXME: can't we get these information from CMake directly? +IF(WIN32) + SET(LIBDIR_KW "/LIBPATH:") + SET(LIBINCL_KW "") + SET(LIB_EXT ".lib") +ELSEIF(UNIX) + SET(LIBDIR_KW "-L") + SET(LIBINCL_KW "-l") + SET(LIB_EXT "") +ENDIF(WIN32) + diff --git a/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt b/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt new file mode 100644 index 0000000..f323753 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt @@ -0,0 +1,116 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +CMAKE_POLICY(SET CMP0048 NEW) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/ +) + +SET ( URDFREADER_SOURCES + urdfreader.cc + ) + +IF (DEFINED ENV{ROS_ROOT}) + MESSAGE (STATUS "ROS found: $ENV{ROS_ROOT}") + find_package(catkin REQUIRED COMPONENTS urdf) + OPTION (RBDL_USE_ROS_URDF_LIBRARY "Use the URDF library provided by ROS" ON) +ELSE () + SET (RBDL_USE_ROS_URDF_LIBRARY FALSE) +ENDIF () + +IF (RBDL_USE_ROS_URDF_LIBRARY) + # find_package(Boost REQUIRED COMPONENTS system) + SET (URDFREADER_DEPENDENCIES + rbdl + ${urdf_LIBRARIES} + # ${Boost_SYSTEM_LIBRARY} + ) +ELSE() + SET (URDFREADER_SOURCES + ${URDFREADER_SOURCES} + thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp + thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h + thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h + thirdparty/tinyxml/tinystr.cpp + thirdparty/tinyxml/tinyxml.cpp + thirdparty/tinyxml/tinyxmlerror.cpp + thirdparty/tinyxml/tinyxmlparser.cpp + thirdparty/urdf/boost_replacement/lexical_cast.h + thirdparty/urdf/boost_replacement/shared_ptr.h + thirdparty/urdf/boost_replacement/printf_console.cpp + thirdparty/urdf/boost_replacement/printf_console.h + thirdparty/urdf/boost_replacement/string_split.cpp + thirdparty/urdf/boost_replacement/string_split.h + ) + SET (URDFREADER_DEPENDENCIES + rbdl + ) +ENDIF() + +ADD_EXECUTABLE (rbdl_urdfreader_util rbdl_urdfreader_util.cc) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${URDFREADER_SOURCES} ) + + IF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES PREFIX "lib") + ENDIF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES OUTPUT_NAME "rbdl_urdfreader") + + TARGET_LINK_LIBRARIES (rbdl_urdfreader-static + rbdl-static + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader_util + rbdl_urdfreader-static + rbdl-static + ) + + INSTALL (TARGETS rbdl_urdfreader-static rbdl_urdfreader_util + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION lib + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_urdfreader SHARED ${URDFREADER_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_urdfreader PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader + ${URDFREADER_DEPENDENCIES} + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader_util + rbdl_urdfreader + ) + + INSTALL (TARGETS rbdl_urdfreader rbdl_urdfreader_util + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + ) + +INSTALL ( FILES ${headers} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/urdfreader + ) diff --git a/3rdparty/rbdl/addons/urdfreader/README.md b/3rdparty/rbdl/addons/urdfreader/README.md new file mode 100644 index 0000000..1a9de79 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/README.md @@ -0,0 +1,50 @@ +urdfreader - load models from (URDF Unified Robot Description Format) files +Copyright (c) 2012 Martin Felis + +Requirements +============ + +This addon depends on urdfdom to load and access the model data in the URDF +files. + +See https://github.com/ros/urdfdom for more details on how to +install urdfdom. + +Warning +======= + +This code is not properly tested as I do not have a proper urdf robot +model. If anyone has one and also some reference values that should come +out for the dynamics computations, please let me know. + +Licensing +========= + +This code is published under the zlib license, however some parts of the +CMake scripts are taken from other projects and are licensed under +different terms. + +Full license text: + +------- +urdfreader - load models from URDF (Unified Robot Description Format) files +Copyright (c) 2012-2015 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc b/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc new file mode 100644 index 0000000..7df3dbb --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc @@ -0,0 +1,69 @@ +#include +#include + +#include "urdfreader.h" + +#include + +using namespace std; + +bool verbose = false; +bool floatbase = false; +string filename = ""; + +void usage (const char* argv_0) { + cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] " << endl; + cerr << " -v | --verbose enable additional output" << endl; + cerr << " -d | --dof-overview print an overview of the degress of freedom" << endl; + cerr << " -m | --model-hierarchy print the hierarchy of the model" << endl; + cerr << " -h | --help print this help" << endl; + exit (1); +} + +int main (int argc, char *argv[]) { + if (argc < 2 || argc > 4) { + usage(argv[0]); + } + + bool verbose = false; + bool dof_overview = false; + bool model_hierarchy = false; + + string filename = argv[1]; + + for (int i = 1; i < argc; i++) { + if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose") + verbose = true; + else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview") + dof_overview = true; + else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy") + model_hierarchy = true; + else if (string(argv[i]) == "-f" || string (argv[i]) == "--floatbase") + floatbase = true; + else if (string(argv[i]) == "-h" || string (argv[i]) == "--help") + usage(argv[0]); + else + filename = argv[i]; + } + + RigidBodyDynamics::Model model; + + if (!RigidBodyDynamics::Addons::URDFReadFromFile(filename.c_str(), &model, floatbase, verbose)) { + cerr << "Loading of urdf model failed!" << endl; + return -1; + } + + cout << "Model loading successful!" << endl; + + if (dof_overview) { + cout << "Degree of freedom overview:" << endl; + cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model); + } + + if (model_hierarchy) { + cout << "Model Hierarchy:" << endl; + cout << RigidBodyDynamics::Utils::GetModelHierarchy (model); + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md b/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md new file mode 100644 index 0000000..cd7a0d9 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md @@ -0,0 +1,5 @@ +Code from the urdf directory was taken from the Bullet 3 source code +repository https://github.com/bulletphysics/bullet3 from the directory +examples/ThirdPartyLibs/urdf. + +The tinyxml code was obtained from http://sourceforge.net/projects/tinyxml/ diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt new file mode 100644 index 0000000..15b51bd --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt @@ -0,0 +1,299 @@ +Changes in version 1.0.1: +- Fixed comment tags which were outputing as ' include. Thanks + to Steve Lhomme for that. + +Changes in version 2.0.0 BETA +- Made the ToXXX() casts safe if 'this' is null. + When "LoadFile" is called with a filename, the value will correctly get set. + Thanks to Brian Yoder. +- Fixed bug where isalpha() and isalnum() would get called with a negative value for + high ascii numbers. Thanks to Alesky Aksenov. +- Fixed some errors codes that were not getting set. +- Made methods "const" that were not. +- Added a switch to enable or disable the ignoring of white space. ( TiXmlDocument::SetIgnoreWhiteSpace() ) +- Greater standardization and code re-use in the parser. +- Added a stream out operator. +- Added a stream in operator. +- Entity support, of predefined entites. &#x entities are untouched by input or output. +- Improved text out formatting. +- Fixed ReplaceChild bug, thanks to Tao Chen. + +Changes in version 2.0.1 +- Fixed hanging on loading a 0 length file. Thanks to Jeff Scozzafava. +- Fixed crashing on InsertBeforeChild and InsertAfterChild. Also possibility of bad links being + created by same function. Thanks to Frank De prins. +- Added missing licence text. Thanks to Lars Willemsens. +- Added include, at the suggestion of Steve Walters. + +Changes in version 2.1.0 +- Yves Berquin brings us the STL switch. The forum on SourceForge, and various emails to + me, have long debated all out STL vs. no STL at all. And now you can have it both ways. + TinyXml will compile either way. + +Changes in version 2.1.1 +- Compilation warnings. + +Changes in version 2.1.2 +- Uneeded code is not compiled in the STL case. +- Changed headers so that STL can be turned on or off in tinyxml.h + +Changes in version 2.1.3 +- Fixed non-const reference in API; now uses a pointer. +- Copy constructor of TiXmlString not checking for assignment to self. +- Nimrod Cohen found a truly evil bug in the STL implementation that occurs + when a string is converted to a c_str and then assigned to self. Search for + STL_STRING_BUG for a full description. I'm asserting this is a Microsoft STL + bug, since &string and string.c_str() should never be the same. Nevertheless, + the code works around it. +- Urivan Saaib pointed out a compiler conflict, where the C headers define + the isblank macro, which was wiping out the TiXmlString::isblank() method. + The method was unused and has been removed. + +Changes in version 2.1.4 +- Reworked the entity code. Entities were not correctly surving round trip input and output. + Will now automatically create entities for high ascii in output. + +Changes in version 2.1.5 +- Bug fix by kylotan : infinite loop on some input (tinyxmlparser.cpp rev 1.27) +- Contributed by Ivica Aracic (bytelord) : 1 new VC++ project to compile versions as static libraries (tinyxml_lib.dsp), + and an example usage in xmltest.dsp + (Patch request ID 678605) +- A suggestion by Ronald Fenner Jr (dormlock) to add #include and for Apple's Project Builder + (Patch request ID 697642) +- A patch from ohommes that allows to parse correctly dots in element names and attribute names + (Patch request 602600 and kylotan 701728) +- A patch from hermitgeek ( James ) and wasteland for improper error reporting +- Reviewed by Lee, with the following changes: + - Got sick of fighting the STL/non-STL thing in the windows build. Broke + them out as seperate projects. + - I have too long not included the dsw. Added. + - TinyXmlText had a protected Print. Odd. + - Made LinkEndChild public, with docs and appropriate warnings. + - Updated the docs. + +2.2.0 +- Fixed an uninitialized pointer in the TiXmlAttributes +- Fixed STL compilation problem in MinGW (and gcc 3?) - thanks Brian Yoder for finding this one +- Fixed a syntax error in TiXmlDeclaration - thanks Brian Yoder +- Fletcher Dunn proposed and submitted new error handling that tracked the row and column. Lee + modified it to not have performance impact. +- General cleanup suggestions from Fletcher Dunn. +- In error handling, general errors will no longer clear the error state of specific ones. +- Fix error in documentation : comments starting with ">) has now + been fixed. + +2.5.2 +- Lieven, and others, pointed out a missing const-cast that upset the Open Watcom compiler. + Should now be fixed. +- ErrorRow and ErrorCol should have been const, and weren't. Fixed thanks to Dmitry Polutov. + +2.5.3 +- zloe_zlo identified a missing string specialization for QueryValueAttribute() [ 1695429 ]. Worked + on this bug, but not sure how to fix it in a safe, cross-compiler way. +- increased warning level to 4 and turned on detect 64 bit portability issues for VC2005. + May address [ 1677737 ] VS2005: /Wp64 warnings +- grosheck identified several problems with the Document copy. Many thanks for [ 1660367 ] +- Nice catch, and suggested fix, be Gilad Novik on the Printer dropping entities. + "[ 1600650 ] Bug when printing xml text" is now fixed. +- A subtle fix from Nicos Gollan in the tinystring initializer: + [ 1581449 ] Fix initialiser of TiXmlString::nullrep_ +- Great catch, although there isn't a submitter for the bug. [ 1475201 ] TinyXML parses entities in comments. + Comments should not, in fact, parse entities. Fixed the code path and added tests. +- We were not catching all the returns from ftell. Thanks to Bernard for catching that. + +2.5.4 +- A TiXMLDocument can't be a sub-node. Block this from happening in the 'replace'. Thanks Noam. +- [ 1714831 ] TiXmlBase::location is not copied by copy-ctors, fix reported and suggested by Nicola Civran. +- Fixed possible memory overrun in the comment reading code - thanks gcarlton77 + +2.5.5 +- Alex van der Wal spotted incorrect types (lf) being used in print and scan. robertnestor pointed out some problems with the simple solution. Types updated. +- Johannes Hillert pointed out some bug typos. +- Christian Mueller identified inconsistent error handling with Attributes. +- olivier barthelemy also reported a problem with double truncation, also related to the %lf issue. +- zaelsius came up with a great (and simple) suggestion to fix QueryValueAttribute truncating strings. +- added some null pointer checks suggested by hansenk +- Sami Väisänen found a (rare) buffer overrun that could occur in parsing. +- vi tri filed a bug that led to a refactoring of the attribute setting mess (as well as adding a missing SetDoubleAttribute() ) +- removed TIXML_ERROR_OUT_OF_MEMORY. TinyXML does not systematically address OOO, and the notion it does is misleading. +- vanneto, keithmarshall, others all reported the warning from IsWhiteSpace() usage. Cleaned this up - many thanks to everyone who reported this one. +- tibur found a bug in end tag parsing + + +2.6.2 +- Switched over to VC 2010 +- Fixed up all the build issues arising from that. (Lots of latent build problems.) +- Removed the old, now unmaintained and likely not working, build files. +- Fixed some static analysis issues reported by orbitcowboy from cppcheck. +- Bayard 95 sent in analysis from a different analyzer - fixes applied from that as well. +- Tim Kosse sent a patch fixing an infinite loop. +- Ma Anguo identified a doc issue. +- Eddie Cohen identified a missing qualifier resulting in a compilation error on some systems. +- Fixed a line ending bug. (What year is this? Can we all agree on a format for text files? Please? ...oh well.) + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt new file mode 100644 index 0000000..89d9e8d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt @@ -0,0 +1,530 @@ +/** @mainpage + +

TinyXML

+ +TinyXML is a simple, small, C++ XML parser that can be easily +integrated into other programs. + +

What it does.

+ +In brief, TinyXML parses an XML document, and builds from that a +Document Object Model (DOM) that can be read, modified, and saved. + +XML stands for "eXtensible Markup Language." It allows you to create +your own document markups. Where HTML does a very good job of marking +documents for browsers, XML allows you to define any kind of document +markup, for example a document that describes a "to do" list for an +organizer application. XML is a very structured and convenient format. +All those random file formats created to store application data can +all be replaced with XML. One parser for everything. + +The best place for the complete, correct, and quite frankly hard to +read spec is at +http://www.w3.org/TR/2004/REC-xml-20040204/. An intro to XML +(that I really like) can be found at +http://skew.org/xml/tutorial. + +There are different ways to access and interact with XML data. +TinyXML uses a Document Object Model (DOM), meaning the XML data is parsed +into a C++ objects that can be browsed and manipulated, and then +written to disk or another output stream. You can also construct an XML document +from scratch with C++ objects and write this to disk or another output +stream. + +TinyXML is designed to be easy and fast to learn. It is two headers +and four cpp files. Simply add these to your project and off you go. +There is an example file - xmltest.cpp - to get you started. + +TinyXML is released under the ZLib license, +so you can use it in open source or commercial code. The details +of the license are at the top of every source file. + +TinyXML attempts to be a flexible parser, but with truly correct and +compliant XML output. TinyXML should compile on any reasonably C++ +compliant system. It does not rely on exceptions or RTTI. It can be +compiled with or without STL support. TinyXML fully supports +the UTF-8 encoding, and the first 64k character entities. + + +

What it doesn't do.

+ +TinyXML doesn't parse or use DTDs (Document Type Definitions) or XSLs +(eXtensible Stylesheet Language.) There are other parsers out there +(check out www.sourceforge.org, search for XML) that are much more fully +featured. But they are also much bigger, take longer to set up in +your project, have a higher learning curve, and often have a more +restrictive license. If you are working with browsers or have more +complete XML needs, TinyXML is not the parser for you. + +The following DTD syntax will not parse at this time in TinyXML: + +@verbatim + + ]> +@endverbatim + +because TinyXML sees this as a !DOCTYPE node with an illegally +embedded !ELEMENT node. This may be addressed in the future. + +

Tutorials.

+ +For the impatient, here is a tutorial to get you going. A great way to get started, +but it is worth your time to read this (very short) manual completely. + +- @subpage tutorial0 + +

Code Status.

+ +TinyXML is mature, tested code. It is very stable. If you find +bugs, please file a bug report on the sourceforge web site +(www.sourceforge.net/projects/tinyxml). We'll get them straightened +out as soon as possible. + +There are some areas of improvement; please check sourceforge if you are +interested in working on TinyXML. + +

Related Projects

+ +TinyXML projects you may find useful! (Descriptions provided by the projects.) + +
    +
  • TinyXPath (http://tinyxpath.sourceforge.net). TinyXPath is a small footprint + XPath syntax decoder, written in C++.
    • +
  • TinyXML++ (http://code.google.com/p/ticpp/). TinyXML++ is a completely new + interface to TinyXML that uses MANY of the C++ strengths. Templates, + exceptions, and much better error handling.
    • +
+ +

Features

+ +

Using STL

+ +TinyXML can be compiled to use or not use STL. When using STL, TinyXML +uses the std::string class, and fully supports std::istream, std::ostream, +operator<<, and operator>>. Many API methods have both 'const char*' and +'const std::string&' forms. + +When STL support is compiled out, no STL files are included whatsoever. All +the string classes are implemented by TinyXML itself. API methods +all use the 'const char*' form for input. + +Use the compile time #define: + + TIXML_USE_STL + +to compile one version or the other. This can be passed by the compiler, +or set as the first line of "tinyxml.h". + +Note: If compiling the test code in Linux, setting the environment +variable TINYXML_USE_STL=YES/NO will control STL compilation. In the +Windows project file, STL and non STL targets are provided. In your project, +It's probably easiest to add the line "#define TIXML_USE_STL" as the first +line of tinyxml.h. + +

UTF-8

+ +TinyXML supports UTF-8 allowing to manipulate XML files in any language. TinyXML +also supports "legacy mode" - the encoding used before UTF-8 support and +probably best described as "extended ascii". + +Normally, TinyXML will try to detect the correct encoding and use it. However, +by setting the value of TIXML_DEFAULT_ENCODING in the header file, TinyXML +can be forced to always use one encoding. + +TinyXML will assume Legacy Mode until one of the following occurs: +
    +
  1. If the non-standard but common "UTF-8 lead bytes" (0xef 0xbb 0xbf) + begin the file or data stream, TinyXML will read it as UTF-8.
    2. +
  2. If the declaration tag is read, and it has an encoding="UTF-8", then + TinyXML will read it as UTF-8.
    3. +
  3. If the declaration tag is read, and it has no encoding specified, then TinyXML will + read it as UTF-8.
    4. +
  4. If the declaration tag is read, and it has an encoding="something else", then TinyXML + will read it as Legacy Mode. In legacy mode, TinyXML will work as it did before. It's + not clear what that mode does exactly, but old content should keep working.
    5. +
  5. Until one of the above criteria is met, TinyXML runs in Legacy Mode.
    6. +
+ +What happens if the encoding is incorrectly set or detected? TinyXML will try +to read and pass through text seen as improperly encoded. You may get some strange results or +mangled characters. You may want to force TinyXML to the correct mode. + +You may force TinyXML to Legacy Mode by using LoadFile( TIXML_ENCODING_LEGACY ) or +LoadFile( filename, TIXML_ENCODING_LEGACY ). You may force it to use legacy mode all +the time by setting TIXML_DEFAULT_ENCODING = TIXML_ENCODING_LEGACY. Likewise, you may +force it to TIXML_ENCODING_UTF8 with the same technique. + +For English users, using English XML, UTF-8 is the same as low-ASCII. You +don't need to be aware of UTF-8 or change your code in any way. You can think +of UTF-8 as a "superset" of ASCII. + +UTF-8 is not a double byte format - but it is a standard encoding of Unicode! +TinyXML does not use or directly support wchar, TCHAR, or Microsoft's _UNICODE at this time. +It is common to see the term "Unicode" improperly refer to UTF-16, a wide byte encoding +of unicode. This is a source of confusion. + +For "high-ascii" languages - everything not English, pretty much - TinyXML can +handle all languages, at the same time, as long as the XML is encoded +in UTF-8. That can be a little tricky, older programs and operating systems +tend to use the "default" or "traditional" code page. Many apps (and almost all +modern ones) can output UTF-8, but older or stubborn (or just broken) ones +still output text in the default code page. + +For example, Japanese systems traditionally use SHIFT-JIS encoding. +Text encoded as SHIFT-JIS can not be read by TinyXML. +A good text editor can import SHIFT-JIS and then save as UTF-8. + +The Skew.org link does a great +job covering the encoding issue. + +The test file "utf8test.xml" is an XML containing English, Spanish, Russian, +and Simplified Chinese. (Hopefully they are translated correctly). The file +"utf8test.gif" is a screen capture of the XML file, rendered in IE. Note that +if you don't have the correct fonts (Simplified Chinese or Russian) on your +system, you won't see output that matches the GIF file even if you can parse +it correctly. Also note that (at least on my Windows machine) console output +is in a Western code page, so that Print() or printf() cannot correctly display +the file. This is not a bug in TinyXML - just an OS issue. No data is lost or +destroyed by TinyXML. The console just doesn't render UTF-8. + + +

Entities

+TinyXML recognizes the pre-defined "character entities", meaning special +characters. Namely: + +@verbatim + & & + < < + > > + " " + ' ' +@endverbatim + +These are recognized when the XML document is read, and translated to there +UTF-8 equivalents. For instance, text with the XML of: + +@verbatim + Far & Away +@endverbatim + +will have the Value() of "Far & Away" when queried from the TiXmlText object, +and will be written back to the XML stream/file as an ampersand. Older versions +of TinyXML "preserved" character entities, but the newer versions will translate +them into characters. + +Additionally, any character can be specified by its Unicode code point: +The syntax " " or " " are both to the non-breaking space characher. + +

Printing

+TinyXML can print output in several different ways that all have strengths and limitations. + +- Print( FILE* ). Output to a std-C stream, which includes all C files as well as stdout. + - "Pretty prints", but you don't have control over printing options. + - The output is streamed directly to the FILE object, so there is no memory overhead + in the TinyXML code. + - used by Print() and SaveFile() + +- operator<<. Output to a c++ stream. + - Integrates with standart C++ iostreams. + - Outputs in "network printing" mode without line breaks. Good for network transmission + and moving XML between C++ objects, but hard for a human to read. + +- TiXmlPrinter. Output to a std::string or memory buffer. + - API is less concise + - Future printing options will be put here. + - Printing may change slightly in future versions as it is refined and expanded. + +

Streams

+With TIXML_USE_STL on TinyXML supports C++ streams (operator <<,>>) streams as well +as C (FILE*) streams. There are some differences that you may need to be aware of. + +C style output: + - based on FILE* + - the Print() and SaveFile() methods + + Generates formatted output, with plenty of white space, intended to be as + human-readable as possible. They are very fast, and tolerant of ill formed + XML documents. For example, an XML document that contains 2 root elements + and 2 declarations, will still print. + +C style input: + - based on FILE* + - the Parse() and LoadFile() methods + + A fast, tolerant read. Use whenever you don't need the C++ streams. + +C++ style output: + - based on std::ostream + - operator<< + + Generates condensed output, intended for network transmission rather than + readability. Depending on your system's implementation of the ostream class, + these may be somewhat slower. (Or may not.) Not tolerant of ill formed XML: + a document should contain the correct one root element. Additional root level + elements will not be streamed out. + +C++ style input: + - based on std::istream + - operator>> + + Reads XML from a stream, making it useful for network transmission. The tricky + part is knowing when the XML document is complete, since there will almost + certainly be other data in the stream. TinyXML will assume the XML data is + complete after it reads the root element. Put another way, documents that + are ill-constructed with more than one root element will not read correctly. + Also note that operator>> is somewhat slower than Parse, due to both + implementation of the STL and limitations of TinyXML. + +

White space

+The world simply does not agree on whether white space should be kept, or condensed. +For example, pretend the '_' is a space, and look at "Hello____world". HTML, and +at least some XML parsers, will interpret this as "Hello_world". They condense white +space. Some XML parsers do not, and will leave it as "Hello____world". (Remember +to keep pretending the _ is a space.) Others suggest that __Hello___world__ should become +Hello___world. + +It's an issue that hasn't been resolved to my satisfaction. TinyXML supports the +first 2 approaches. Call TiXmlBase::SetCondenseWhiteSpace( bool ) to set the desired behavior. +The default is to condense white space. + +If you change the default, you should call TiXmlBase::SetCondenseWhiteSpace( bool ) +before making any calls to Parse XML data, and I don't recommend changing it after +it has been set. + + +

Handles

+ +Where browsing an XML document in a robust way, it is important to check +for null returns from method calls. An error safe implementation can +generate a lot of code like: + +@verbatim +TiXmlElement* root = document.FirstChildElement( "Document" ); +if ( root ) +{ + TiXmlElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + TiXmlElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + TiXmlElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. +@endverbatim + +Handles have been introduced to clean this up. Using the TiXmlHandle class, +the previous code reduces to: + +@verbatim +TiXmlHandle docHandle( &document ); +TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); +if ( child2 ) +{ + // do something useful +@endverbatim + +Which is much easier to deal with. See TiXmlHandle for more information. + + +

Row and Column tracking

+Being able to track nodes and attributes back to their origin location +in source files can be very important for some applications. Additionally, +knowing where parsing errors occured in the original source can be very +time saving. + +TinyXML can tracks the row and column origin of all nodes and attributes +in a text file. The TiXmlBase::Row() and TiXmlBase::Column() methods return +the origin of the node in the source text. The correct tabs can be +configured in TiXmlDocument::SetTabSize(). + + +

Using and Installing

+ +To Compile and Run xmltest: + +A Linux Makefile and a Windows Visual C++ .dsw file is provided. +Simply compile and run. It will write the file demotest.xml to your +disk and generate output on the screen. It also tests walking the +DOM by printing out the number of nodes found using different +techniques. + +The Linux makefile is very generic and runs on many systems - it +is currently tested on mingw and +MacOSX. You do not need to run 'make depend'. The dependecies have been +hard coded. + +

Windows project file for VC6

+
    +
  • tinyxml: tinyxml library, non-STL
    • +
  • tinyxmlSTL: tinyxml library, STL
    • +
  • tinyXmlTest: test app, non-STL
    • +
  • tinyXmlTestSTL: test app, STL
    • +
+ +

Makefile

+At the top of the makefile you can set: + +PROFILE, DEBUG, and TINYXML_USE_STL. Details (such that they are) are in +the makefile. + +In the tinyxml directory, type "make clean" then "make". The executable +file 'xmltest' will be created. + + + +

To Use in an Application:

+ +Add tinyxml.cpp, tinyxml.h, tinyxmlerror.cpp, tinyxmlparser.cpp, tinystr.cpp, and tinystr.h to your +project or make file. That's it! It should compile on any reasonably +compliant C++ system. You do not need to enable exceptions or +RTTI for TinyXML. + + +

How TinyXML works.

+ +An example is probably the best way to go. Take: +@verbatim + + + + Go to the Toy store! + Do bills + +@endverbatim + +Its not much of a To Do list, but it will do. To read this file +(say "demo.xml") you would create a document, and parse it in: +@verbatim + TiXmlDocument doc( "demo.xml" ); + doc.LoadFile(); +@endverbatim + +And its ready to go. Now lets look at some lines and how they +relate to the DOM. + +@verbatim + +@endverbatim + + The first line is a declaration, and gets turned into the + TiXmlDeclaration class. It will be the first child of the + document node. + + This is the only directive/special tag parsed by TinyXML. + Generally directive tags are stored in TiXmlUnknown so the + commands wont be lost when it is saved back to disk. + +@verbatim + +@endverbatim + + A comment. Will become a TiXmlComment object. + +@verbatim + +@endverbatim + + The "ToDo" tag defines a TiXmlElement object. This one does not have + any attributes, but does contain 2 other elements. + +@verbatim + +@endverbatim + + Creates another TiXmlElement which is a child of the "ToDo" element. + This element has 1 attribute, with the name "priority" and the value + "1". + +@verbatim +Go to the +@endverbatim + + A TiXmlText. This is a leaf node and cannot contain other nodes. + It is a child of the "Item" TiXmlElement. + +@verbatim + +@endverbatim + + + Another TiXmlElement, this one a child of the "Item" element. + +Etc. + +Looking at the entire object tree, you end up with: +@verbatim +TiXmlDocument "demo.xml" + TiXmlDeclaration "version='1.0'" "standalone=no" + TiXmlComment " Our to do list data" + TiXmlElement "ToDo" + TiXmlElement "Item" Attribtutes: priority = 1 + TiXmlText "Go to the " + TiXmlElement "bold" + TiXmlText "Toy store!" + TiXmlElement "Item" Attributes: priority=2 + TiXmlText "Do bills" +@endverbatim + +

Documentation

+ +The documentation is build with Doxygen, using the 'dox' +configuration file. + +

License

+ +TinyXML is released under the zlib license: + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +

References

+ +The World Wide Web Consortium is the definitive standard body for +XML, and their web pages contain huge amounts of information. + +The definitive spec: +http://www.w3.org/TR/2004/REC-xml-20040204/ + +I also recommend "XML Pocket Reference" by Robert Eckstein and published by +OReilly...the book that got the whole thing started. + +

Contributors, Contacts, and a Brief History

+ +Thanks very much to everyone who sends suggestions, bugs, ideas, and +encouragement. It all helps, and makes this project fun. A special thanks +to the contributors on the web pages that keep it lively. + +So many people have sent in bugs and ideas, that rather than list here +we try to give credit due in the "changes.txt" file. + +TinyXML was originally written by Lee Thomason. (Often the "I" still +in the documentation.) Lee reviews changes and releases new versions, +with the help of Yves Berquin, Andrew Ellerton, and the tinyXml community. + +We appreciate your suggestions, and would love to know if you +use TinyXML. Hopefully you will enjoy it and find it useful. +Please post questions, comments, file bugs, or contact us at: + +www.sourceforge.net/projects/tinyxml + +Lee Thomason, Yves Berquin, Andrew Ellerton +*/ diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp new file mode 100644 index 0000000..0665768 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp @@ -0,0 +1,111 @@ +/* +www.sourceforge.net/projects/tinyxml + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TIXML_USE_STL + +#include "tinystr.h" + +// Error value for find primitive +const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1); + + +// Null rep. +TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; + + +void TiXmlString::reserve (size_type cap) +{ + if (cap > capacity()) + { + TiXmlString tmp; + tmp.init(length(), cap); + memcpy(tmp.start(), data(), length()); + swap(tmp); + } +} + + +TiXmlString& TiXmlString::assign(const char* str, size_type len) +{ + size_type cap = capacity(); + if (len > cap || cap > 3*(len + 8)) + { + TiXmlString tmp; + tmp.init(len); + memcpy(tmp.start(), str, len); + swap(tmp); + } + else + { + memmove(start(), str, len); + set_size(len); + } + return *this; +} + + +TiXmlString& TiXmlString::append(const char* str, size_type len) +{ + size_type newsize = length() + len; + if (newsize > capacity()) + { + reserve (newsize + capacity()); + } + memmove(finish(), str, len); + set_size(newsize); + return *this; +} + + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) +{ + TiXmlString tmp; + tmp.reserve(a.length() + b.length()); + tmp += a; + tmp += b; + return tmp; +} + +TiXmlString operator + (const TiXmlString & a, const char* b) +{ + TiXmlString tmp; + TiXmlString::size_type b_len = static_cast( strlen(b) ); + tmp.reserve(a.length() + b_len); + tmp += a; + tmp.append(b, b_len); + return tmp; +} + +TiXmlString operator + (const char* a, const TiXmlString & b) +{ + TiXmlString tmp; + TiXmlString::size_type a_len = static_cast( strlen(a) ); + tmp.reserve(a_len + b.length()); + tmp.append(a, a_len); + tmp += b; + return tmp; +} + + +#endif // TIXML_USE_STL diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h new file mode 100644 index 0000000..89cca33 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h @@ -0,0 +1,305 @@ +/* +www.sourceforge.net/projects/tinyxml + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TIXML_USE_STL + +#ifndef TIXML_STRING_INCLUDED +#define TIXML_STRING_INCLUDED + +#include +#include + +/* The support for explicit isn't that universal, and it isn't really + required - it is used to check that the TiXmlString class isn't incorrectly + used. Be nice to old compilers and macro it here: +*/ +#if defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + #define TIXML_EXPLICIT explicit +#elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + #define TIXML_EXPLICIT explicit +#else + #define TIXML_EXPLICIT +#endif + + +/* + TiXmlString is an emulation of a subset of the std::string template. + Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. + Only the member functions relevant to the TinyXML project have been implemented. + The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase + a string and there's no more room, we allocate a buffer twice as big as we need. +*/ +class TiXmlString +{ + public : + // The size type used + typedef size_t size_type; + + // Error value for find primitive + static const size_type npos; // = -1; + + + // TiXmlString empty constructor + TiXmlString () : rep_(&nullrep_) + { + } + + // TiXmlString copy constructor + TiXmlString ( const TiXmlString & copy) : rep_(0) + { + init(copy.length()); + memcpy(start(), copy.data(), length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) + { + init( static_cast( strlen(copy) )); + memcpy(start(), copy, length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) + { + init(len); + memcpy(start(), str, len); + } + + // TiXmlString destructor + ~TiXmlString () + { + quit(); + } + + TiXmlString& operator = (const char * copy) + { + return assign( copy, (size_type)strlen(copy)); + } + + TiXmlString& operator = (const TiXmlString & copy) + { + return assign(copy.start(), copy.length()); + } + + + // += operator. Maps to append + TiXmlString& operator += (const char * suffix) + { + return append(suffix, static_cast( strlen(suffix) )); + } + + // += operator. Maps to append + TiXmlString& operator += (char single) + { + return append(&single, 1); + } + + // += operator. Maps to append + TiXmlString& operator += (const TiXmlString & suffix) + { + return append(suffix.data(), suffix.length()); + } + + + // Convert a TiXmlString into a null-terminated char * + const char * c_str () const { return rep_->str; } + + // Convert a TiXmlString into a char * (need not be null terminated). + const char * data () const { return rep_->str; } + + // Return the length of a TiXmlString + size_type length () const { return rep_->size; } + + // Alias for length() + size_type size () const { return rep_->size; } + + // Checks if a TiXmlString is empty + bool empty () const { return rep_->size == 0; } + + // Return capacity of string + size_type capacity () const { return rep_->capacity; } + + + // single char extraction + const char& at (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // [] operator + char& operator [] (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // find a char in a string. Return TiXmlString::npos if not found + size_type find (char lookup) const + { + return find(lookup, 0); + } + + // find a char in a string from an offset. Return TiXmlString::npos if not found + size_type find (char tofind, size_type offset) const + { + if (offset >= length()) return npos; + + for (const char* p = c_str() + offset; *p != '\0'; ++p) + { + if (*p == tofind) return static_cast< size_type >( p - c_str() ); + } + return npos; + } + + void clear () + { + //Lee: + //The original was just too strange, though correct: + // TiXmlString().swap(*this); + //Instead use the quit & re-init: + quit(); + init(0,0); + } + + /* Function to reserve a big amount of data when we know we'll need it. Be aware that this + function DOES NOT clear the content of the TiXmlString if any exists. + */ + void reserve (size_type cap); + + TiXmlString& assign (const char* str, size_type len); + + TiXmlString& append (const char* str, size_type len); + + void swap (TiXmlString& other) + { + Rep* r = rep_; + rep_ = other.rep_; + other.rep_ = r; + } + + private: + + void init(size_type sz) { init(sz, sz); } + void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } + char* start() const { return rep_->str; } + char* finish() const { return rep_->str + rep_->size; } + + struct Rep + { + size_type size, capacity; + char str[1]; + }; + + void init(size_type sz, size_type cap) + { + if (cap) + { + // Lee: the original form: + // rep_ = static_cast(operator new(sizeof(Rep) + cap)); + // doesn't work in some cases of new being overloaded. Switching + // to the normal allocation, although use an 'int' for systems + // that are overly picky about structure alignment. + const size_type bytesNeeded = sizeof(Rep) + cap; + const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); + rep_ = reinterpret_cast( new int[ intsNeeded ] ); + + rep_->str[ rep_->size = sz ] = '\0'; + rep_->capacity = cap; + } + else + { + rep_ = &nullrep_; + } + } + + void quit() + { + if (rep_ != &nullrep_) + { + // The rep_ is really an array of ints. (see the allocator, above). + // Cast it back before delete, so the compiler won't incorrectly call destructors. + delete [] ( reinterpret_cast( rep_ ) ); + } + } + + Rep * rep_; + static Rep nullrep_; + +} ; + + +inline bool operator == (const TiXmlString & a, const TiXmlString & b) +{ + return ( a.length() == b.length() ) // optimization on some platforms + && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare +} +inline bool operator < (const TiXmlString & a, const TiXmlString & b) +{ + return strcmp(a.c_str(), b.c_str()) < 0; +} + +inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } +inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } +inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } +inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } + +inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } +inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } +inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } +inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); +TiXmlString operator + (const TiXmlString & a, const char* b); +TiXmlString operator + (const char* a, const TiXmlString & b); + + +/* + TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. + Only the operators that we need for TinyXML have been developped. +*/ +class TiXmlOutStream : public TiXmlString +{ +public : + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const TiXmlString & in) + { + *this += in; + return *this; + } + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const char * in) + { + *this += in; + return *this; + } + +} ; + +#endif // TIXML_STRING_INCLUDED +#endif // TIXML_USE_STL diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp new file mode 100644 index 0000000..9c161df --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp @@ -0,0 +1,1886 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include + +#ifdef TIXML_USE_STL +#include +#include +#endif + +#include "tinyxml.h" + +FILE* TiXmlFOpen( const char* filename, const char* mode ); + +bool TiXmlBase::condenseWhiteSpace = true; + +// Microsoft compiler security +FILE* TiXmlFOpen( const char* filename, const char* mode ) +{ + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + FILE* fp = 0; + errno_t err = fopen_s( &fp, filename, mode ); + if ( !err && fp ) + return fp; + return 0; + #else + return fopen( filename, mode ); + #endif +} + +void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) +{ + int i=0; + + while( i<(int)str.length() ) + { + unsigned char c = (unsigned char) str[i]; + + if ( c == '&' + && i < ( (int)str.length() - 2 ) + && str[i+1] == '#' + && str[i+2] == 'x' ) + { + // Hexadecimal character reference. + // Pass through unchanged. + // © -- copyright symbol, for example. + // + // The -1 is a bug fix from Rob Laveaux. It keeps + // an overflow from happening if there is no ';'. + // There are actually 2 ways to exit this loop - + // while fails (error case) and break (semicolon found). + // However, there is no mechanism (currently) for + // this function to return an error. + while ( i<(int)str.length()-1 ) + { + outString->append( str.c_str() + i, 1 ); + ++i; + if ( str[i] == ';' ) + break; + } + } + else if ( c == '&' ) + { + outString->append( entity[0].str, entity[0].strLength ); + ++i; + } + else if ( c == '<' ) + { + outString->append( entity[1].str, entity[1].strLength ); + ++i; + } + else if ( c == '>' ) + { + outString->append( entity[2].str, entity[2].strLength ); + ++i; + } + else if ( c == '\"' ) + { + outString->append( entity[3].str, entity[3].strLength ); + ++i; + } + else if ( c == '\'' ) + { + outString->append( entity[4].str, entity[4].strLength ); + ++i; + } + else if ( c < 32 ) + { + // Easy pass at non-alpha/numeric/symbol + // Below 32 is symbolic. + char buf[ 32 ]; + + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); + #else + sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); + #endif + + //*ME: warning C4267: convert 'size_t' to 'int' + //*ME: Int-Cast to make compiler happy ... + outString->append( buf, (int)strlen( buf ) ); + ++i; + } + else + { + //char realc = (char) c; + //outString->append( &realc, 1 ); + *outString += (char) c; // somewhat more efficient function call. + ++i; + } + } +} + + +TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() +{ + parent = 0; + type = _type; + firstChild = 0; + lastChild = 0; + prev = 0; + next = 0; +} + + +TiXmlNode::~TiXmlNode() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } +} + + +void TiXmlNode::CopyTo( TiXmlNode* target ) const +{ + target->SetValue (value.c_str() ); + target->userData = userData; + target->location = location; +} + + +void TiXmlNode::Clear() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } + + firstChild = 0; + lastChild = 0; +} + + +TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) +{ + assert( node->parent == 0 || node->parent == this ); + assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); + + if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + delete node; + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + node->parent = this; + + node->prev = lastChild; + node->next = 0; + + if ( lastChild ) + lastChild->next = node; + else + firstChild = node; // it was an empty list. + + lastChild = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) +{ + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + + return LinkEndChild( node ); +} + + +TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) +{ + if ( !beforeThis || beforeThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->next = beforeThis; + node->prev = beforeThis->prev; + if ( beforeThis->prev ) + { + beforeThis->prev->next = node; + } + else + { + assert( firstChild == beforeThis ); + firstChild = node; + } + beforeThis->prev = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) +{ + if ( !afterThis || afterThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->prev = afterThis; + node->next = afterThis->next; + if ( afterThis->next ) + { + afterThis->next->prev = node; + } + else + { + assert( lastChild == afterThis ); + lastChild = node; + } + afterThis->next = node; + return node; +} + + +TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) +{ + if ( !replaceThis ) + return 0; + + if ( replaceThis->parent != this ) + return 0; + + if ( withThis.ToDocument() ) { + // A document can never be a child. Thanks to Noam. + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = withThis.Clone(); + if ( !node ) + return 0; + + node->next = replaceThis->next; + node->prev = replaceThis->prev; + + if ( replaceThis->next ) + replaceThis->next->prev = node; + else + lastChild = node; + + if ( replaceThis->prev ) + replaceThis->prev->next = node; + else + firstChild = node; + + delete replaceThis; + node->parent = this; + return node; +} + + +bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) +{ + if ( !removeThis ) { + return false; + } + + if ( removeThis->parent != this ) + { + assert( 0 ); + return false; + } + + if ( removeThis->next ) + removeThis->next->prev = removeThis->prev; + else + lastChild = removeThis->prev; + + if ( removeThis->prev ) + removeThis->prev->next = removeThis->next; + else + firstChild = removeThis->next; + + delete removeThis; + return true; +} + +const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = firstChild; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = lastChild; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild(); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling(); + } +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild( val ); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling( val ); + } +} + + +const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = next; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = prev; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +void TiXmlElement::RemoveAttribute( const char * name ) +{ + #ifdef TIXML_USE_STL + TIXML_STRING str( name ); + TiXmlAttribute* node = attributeSet.Find( str ); + #else + TiXmlAttribute* node = attributeSet.Find( name ); + #endif + if ( node ) + { + attributeSet.Remove( node ); + delete node; + } +} + +const TiXmlElement* TiXmlNode::FirstChildElement() const +{ + const TiXmlNode* node; + + for ( node = FirstChild(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = FirstChild( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement() const +{ + const TiXmlNode* node; + + for ( node = NextSibling(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = NextSibling( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlDocument* TiXmlNode::GetDocument() const +{ + const TiXmlNode* node; + + for( node = this; node; node = node->parent ) + { + if ( node->ToDocument() ) + return node->ToDocument(); + } + return 0; +} + + +TiXmlElement::TiXmlElement (const char * _value) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} + + +#ifdef TIXML_USE_STL +TiXmlElement::TiXmlElement( const std::string& _value ) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} +#endif + + +TiXmlElement::TiXmlElement( const TiXmlElement& copy) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + copy.CopyTo( this ); +} + + +TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) +{ + ClearThis(); + base.CopyTo( this ); + return *this; +} + + +TiXmlElement::~TiXmlElement() +{ + ClearThis(); +} + + +void TiXmlElement::ClearThis() +{ + Clear(); + while( attributeSet.First() ) + { + TiXmlAttribute* node = attributeSet.First(); + attributeSet.Remove( node ); + delete node; + } +} + + +const char* TiXmlElement::Attribute( const char* name ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( node ) + return node->Value(); + return 0; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( attrib ) + return &attrib->ValueStr(); + return 0; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} +#endif + + +int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} + + +int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int ival = 0; + int result = node->QueryIntValue( &ival ); + *value = (unsigned)ival; + return result; +} + + +int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int result = TIXML_WRONG_TYPE; + if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = true; + result = TIXML_SUCCESS; + } + else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = false; + result = TIXML_SUCCESS; + } + return result; +} + + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} +#endif + + +int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} +#endif + + +void TiXmlElement::SetAttribute( const char * name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} +#endif + + +void TiXmlElement::SetDoubleAttribute( const char * name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} +#endif + + +void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); + if ( attrib ) { + attrib->SetValue( cvalue ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); + if ( attrib ) { + attrib->SetValue( _value ); + } +} +#endif + + +void TiXmlElement::Print( FILE* cfile, int depth ) const +{ + int i; + assert( cfile ); + for ( i=0; iNext() ) + { + fprintf( cfile, " " ); + attrib->Print( cfile, depth ); + } + + // There are 3 different formatting approaches: + // 1) An element without children is printed as a node + // 2) An element with only a text child is printed as text + // 3) An element with children is printed on multiple lines. + TiXmlNode* node; + if ( !firstChild ) + { + fprintf( cfile, " />" ); + } + else if ( firstChild == lastChild && firstChild->ToText() ) + { + fprintf( cfile, ">" ); + firstChild->Print( cfile, depth + 1 ); + fprintf( cfile, "", value.c_str() ); + } + else + { + fprintf( cfile, ">" ); + + for ( node = firstChild; node; node=node->NextSibling() ) + { + if ( !node->ToText() ) + { + fprintf( cfile, "\n" ); + } + node->Print( cfile, depth+1 ); + } + fprintf( cfile, "\n" ); + for( i=0; i", value.c_str() ); + } +} + + +void TiXmlElement::CopyTo( TiXmlElement* target ) const +{ + // superclass: + TiXmlNode::CopyTo( target ); + + // Element class: + // Clone the attributes, then clone the children. + const TiXmlAttribute* attribute = 0; + for( attribute = attributeSet.First(); + attribute; + attribute = attribute->Next() ) + { + target->SetAttribute( attribute->Name(), attribute->Value() ); + } + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + +bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this, attributeSet.First() ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +TiXmlNode* TiXmlElement::Clone() const +{ + TiXmlElement* clone = new TiXmlElement( Value() ); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +const char* TiXmlElement::GetText() const +{ + const TiXmlNode* child = this->FirstChild(); + if ( child ) { + const TiXmlText* childText = child->ToText(); + if ( childText ) { + return childText->Value(); + } + } + return 0; +} + + +TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + ClearError(); +} + +TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} + + +#ifdef TIXML_USE_STL +TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} +#endif + + +TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) +{ + return LoadFile( Value(), encoding ); +} + + +bool TiXmlDocument::SaveFile() const +{ + return SaveFile( Value() ); +} + +bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) +{ + TIXML_STRING filename( _filename ); + value = filename; + + // reading in binary mode so that tinyxml can normalize the EOL + FILE* file = TiXmlFOpen( value.c_str (), "rb" ); + + if ( file ) + { + bool result = LoadFile( file, encoding ); + fclose( file ); + return result; + } + else + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } +} + +bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) +{ + if ( !file ) + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Delete the existing data: + Clear(); + location.Clear(); + + // Get the file size, so we can pre-allocate the string. HUGE speed impact. + long length = 0; + fseek( file, 0, SEEK_END ); + length = ftell( file ); + fseek( file, 0, SEEK_SET ); + + // Strange case, but good to handle up front. + if ( length <= 0 ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Subtle bug here. TinyXml did use fgets. But from the XML spec: + // 2.11 End-of-Line Handling + // + // + // ...the XML processor MUST behave as if it normalized all line breaks in external + // parsed entities (including the document entity) on input, before parsing, by translating + // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to + // a single #xA character. + // + // + // It is not clear fgets does that, and certainly isn't clear it works cross platform. + // Generally, you expect fgets to translate from the convention of the OS to the c/unix + // convention, and not work generally. + + /* + while( fgets( buf, sizeof(buf), file ) ) + { + data += buf; + } + */ + + char* buf = new char[ length+1 ]; + buf[0] = 0; + + if ( fread( buf, length, 1, file ) != 1 ) { + delete [] buf; + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Process the buffer in place to normalize new lines. (See comment above.) + // Copies from the 'p' to 'q' pointer, where p can advance faster if + // a newline-carriage return is hit. + // + // Wikipedia: + // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or + // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... + // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others + // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS + // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 + + const char* p = buf; // the read head + char* q = buf; // the write head + const char CR = 0x0d; + const char LF = 0x0a; + + buf[length] = 0; + while( *p ) { + assert( p < (buf+length) ); + assert( q <= (buf+length) ); + assert( q <= p ); + + if ( *p == CR ) { + *q++ = LF; + p++; + if ( *p == LF ) { // check for CR+LF (and skip LF) + p++; + } + } + else { + *q++ = *p++; + } + } + assert( q <= (buf+length) ); + *q = 0; + + Parse( buf, 0, encoding ); + + delete [] buf; + return !Error(); +} + + +bool TiXmlDocument::SaveFile( const char * filename ) const +{ + // The old c stuff lives on... + FILE* fp = TiXmlFOpen( filename, "w" ); + if ( fp ) + { + bool result = SaveFile( fp ); + fclose( fp ); + return result; + } + return false; +} + + +bool TiXmlDocument::SaveFile( FILE* fp ) const +{ + if ( useMicrosoftBOM ) + { + const unsigned char TIXML_UTF_LEAD_0 = 0xefU; + const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; + const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + + fputc( TIXML_UTF_LEAD_0, fp ); + fputc( TIXML_UTF_LEAD_1, fp ); + fputc( TIXML_UTF_LEAD_2, fp ); + } + Print( fp, 0 ); + return (ferror(fp) == 0); +} + + +void TiXmlDocument::CopyTo( TiXmlDocument* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->error = error; + target->errorId = errorId; + target->errorDesc = errorDesc; + target->tabsize = tabsize; + target->errorLocation = errorLocation; + target->useMicrosoftBOM = useMicrosoftBOM; + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + + +TiXmlNode* TiXmlDocument::Clone() const +{ + TiXmlDocument* clone = new TiXmlDocument(); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlDocument::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + node->Print( cfile, depth ); + fprintf( cfile, "\n" ); + } +} + + +bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +const TiXmlAttribute* TiXmlAttribute::Next() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} + +/* +TiXmlAttribute* TiXmlAttribute::Next() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} +*/ + +const TiXmlAttribute* TiXmlAttribute::Previous() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} + +/* +TiXmlAttribute* TiXmlAttribute::Previous() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} +*/ + +void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + TIXML_STRING n, v; + + EncodeString( name, &n ); + EncodeString( value, &v ); + + if (value.find ('\"') == TIXML_STRING::npos) { + if ( cfile ) { + fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; + } + } + else { + if ( cfile ) { + fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; + } + } +} + + +int TiXmlAttribute::QueryIntValue( int* ival ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +int TiXmlAttribute::QueryDoubleValue( double* dval ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +void TiXmlAttribute::SetIntValue( int _value ) +{ + char buf [64]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); + #else + sprintf (buf, "%d", _value); + #endif + SetValue (buf); +} + +void TiXmlAttribute::SetDoubleValue( double _value ) +{ + char buf [256]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); + #else + sprintf (buf, "%g", _value); + #endif + SetValue (buf); +} + +int TiXmlAttribute::IntValue() const +{ + return atoi (value.c_str ()); +} + +double TiXmlAttribute::DoubleValue() const +{ + return atof (value.c_str ()); +} + + +TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) +{ + Clear(); + base.CopyTo( this ); + return *this; +} + + +void TiXmlComment::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlComment::CopyTo( TiXmlComment* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlComment::Clone() const +{ + TiXmlComment* clone = new TiXmlComment(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlText::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + if ( cdata ) + { + int i; + fprintf( cfile, "\n" ); + for ( i=0; i\n", value.c_str() ); // unformatted output + } + else + { + TIXML_STRING buffer; + EncodeString( value, &buffer ); + fprintf( cfile, "%s", buffer.c_str() ); + } +} + + +void TiXmlText::CopyTo( TiXmlText* target ) const +{ + TiXmlNode::CopyTo( target ); + target->cdata = cdata; +} + + +bool TiXmlText::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlText::Clone() const +{ + TiXmlText* clone = 0; + clone = new TiXmlText( "" ); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlDeclaration::TiXmlDeclaration( const char * _version, + const char * _encoding, + const char * _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} + + +#ifdef TIXML_USE_STL +TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} +#endif + + +TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + copy.CopyTo( this ); +} + + +TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + if ( cfile ) fprintf( cfile, "" ); + if ( str ) (*str) += "?>"; +} + + +void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->version = version; + target->encoding = encoding; + target->standalone = standalone; +} + + +bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlDeclaration::Clone() const +{ + TiXmlDeclaration* clone = new TiXmlDeclaration(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlUnknown::Print( FILE* cfile, int depth ) const +{ + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlUnknown::Clone() const +{ + TiXmlUnknown* clone = new TiXmlUnknown(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlAttributeSet::TiXmlAttributeSet() +{ + sentinel.next = &sentinel; + sentinel.prev = &sentinel; +} + + +TiXmlAttributeSet::~TiXmlAttributeSet() +{ + assert( sentinel.next == &sentinel ); + assert( sentinel.prev == &sentinel ); +} + + +void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) +{ + #ifdef TIXML_USE_STL + assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. + #else + assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. + #endif + + addMe->next = &sentinel; + addMe->prev = sentinel.prev; + + sentinel.prev->next = addMe; + sentinel.prev = addMe; +} + +void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) +{ + TiXmlAttribute* node; + + for( node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node == removeMe ) + { + node->prev->next = node->next; + node->next->prev = node->prev; + node->next = 0; + node->prev = 0; + return; + } + } + assert( 0 ); // we tried to remove a non-linked attribute. +} + + +#ifdef TIXML_USE_STL +TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node->name == name ) + return node; + } + return 0; +} + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} +#endif + + +TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( strcmp( node->name.c_str(), name ) == 0 ) + return node; + } + return 0; +} + + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} + + +#ifdef TIXML_USE_STL +std::istream& operator>> (std::istream & in, TiXmlNode & base) +{ + TIXML_STRING tag; + tag.reserve( 8 * 1000 ); + base.StreamIn( &in, &tag ); + + base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); + return in; +} +#endif + + +#ifdef TIXML_USE_STL +std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out << printer.Str(); + + return out; +} + + +std::string& operator<< (std::string& out, const TiXmlNode& base ) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out.append( printer.Str() ); + + return out; +} +#endif + + +TiXmlHandle TiXmlHandle::FirstChild() const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement() const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild(); + for ( i=0; + child && iNextSibling(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild( value ); + for ( i=0; + child && iNextSibling( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement(); + for ( i=0; + child && iNextSiblingElement(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement( value ); + for ( i=0; + child && iNextSiblingElement( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) +{ + DoIndent(); + buffer += "<"; + buffer += element.Value(); + + for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) + { + buffer += " "; + attrib->Print( 0, 0, &buffer ); + } + + if ( !element.FirstChild() ) + { + buffer += " />"; + DoLineBreak(); + } + else + { + buffer += ">"; + if ( element.FirstChild()->ToText() + && element.LastChild() == element.FirstChild() + && element.FirstChild()->ToText()->CDATA() == false ) + { + simpleTextPrint = true; + // no DoLineBreak()! + } + else + { + DoLineBreak(); + } + } + ++depth; + return true; +} + + +bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) +{ + --depth; + if ( !element.FirstChild() ) + { + // nothing. + } + else + { + if ( simpleTextPrint ) + { + simpleTextPrint = false; + } + else + { + DoIndent(); + } + buffer += ""; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlText& text ) +{ + if ( text.CDATA() ) + { + DoIndent(); + buffer += ""; + DoLineBreak(); + } + else if ( simpleTextPrint ) + { + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + } + else + { + DoIndent(); + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) +{ + DoIndent(); + declaration.Print( 0, 0, &buffer ); + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlComment& comment ) +{ + DoIndent(); + buffer += ""; + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) +{ + DoIndent(); + buffer += "<"; + buffer += unknown.Value(); + buffer += ">"; + DoLineBreak(); + return true; +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h new file mode 100644 index 0000000..a3589e5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h @@ -0,0 +1,1805 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TINYXML_INCLUDED +#define TINYXML_INCLUDED + +#ifdef _MSC_VER +#pragma warning( push ) +#pragma warning( disable : 4530 ) +#pragma warning( disable : 4786 ) +#endif + +#include +#include +#include +#include +#include + +// Help out windows: +#if defined( _DEBUG ) && !defined( DEBUG ) +#define DEBUG +#endif + +#ifdef TIXML_USE_STL + #include + #include + #include + #define TIXML_STRING std::string +#else + #include "tinystr.h" + #define TIXML_STRING TiXmlString +#endif + +// Deprecated library function hell. Compilers want to use the +// new safe versions. This probably doesn't fully address the problem, +// but it gets closer. There are too many compilers for me to fully +// test. If you get compilation troubles, undefine TIXML_SAFE +#define TIXML_SAFE + +#ifdef TIXML_SAFE + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + // Microsoft visual studio, version 2005 and higher. + #define TIXML_SNPRINTF _snprintf_s + #define TIXML_SSCANF sscanf_s + #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + //#pragma message( "Using _sn* functions." ) + #define TIXML_SNPRINTF _snprintf + #define TIXML_SSCANF sscanf + #elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + //#warning( "Using sn* functions." ) + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #else + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #endif +#endif + +class TiXmlDocument; +class TiXmlElement; +class TiXmlComment; +class TiXmlUnknown; +class TiXmlAttribute; +class TiXmlText; +class TiXmlDeclaration; +class TiXmlParsingData; + +const int TIXML_MAJOR_VERSION = 2; +const int TIXML_MINOR_VERSION = 6; +const int TIXML_PATCH_VERSION = 2; + +/* Internal structure for tracking location of items + in the XML file. +*/ +struct TiXmlCursor +{ + TiXmlCursor() { Clear(); } + void Clear() { row = col = -1; } + + int row; // 0 based. + int col; // 0 based. +}; + + +/** + Implements the interface to the "Visitor pattern" (see the Accept() method.) + If you call the Accept() method, it requires being passed a TiXmlVisitor + class to handle callbacks. For nodes that contain other nodes (Document, Element) + you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves + are simply called with Visit(). + + If you return 'true' from a Visit method, recursive parsing will continue. If you return + false, no children of this node or its sibilings will be Visited. + + All flavors of Visit methods have a default implementation that returns 'true' (continue + visiting). You need to only override methods that are interesting to you. + + Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. + + You should never change the document from a callback. + + @sa TiXmlNode::Accept() +*/ +class TiXmlVisitor +{ +public: + virtual ~TiXmlVisitor() {} + + /// Visit a document. + virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } + /// Visit a document. + virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } + + /// Visit an element. + virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } + /// Visit an element. + virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } + + /// Visit a declaration + virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } + /// Visit a text node + virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } + /// Visit a comment node + virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } + /// Visit an unknown node + virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } +}; + +// Only used by Attribute::Query functions +enum +{ + TIXML_SUCCESS, + TIXML_NO_ATTRIBUTE, + TIXML_WRONG_TYPE +}; + + +// Used by the parsing routines. +enum TiXmlEncoding +{ + TIXML_ENCODING_UNKNOWN, + TIXML_ENCODING_UTF8, + TIXML_ENCODING_LEGACY +}; + +const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; + +/** TiXmlBase is a base class for every class in TinyXml. + It does little except to establish that TinyXml classes + can be printed and provide some utility functions. + + In XML, the document and elements can contain + other elements and other types of nodes. + + @verbatim + A Document can contain: Element (container or leaf) + Comment (leaf) + Unknown (leaf) + Declaration( leaf ) + + An Element can contain: Element (container or leaf) + Text (leaf) + Attributes (not on tree) + Comment (leaf) + Unknown (leaf) + + A Decleration contains: Attributes (not on tree) + @endverbatim +*/ +class TiXmlBase +{ + friend class TiXmlNode; + friend class TiXmlElement; + friend class TiXmlDocument; + +public: + TiXmlBase() : userData(0) {} + virtual ~TiXmlBase() {} + + /** All TinyXml classes can print themselves to a filestream + or the string class (TiXmlString in non-STL mode, std::string + in STL mode.) Either or both cfile and str can be null. + + This is a formatted print, and will insert + tabs and newlines. + + (For an unformatted stream, use the << operator.) + */ + virtual void Print( FILE* cfile, int depth ) const = 0; + + /** The world does not agree on whether white space should be kept or + not. In order to make everyone happy, these global, static functions + are provided to set whether or not TinyXml will condense all white space + into a single space or not. The default is to condense. Note changing this + value is not thread safe. + */ + static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } + + /// Return the current white space setting. + static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } + + /** Return the position, in the original source file, of this node or attribute. + The row and column are 1-based. (That is the first row and first column is + 1,1). If the returns values are 0 or less, then the parser does not have + a row and column value. + + Generally, the row and column value will be set when the TiXmlDocument::Load(), + TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set + when the DOM was created from operator>>. + + The values reflect the initial load. Once the DOM is modified programmatically + (by adding or changing nodes and attributes) the new values will NOT update to + reflect changes in the document. + + There is a minor performance cost to computing the row and column. Computation + can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. + + @sa TiXmlDocument::SetTabSize() + */ + int Row() const { return location.row + 1; } + int Column() const { return location.col + 1; } ///< See Row() + + void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. + void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. + const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. + + // Table that returs, for a given lead byte, the total number of bytes + // in the UTF-8 sequence. + static const int utf8ByteTable[256]; + + virtual const char* Parse( const char* p, + TiXmlParsingData* data, + TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; + + /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, + or they will be transformed into entities! + */ + static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); + + enum + { + TIXML_NO_ERROR = 0, + TIXML_ERROR, + TIXML_ERROR_OPENING_FILE, + TIXML_ERROR_PARSING_ELEMENT, + TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, + TIXML_ERROR_READING_ELEMENT_VALUE, + TIXML_ERROR_READING_ATTRIBUTES, + TIXML_ERROR_PARSING_EMPTY, + TIXML_ERROR_READING_END_TAG, + TIXML_ERROR_PARSING_UNKNOWN, + TIXML_ERROR_PARSING_COMMENT, + TIXML_ERROR_PARSING_DECLARATION, + TIXML_ERROR_DOCUMENT_EMPTY, + TIXML_ERROR_EMBEDDED_NULL, + TIXML_ERROR_PARSING_CDATA, + TIXML_ERROR_DOCUMENT_TOP_ONLY, + + TIXML_ERROR_STRING_COUNT + }; + +protected: + + static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); + + inline static bool IsWhiteSpace( char c ) + { + return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); + } + inline static bool IsWhiteSpace( int c ) + { + if ( c < 256 ) + return IsWhiteSpace( (char) c ); + return false; // Again, only truly correct for English/Latin...but usually works. + } + + #ifdef TIXML_USE_STL + static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); + static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); + #endif + + /* Reads an XML name into the string provided. Returns + a pointer just past the last character of the name, + or 0 if the function has an error. + */ + static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); + + /* Reads text. Returns a pointer past the given end tag. + Wickedly complex options, but it keeps the (sensitive) code in one place. + */ + static const char* ReadText( const char* in, // where to start + TIXML_STRING* text, // the string read + bool ignoreWhiteSpace, // whether to keep the white space + const char* endTag, // what ends this text + bool ignoreCase, // whether to ignore case in the end tag + TiXmlEncoding encoding ); // the current encoding + + // If an entity has been found, transform it into a character. + static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); + + // Get a character, while interpreting entities. + // The length can be from 0 to 4 bytes. + inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) + { + assert( p ); + if ( encoding == TIXML_ENCODING_UTF8 ) + { + *length = utf8ByteTable[ *((const unsigned char*)p) ]; + assert( *length >= 0 && *length < 5 ); + } + else + { + *length = 1; + } + + if ( *length == 1 ) + { + if ( *p == '&' ) + return GetEntity( p, _value, length, encoding ); + *_value = *p; + return p+1; + } + else if ( *length ) + { + //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), + // and the null terminator isn't needed + for( int i=0; p[i] && i<*length; ++i ) { + _value[i] = p[i]; + } + return p + (*length); + } + else + { + // Not valid text. + return 0; + } + } + + // Return true if the next characters in the stream are any of the endTag sequences. + // Ignore case only works for english, and should only be relied on when comparing + // to English words: StringEqual( p, "version", true ) is fine. + static bool StringEqual( const char* p, + const char* endTag, + bool ignoreCase, + TiXmlEncoding encoding ); + + static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; + + TiXmlCursor location; + + /// Field containing a generic user pointer + void* userData; + + // None of these methods are reliable for any language except English. + // Good for approximation, not great for accuracy. + static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); + static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); + inline static int ToLower( int v, TiXmlEncoding encoding ) + { + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( v < 128 ) return tolower( v ); + return v; + } + else + { + return tolower( v ); + } + } + static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); + +private: + TiXmlBase( const TiXmlBase& ); // not implemented. + void operator=( const TiXmlBase& base ); // not allowed. + + struct Entity + { + const char* str; + unsigned int strLength; + char chr; + }; + enum + { + NUM_ENTITY = 5, + MAX_ENTITY_LENGTH = 6 + + }; + static Entity entity[ NUM_ENTITY ]; + static bool condenseWhiteSpace; +}; + + +/** The parent class for everything in the Document Object Model. + (Except for attributes). + Nodes have siblings, a parent, and children. A node can be + in a document, or stand on its own. The type of a TiXmlNode + can be queried, and it can be cast to its more defined type. +*/ +class TiXmlNode : public TiXmlBase +{ + friend class TiXmlDocument; + friend class TiXmlElement; + +public: + #ifdef TIXML_USE_STL + + /** An input stream operator, for every class. Tolerant of newlines and + formatting, but doesn't expect them. + */ + friend std::istream& operator >> (std::istream& in, TiXmlNode& base); + + /** An output stream operator, for every class. Note that this outputs + without any newlines or formatting, as opposed to Print(), which + includes tabs and new lines. + + The operator<< and operator>> are not completely symmetric. Writing + a node to a stream is very well defined. You'll get a nice stream + of output, without any extra whitespace or newlines. + + But reading is not as well defined. (As it always is.) If you create + a TiXmlElement (for example) and read that from an input stream, + the text needs to define an element or junk will result. This is + true of all input streams, but it's worth keeping in mind. + + A TiXmlDocument will read nodes until it reads a root element, and + all the children of that root element. + */ + friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); + + /// Appends the XML node or attribute to a std::string. + friend std::string& operator<< (std::string& out, const TiXmlNode& base ); + + #endif + + /** The types of XML nodes supported by TinyXml. (All the + unsupported types are picked up by UNKNOWN.) + */ + enum NodeType + { + TINYXML_DOCUMENT, + TINYXML_ELEMENT, + TINYXML_COMMENT, + TINYXML_UNKNOWN, + TINYXML_TEXT, + TINYXML_DECLARATION, + TINYXML_TYPECOUNT + }; + + virtual ~TiXmlNode(); + + /** The meaning of 'value' changes for the specific type of + TiXmlNode. + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + + The subclasses will wrap this function. + */ + const char *Value() const { return value.c_str (); } + + #ifdef TIXML_USE_STL + /** Return Value() as a std::string. If you only use STL, + this is more efficient than calling Value(). + Only available in STL mode. + */ + const std::string& ValueStr() const { return value; } + #endif + + const TIXML_STRING& ValueTStr() const { return value; } + + /** Changes the value of the node. Defined as: + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + */ + void SetValue(const char * _value) { value = _value;} + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Delete all the children of this node. Does not affect 'this'. + void Clear(); + + /// One step up the DOM. + TiXmlNode* Parent() { return parent; } + const TiXmlNode* Parent() const { return parent; } + + const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. + TiXmlNode* FirstChild() { return firstChild; } + const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. + /// The first child of this node with the matching 'value'. Will be null if none found. + TiXmlNode* FirstChild( const char * _value ) { + // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) + // call the method, cast the return back to non-const. + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); + } + const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. + TiXmlNode* LastChild() { return lastChild; } + + const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. + TiXmlNode* LastChild( const char * _value ) { + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. + #endif + + /** An alternate way to walk the children of a node. + One way to iterate over nodes is: + @verbatim + for( child = parent->FirstChild(); child; child = child->NextSibling() ) + @endverbatim + + IterateChildren does the same thing with the syntax: + @verbatim + child = 0; + while( child = parent->IterateChildren( child ) ) + @endverbatim + + IterateChildren takes the previous child as input and finds + the next one. If the previous child is null, it returns the + first. IterateChildren will return null when done. + */ + const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); + } + + /// This flavor of IterateChildren searches for children with a particular 'value' + const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + #endif + + /** Add a new node related to this. Adds a child past the LastChild. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); + + + /** Add a new node related to this. Adds a child past the LastChild. + + NOTE: the node to be added is passed by pointer, and will be + henceforth owned (and deleted) by tinyXml. This method is efficient + and avoids an extra copy, but should be used with care as it + uses a different memory model than the other insert functions. + + @sa InsertEndChild + */ + TiXmlNode* LinkEndChild( TiXmlNode* addThis ); + + /** Add a new node related to this. Adds a child before the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); + + /** Add a new node related to this. Adds a child after the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); + + /** Replace a child of this node. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); + + /// Delete a child of this node. + bool RemoveChild( TiXmlNode* removeThis ); + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling() const { return prev; } + TiXmlNode* PreviousSibling() { return prev; } + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling( const char * ) const; + TiXmlNode* PreviousSibling( const char *_prev ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Navigate to a sibling node. + const TiXmlNode* NextSibling() const { return next; } + TiXmlNode* NextSibling() { return next; } + + /// Navigate to a sibling node with the given 'value'. + const TiXmlNode* NextSibling( const char * ) const; + TiXmlNode* NextSibling( const char* _next ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement() const; + TiXmlElement* NextSiblingElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement( const char * ) const; + TiXmlElement* NextSiblingElement( const char *_next ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement() const; + TiXmlElement* FirstChildElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); + } + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement( const char * _value ) const; + TiXmlElement* FirstChildElement( const char * _value ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /** Query the type (as an enumerated value, above) of this node. + The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, + TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. + */ + int Type() const { return type; } + + /** Return a pointer to the Document this node lives in. + Returns null if not in a document. + */ + const TiXmlDocument* GetDocument() const; + TiXmlDocument* GetDocument() { + return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); + } + + /// Returns true if this node has no children. + bool NoChildren() const { return !firstChild; } + + virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + /** Create an exact duplicate of this node and return it. The memory must be deleted + by the caller. + */ + virtual TiXmlNode* Clone() const = 0; + + /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the + XML tree will be conditionally visited and the host will be called back + via the TiXmlVisitor interface. + + This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse + the XML for the callbacks, so the performance of TinyXML is unchanged by using this + interface versus any other.) + + The interface has been based on ideas from: + + - http://www.saxproject.org/ + - http://c2.com/cgi/wiki?HierarchicalVisitorPattern + + Which are both good references for "visiting". + + An example of using Accept(): + @verbatim + TiXmlPrinter printer; + tinyxmlDoc.Accept( &printer ); + const char* xmlcstr = printer.CStr(); + @endverbatim + */ + virtual bool Accept( TiXmlVisitor* visitor ) const = 0; + +protected: + TiXmlNode( NodeType _type ); + + // Copy to the allocated object. Shared functionality between Clone, Copy constructor, + // and the assignment operator. + void CopyTo( TiXmlNode* target ) const; + + #ifdef TIXML_USE_STL + // The real work of the input operator. + virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; + #endif + + // Figure out what is at *p, and parse it. Returns null if it is not an xml node. + TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); + + TiXmlNode* parent; + NodeType type; + + TiXmlNode* firstChild; + TiXmlNode* lastChild; + + TIXML_STRING value; + + TiXmlNode* prev; + TiXmlNode* next; + +private: + TiXmlNode( const TiXmlNode& ); // not implemented. + void operator=( const TiXmlNode& base ); // not allowed. +}; + + +/** An attribute is a name-value pair. Elements have an arbitrary + number of attributes, each with a unique name. + + @note The attributes are not TiXmlNodes, since they are not + part of the tinyXML document object model. There are other + suggested ways to look at this problem. +*/ +class TiXmlAttribute : public TiXmlBase +{ + friend class TiXmlAttributeSet; + +public: + /// Construct an empty attribute. + TiXmlAttribute() : TiXmlBase() + { + document = 0; + prev = next = 0; + } + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlAttribute( const std::string& _name, const std::string& _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + #endif + + /// Construct an attribute with a name and value. + TiXmlAttribute( const char * _name, const char * _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + + const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. + const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. + #ifdef TIXML_USE_STL + const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. + #endif + int IntValue() const; ///< Return the value of this attribute, converted to an integer. + double DoubleValue() const; ///< Return the value of this attribute, converted to a double. + + // Get the tinyxml string representation + const TIXML_STRING& NameTStr() const { return name; } + + /** QueryIntValue examines the value string. It is an alternative to the + IntValue() method with richer error checking. + If the value is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. + + A specialized but useful call. Note that for success it returns 0, + which is the opposite of almost all other TinyXml calls. + */ + int QueryIntValue( int* _value ) const; + /// QueryDoubleValue examines the value string. See QueryIntValue(). + int QueryDoubleValue( double* _value ) const; + + void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. + void SetValue( const char* _value ) { value = _value; } ///< Set the value. + + void SetIntValue( int _value ); ///< Set the value from an integer. + void SetDoubleValue( double _value ); ///< Set the value from a double. + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetName( const std::string& _name ) { name = _name; } + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Get the next sibling attribute in the DOM. Returns null at end. + const TiXmlAttribute* Next() const; + TiXmlAttribute* Next() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); + } + + /// Get the previous sibling attribute in the DOM. Returns null at beginning. + const TiXmlAttribute* Previous() const; + TiXmlAttribute* Previous() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); + } + + bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } + bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } + bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } + + /* Attribute parsing starts: first letter of the name + returns: the next char after the value end quote + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + // Prints this Attribute to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + + // [internal use] + // Set the document pointer so the attribute can report errors. + void SetDocument( TiXmlDocument* doc ) { document = doc; } + +private: + TiXmlAttribute( const TiXmlAttribute& ); // not implemented. + void operator=( const TiXmlAttribute& base ); // not allowed. + + TiXmlDocument* document; // A pointer back to a document, for error reporting. + TIXML_STRING name; + TIXML_STRING value; + TiXmlAttribute* prev; + TiXmlAttribute* next; +}; + + +/* A class used to manage a group of attributes. + It is only used internally, both by the ELEMENT and the DECLARATION. + + The set can be changed transparent to the Element and Declaration + classes that use it, but NOT transparent to the Attribute + which has to implement a next() and previous() method. Which makes + it a bit problematic and prevents the use of STL. + + This version is implemented with circular lists because: + - I like circular lists + - it demonstrates some independence from the (typical) doubly linked list. +*/ +class TiXmlAttributeSet +{ +public: + TiXmlAttributeSet(); + ~TiXmlAttributeSet(); + + void Add( TiXmlAttribute* attribute ); + void Remove( TiXmlAttribute* attribute ); + + const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + + TiXmlAttribute* Find( const char* _name ) const; + TiXmlAttribute* FindOrCreate( const char* _name ); + +# ifdef TIXML_USE_STL + TiXmlAttribute* Find( const std::string& _name ) const; + TiXmlAttribute* FindOrCreate( const std::string& _name ); +# endif + + +private: + //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), + //*ME: this class must be also use a hidden/disabled copy-constructor !!! + TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed + void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) + + TiXmlAttribute sentinel; +}; + + +/** The element is a container class. It has a value, the element name, + and can contain other elements, text, comments, and unknowns. + Elements also contain an arbitrary number of attributes. +*/ +class TiXmlElement : public TiXmlNode +{ +public: + /// Construct an element. + TiXmlElement (const char * in_value); + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlElement( const std::string& _value ); + #endif + + TiXmlElement( const TiXmlElement& ); + + TiXmlElement& operator=( const TiXmlElement& base ); + + virtual ~TiXmlElement(); + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + */ + const char* Attribute( const char* name ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an integer, + the integer value will be put in the return 'i', if 'i' + is non-null. + */ + const char* Attribute( const char* name, int* i ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an double, + the double value will be put in the return 'd', if 'd' + is non-null. + */ + const char* Attribute( const char* name, double* d ) const; + + /** QueryIntAttribute examines the attribute - it is an alternative to the + Attribute() method with richer error checking. + If the attribute is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. If the attribute + does not exist, then TIXML_NO_ATTRIBUTE is returned. + */ + int QueryIntAttribute( const char* name, int* _value ) const; + /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). + int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; + /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). + Note that '1', 'true', or 'yes' are considered true, while '0', 'false' + and 'no' are considered false. + */ + int QueryBoolAttribute( const char* name, bool* _value ) const; + /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). + int QueryDoubleAttribute( const char* name, double* _value ) const; + /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). + int QueryFloatAttribute( const char* name, float* _value ) const { + double d; + int result = QueryDoubleAttribute( name, &d ); + if ( result == TIXML_SUCCESS ) { + *_value = (float)d; + } + return result; + } + + #ifdef TIXML_USE_STL + /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). + int QueryStringAttribute( const char* name, std::string* _value ) const { + const char* cstr = Attribute( name ); + if ( cstr ) { + *_value = std::string( cstr ); + return TIXML_SUCCESS; + } + return TIXML_NO_ATTRIBUTE; + } + + /** Template form of the attribute query which will try to read the + attribute into the specified type. Very easy, very powerful, but + be careful to make sure to call this with the correct type. + + NOTE: This method doesn't work correctly for 'string' types that contain spaces. + + @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE + */ + template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + std::stringstream sstream( node->ValueStr() ); + sstream >> *outValue; + if ( !sstream.fail() ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; + } + + int QueryValueAttribute( const std::string& name, std::string* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + *outValue = node->ValueStr(); + return TIXML_SUCCESS; + } + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char* name, const char * _value ); + + #ifdef TIXML_USE_STL + const std::string* Attribute( const std::string& name ) const; + const std::string* Attribute( const std::string& name, int* i ) const; + const std::string* Attribute( const std::string& name, double* d ) const; + int QueryIntAttribute( const std::string& name, int* _value ) const; + int QueryDoubleAttribute( const std::string& name, double* _value ) const; + + /// STL std::string form. + void SetAttribute( const std::string& name, const std::string& _value ); + ///< STL std::string form. + void SetAttribute( const std::string& name, int _value ); + ///< STL std::string form. + void SetDoubleAttribute( const std::string& name, double value ); + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char * name, int value ); + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetDoubleAttribute( const char * name, double value ); + + /** Deletes an attribute with the given name. + */ + void RemoveAttribute( const char * name ); + #ifdef TIXML_USE_STL + void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. + #endif + + const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. + TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } + const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. + TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, GetText() is limited compared to getting the TiXmlText child + and accessing it directly. + + If the first child of 'this' is a TiXmlText, the GetText() + returns the character string of the Text node, else null is returned. + + This is a convenient method for getting the text of simple contained text: + @verbatim + This is text + const char* str = fooElement->GetText(); + @endverbatim + + 'str' will be a pointer to "This is text". + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + This is text + @endverbatim + + then the value of str would be null. The first child node isn't a text node, it is + another element. From this XML: + @verbatim + This is text + @endverbatim + GetText() will return "This is ". + + WARNING: GetText() accesses a child node - don't become confused with the + similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are + safe type casts on the referenced node. + */ + const char* GetText() const; + + /// Creates a new Element and returns it - the returned element is a copy. + virtual TiXmlNode* Clone() const; + // Print the Element to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: next char past '<' + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + + void CopyTo( TiXmlElement* target ) const; + void ClearThis(); // like clear, but initializes 'this' object as well + + // Used to be public [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + /* [internal use] + Reads the "value" of the element -- another element, or text. + This should terminate with the current end tag. + */ + const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + +private: + TiXmlAttributeSet attributeSet; +}; + + +/** An XML comment. +*/ +class TiXmlComment : public TiXmlNode +{ +public: + /// Constructs an empty comment. + TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} + /// Construct a comment from text. + TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { + SetValue( _value ); + } + TiXmlComment( const TiXmlComment& ); + TiXmlComment& operator=( const TiXmlComment& base ); + + virtual ~TiXmlComment() {} + + /// Returns a copy of this Comment. + virtual TiXmlNode* Clone() const; + // Write this Comment to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: at the ! of the !-- + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlComment* target ) const; + + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif +// virtual void StreamOut( TIXML_OSTREAM * out ) const; + +private: + +}; + + +/** XML text. A text node can have 2 ways to output the next. "normal" output + and CDATA. It will default to the mode it was parsed from the XML file and + you generally want to leave it alone, but you can change the output mode with + SetCDATA() and query it with CDATA(). +*/ +class TiXmlText : public TiXmlNode +{ + friend class TiXmlElement; +public: + /** Constructor for text element. By default, it is treated as + normal, encoded text. If you want it be output as a CDATA text + element, set the parameter _cdata to 'true' + */ + TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + virtual ~TiXmlText() {} + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + #endif + + TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } + TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } + + // Write this text object to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /// Queries whether this represents text using a CDATA section. + bool CDATA() const { return cdata; } + /// Turns on or off a CDATA representation of text. + void SetCDATA( bool _cdata ) { cdata = _cdata; } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + /// [internal use] Creates a new Element and returns it. + virtual TiXmlNode* Clone() const; + void CopyTo( TiXmlText* target ) const; + + bool Blank() const; // returns true if all white space and new lines + // [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + bool cdata; // true if this should be input and output as a CDATA style text element +}; + + +/** In correct XML the declaration is the first entry in the file. + @verbatim + + @endverbatim + + TinyXml will happily read or write files without a declaration, + however. There are 3 possible attributes to the declaration: + version, encoding, and standalone. + + Note: In this version of the code, the attributes are + handled as special cases, not generic attributes, simply + because there can only be at most 3 and they are always the same. +*/ +class TiXmlDeclaration : public TiXmlNode +{ +public: + /// Construct an empty declaration. + TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} + +#ifdef TIXML_USE_STL + /// Constructor. + TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ); +#endif + + /// Construct. + TiXmlDeclaration( const char* _version, + const char* _encoding, + const char* _standalone ); + + TiXmlDeclaration( const TiXmlDeclaration& copy ); + TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); + + virtual ~TiXmlDeclaration() {} + + /// Version. Will return an empty string if none was found. + const char *Version() const { return version.c_str (); } + /// Encoding. Will return an empty string if none was found. + const char *Encoding() const { return encoding.c_str (); } + /// Is this a standalone document? + const char *Standalone() const { return standalone.c_str (); } + + /// Creates a copy of this Declaration and returns it. + virtual TiXmlNode* Clone() const; + // Print this declaration to a FILE stream. + virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlDeclaration* target ) const; + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + + TIXML_STRING version; + TIXML_STRING encoding; + TIXML_STRING standalone; +}; + + +/** Any tag that tinyXml doesn't recognize is saved as an + unknown. It is a tag of text, but should not be modified. + It will be written back to the XML, unchanged, when the file + is saved. + + DTD tags get thrown into TiXmlUnknowns. +*/ +class TiXmlUnknown : public TiXmlNode +{ +public: + TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} + virtual ~TiXmlUnknown() {} + + TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } + TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } + + /// Creates a copy of this Unknown and returns it. + virtual TiXmlNode* Clone() const; + // Print this Unknown to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected: + void CopyTo( TiXmlUnknown* target ) const; + + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + +}; + + +/** Always the top level node. A document binds together all the + XML pieces. It can be saved, loaded, and printed to the screen. + The 'value' of a document node is the xml file name. +*/ +class TiXmlDocument : public TiXmlNode +{ +public: + /// Create an empty document, that has no name. + TiXmlDocument(); + /// Create a document with a name. The name of the document is also the filename of the xml. + TiXmlDocument( const char * documentName ); + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlDocument( const std::string& documentName ); + #endif + + TiXmlDocument( const TiXmlDocument& copy ); + TiXmlDocument& operator=( const TiXmlDocument& copy ); + + virtual ~TiXmlDocument() {} + + /** Load a file using the current document value. + Returns true if successful. Will delete any existing + document data before loading. + */ + bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the current document value. Returns true if successful. + bool SaveFile() const; + /// Load a file using the given filename. Returns true if successful. + bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given filename. Returns true if successful. + bool SaveFile( const char * filename ) const; + /** Load a file using the given FILE*. Returns true if successful. Note that this method + doesn't stream - the entire object pointed at by the FILE* + will be interpreted as an XML file. TinyXML doesn't stream in XML from the current + file location. Streaming may be added in the future. + */ + bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given FILE*. Returns true if successful. + bool SaveFile( FILE* ) const; + + #ifdef TIXML_USE_STL + bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. + { + return LoadFile( filename.c_str(), encoding ); + } + bool SaveFile( const std::string& filename ) const ///< STL std::string version. + { + return SaveFile( filename.c_str() ); + } + #endif + + /** Parse the given null terminated block of xml data. Passing in an encoding to this + method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml + to use that encoding, regardless of what TinyXml might otherwise try to detect. + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + + /** Get the root element -- the only top level element -- of the document. + In well formed XML, there should only be one. TinyXml is tolerant of + multiple elements at the document level. + */ + const TiXmlElement* RootElement() const { return FirstChildElement(); } + TiXmlElement* RootElement() { return FirstChildElement(); } + + /** If an error occurs, Error will be set to true. Also, + - The ErrorId() will contain the integer identifier of the error (not generally useful) + - The ErrorDesc() method will return the name of the error. (very useful) + - The ErrorRow() and ErrorCol() will return the location of the error (if known) + */ + bool Error() const { return error; } + + /// Contains a textual (english) description of the error if one occurs. + const char * ErrorDesc() const { return errorDesc.c_str (); } + + /** Generally, you probably want the error string ( ErrorDesc() ). But if you + prefer the ErrorId, this function will fetch it. + */ + int ErrorId() const { return errorId; } + + /** Returns the location (if known) of the error. The first column is column 1, + and the first row is row 1. A value of 0 means the row and column wasn't applicable + (memory errors, for example, have no row/column) or the parser lost the error. (An + error in the error reporting, in that case.) + + @sa SetTabSize, Row, Column + */ + int ErrorRow() const { return errorLocation.row+1; } + int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() + + /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) + to report the correct values for row and column. It does not change the output + or input in any way. + + By calling this method, with a tab size + greater than 0, the row and column of each node and attribute is stored + when the file is loaded. Very useful for tracking the DOM back in to + the source file. + + The tab size is required for calculating the location of nodes. If not + set, the default of 4 is used. The tabsize is set per document. Setting + the tabsize to 0 disables row/column tracking. + + Note that row and column tracking is not supported when using operator>>. + + The tab size needs to be enabled before the parse or load. Correct usage: + @verbatim + TiXmlDocument doc; + doc.SetTabSize( 8 ); + doc.Load( "myfile.xml" ); + @endverbatim + + @sa Row, Column + */ + void SetTabSize( int _tabsize ) { tabsize = _tabsize; } + + int TabSize() const { return tabsize; } + + /** If you have handled the error, it can be reset with this call. The error + state is automatically cleared if you Parse a new XML block. + */ + void ClearError() { error = false; + errorId = 0; + errorDesc = ""; + errorLocation.row = errorLocation.col = 0; + //errorLocation.last = 0; + } + + /** Write the document to standard out using formatted printing ("pretty print"). */ + void Print() const { Print( stdout, 0 ); } + + /* Write the document to a string using formatted printing ("pretty print"). This + will allocate a character array (new char[]) and return it as a pointer. The + calling code pust call delete[] on the return char* to avoid a memory leak. + */ + //char* PrintToMemory() const; + + /// Print this Document to a FILE stream. + virtual void Print( FILE* cfile, int depth = 0 ) const; + // [internal use] + void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + + virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + // [internal use] + virtual TiXmlNode* Clone() const; + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + void CopyTo( TiXmlDocument* target ) const; + + bool error; + int errorId; + TIXML_STRING errorDesc; + int tabsize; + TiXmlCursor errorLocation; + bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. +}; + + +/** + A TiXmlHandle is a class that wraps a node pointer with null checks; this is + an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml + DOM structure. It is a separate utility class. + + Take an example: + @verbatim + + + + + + + @endverbatim + + Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very + easy to write a *lot* of code that looks like: + + @verbatim + TiXmlElement* root = document.FirstChildElement( "Document" ); + if ( root ) + { + TiXmlElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + TiXmlElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + TiXmlElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. + @endverbatim + + And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity + of such code. A TiXmlHandle checks for null pointers so it is perfectly safe + and correct to use: + + @verbatim + TiXmlHandle docHandle( &document ); + TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); + if ( child2 ) + { + // do something useful + @endverbatim + + Which is MUCH more concise and useful. + + It is also safe to copy handles - internally they are nothing more than node pointers. + @verbatim + TiXmlHandle handleCopy = handle; + @endverbatim + + What they should not be used for is iteration: + + @verbatim + int i=0; + while ( true ) + { + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); + if ( !child ) + break; + // do something + ++i; + } + @endverbatim + + It seems reasonable, but it is in fact two embedded while loops. The Child method is + a linear walk to find the element, so this code would iterate much more than it needs + to. Instead, prefer: + + @verbatim + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); + + for( child; child; child=child->NextSiblingElement() ) + { + // do something + } + @endverbatim +*/ +class TiXmlHandle +{ +public: + /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. + TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } + /// Copy constructor + TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } + TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } + + /// Return a handle to the first child node. + TiXmlHandle FirstChild() const; + /// Return a handle to the first child node with the given name. + TiXmlHandle FirstChild( const char * value ) const; + /// Return a handle to the first child element. + TiXmlHandle FirstChildElement() const; + /// Return a handle to the first child element with the given name. + TiXmlHandle FirstChildElement( const char * value ) const; + + /** Return a handle to the "index" child with the given name. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( const char* value, int index ) const; + /** Return a handle to the "index" child. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( int index ) const; + /** Return a handle to the "index" child element with the given name. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( const char* value, int index ) const; + /** Return a handle to the "index" child element. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( int index ) const; + + #ifdef TIXML_USE_STL + TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } + TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } + + TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } + TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } + #endif + + /** Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* ToNode() const { return node; } + /** Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } + /** Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } + /** Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } + + /** @deprecated use ToNode. + Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* Node() const { return ToNode(); } + /** @deprecated use ToElement. + Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* Element() const { return ToElement(); } + /** @deprecated use ToText() + Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* Text() const { return ToText(); } + /** @deprecated use ToUnknown() + Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* Unknown() const { return ToUnknown(); } + +private: + TiXmlNode* node; +}; + + +/** Print to memory functionality. The TiXmlPrinter is useful when you need to: + + -# Print to memory (especially in non-STL mode) + -# Control formatting (line endings, etc.) + + When constructed, the TiXmlPrinter is in its default "pretty printing" mode. + Before calling Accept() you can call methods to control the printing + of the XML document. After TiXmlNode::Accept() is called, the printed document can + be accessed via the CStr(), Str(), and Size() methods. + + TiXmlPrinter uses the Visitor API. + @verbatim + TiXmlPrinter printer; + printer.SetIndent( "\t" ); + + doc.Accept( &printer ); + fprintf( stdout, "%s", printer.CStr() ); + @endverbatim +*/ +class TiXmlPrinter : public TiXmlVisitor +{ +public: + TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), + buffer(), indent( " " ), lineBreak( "\n" ) {} + + virtual bool VisitEnter( const TiXmlDocument& doc ); + virtual bool VisitExit( const TiXmlDocument& doc ); + + virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); + virtual bool VisitExit( const TiXmlElement& element ); + + virtual bool Visit( const TiXmlDeclaration& declaration ); + virtual bool Visit( const TiXmlText& text ); + virtual bool Visit( const TiXmlComment& comment ); + virtual bool Visit( const TiXmlUnknown& unknown ); + + /** Set the indent characters for printing. By default 4 spaces + but tab (\t) is also useful, or null/empty string for no indentation. + */ + void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } + /// Query the indention string. + const char* Indent() { return indent.c_str(); } + /** Set the line breaking string. By default set to newline (\n). + Some operating systems prefer other characters, or can be + set to the null/empty string for no indenation. + */ + void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } + /// Query the current line breaking string. + const char* LineBreak() { return lineBreak.c_str(); } + + /** Switch over to "stream printing" which is the most dense formatting without + linebreaks. Common when the XML is needed for network transmission. + */ + void SetStreamPrinting() { indent = ""; + lineBreak = ""; + } + /// Return the result. + const char* CStr() { return buffer.c_str(); } + /// Return the length of the result string. + size_t Size() { return buffer.size(); } + + #ifdef TIXML_USE_STL + /// Return the result. + const std::string& Str() { return buffer; } + #endif + +private: + void DoIndent() { + for( int i=0; i +#include + +#include "tinyxml.h" + +//#define DEBUG_PARSER +#if defined( DEBUG_PARSER ) +# if defined( DEBUG ) && defined( _MSC_VER ) +# include +# define TIXML_LOG OutputDebugString +# else +# define TIXML_LOG printf +# endif +#endif + +// Note tha "PutString" hardcodes the same list. This +// is less flexible than it appears. Changing the entries +// or order will break putstring. +TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = +{ + { "&", 5, '&' }, + { "<", 4, '<' }, + { ">", 4, '>' }, + { """, 6, '\"' }, + { "'", 6, '\'' } +}; + +// Bunch of unicode info at: +// http://www.unicode.org/faq/utf_bom.html +// Including the basic of this table, which determines the #bytes in the +// sequence from the lead byte. 1 placed for invalid sequences -- +// although the result will be junk, pass it through as much as possible. +// Beware of the non-characters in UTF-8: +// ef bb bf (Microsoft "lead bytes") +// ef bf be +// ef bf bf + +const unsigned char TIXML_UTF_LEAD_0 = 0xefU; +const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; +const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + +const int TiXmlBase::utf8ByteTable[256] = +{ + // 0 1 2 3 4 5 6 7 8 9 a b c d e f + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 + 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 + 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte + 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid +}; + + +void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) +{ + const unsigned long BYTE_MASK = 0xBF; + const unsigned long BYTE_MARK = 0x80; + const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; + + if (input < 0x80) + *length = 1; + else if ( input < 0x800 ) + *length = 2; + else if ( input < 0x10000 ) + *length = 3; + else if ( input < 0x200000 ) + *length = 4; + else + { *length = 0; return; } // This code won't covert this correctly anyway. + + output += *length; + + // Scary scary fall throughs. + switch (*length) + { + case 4: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 3: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 2: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 1: + --output; + *output = (char)(input | FIRST_BYTE_MARK[*length]); + } +} + + +/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalpha( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalpha( anyByte ); +// } +} + + +/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalnum( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalnum( anyByte ); +// } +} + + +class TiXmlParsingData +{ + friend class TiXmlDocument; + public: + void Stamp( const char* now, TiXmlEncoding encoding ); + + const TiXmlCursor& Cursor() const { return cursor; } + + private: + // Only used by the document! + TiXmlParsingData( const char* start, int _tabsize, int row, int col ) + { + assert( start ); + stamp = start; + tabsize = _tabsize; + cursor.row = row; + cursor.col = col; + } + + TiXmlCursor cursor; + const char* stamp; + int tabsize; +}; + + +void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) +{ + assert( now ); + + // Do nothing if the tabsize is 0. + if ( tabsize < 1 ) + { + return; + } + + // Get the current row, column. + int row = cursor.row; + int col = cursor.col; + const char* p = stamp; + assert( p ); + + while ( p < now ) + { + // Treat p as unsigned, so we have a happy compiler. + const unsigned char* pU = (const unsigned char*)p; + + // Code contributed by Fletcher Dunn: (modified by lee) + switch (*pU) { + case 0: + // We *should* never get here, but in case we do, don't + // advance past the terminating null character, ever + return; + + case '\r': + // bump down to the next line + ++row; + col = 0; + // Eat the character + ++p; + + // Check for \r\n sequence, and treat this as a single character + if (*p == '\n') { + ++p; + } + break; + + case '\n': + // bump down to the next line + ++row; + col = 0; + + // Eat the character + ++p; + + // Check for \n\r sequence, and treat this as a single + // character. (Yes, this bizarre thing does occur still + // on some arcane platforms...) + if (*p == '\r') { + ++p; + } + break; + + case '\t': + // Eat the character + ++p; + + // Skip to next tab stop + col = (col / tabsize + 1) * tabsize; + break; + + case TIXML_UTF_LEAD_0: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( *(p+1) && *(p+2) ) + { + // In these cases, don't advance the column. These are + // 0-width spaces. + if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) + p += 3; + else + { p +=3; ++col; } // A normal character. + } + } + else + { + ++p; + ++col; + } + break; + + default: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // Eat the 1 to 4 byte utf8 character. + int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; + if ( step == 0 ) + step = 1; // Error case from bad encoding, but handle gracefully. + p += step; + + // Just advance one column, of course. + ++col; + } + else + { + ++p; + ++col; + } + break; + } + } + cursor.row = row; + cursor.col = col; + assert( cursor.row >= -1 ); + assert( cursor.col >= -1 ); + stamp = p; + assert( stamp ); +} + + +const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) +{ + if ( !p || !*p ) + { + return 0; + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + while ( *p ) + { + const unsigned char* pU = (const unsigned char*)p; + + // Skip the stupid Microsoft UTF-8 Byte order marks + if ( *(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==TIXML_UTF_LEAD_1 + && *(pU+2)==TIXML_UTF_LEAD_2 ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbeU ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbfU ) + { + p += 3; + continue; + } + + if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. + ++p; + else + break; + } + } + else + { + while ( *p && IsWhiteSpace( *p ) ) + ++p; + } + + return p; +} + +#ifdef TIXML_USE_STL +/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) +{ + for( ;; ) + { + if ( !in->good() ) return false; + + int c = in->peek(); + // At this scope, we can't get to a document. So fail silently. + if ( !IsWhiteSpace( c ) || c <= 0 ) + return true; + + *tag += (char) in->get(); + } +} + +/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) +{ + //assert( character > 0 && character < 128 ); // else it won't work in utf-8 + while ( in->good() ) + { + int c = in->peek(); + if ( c == character ) + return true; + if ( c <= 0 ) // Silent failure: can't get document at this scope + return false; + + in->get(); + *tag += (char) c; + } + return false; +} +#endif + +// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The +// "assign" optimization removes over 10% of the execution time. +// +const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) +{ + // Oddly, not supported on some comilers, + //name->clear(); + // So use this: + *name = ""; + assert( p ); + + // Names start with letters or underscores. + // Of course, in unicode, tinyxml has no idea what a letter *is*. The + // algorithm is generous. + // + // After that, they can be letters, underscores, numbers, + // hyphens, or colons. (Colons are valid ony for namespaces, + // but tinyxml can't tell namespaces from names.) + if ( p && *p + && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) + { + const char* start = p; + while( p && *p + && ( IsAlphaNum( (unsigned char ) *p, encoding ) + || *p == '_' + || *p == '-' + || *p == '.' + || *p == ':' ) ) + { + //(*name) += *p; // expensive + ++p; + } + if ( p-start > 0 ) { + name->assign( start, p-start ); + } + return p; + } + return 0; +} + +const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) +{ + // Presume an entity, and pull it out. + TIXML_STRING ent; + int i; + *length = 0; + + if ( *(p+1) && *(p+1) == '#' && *(p+2) ) + { + unsigned long ucs = 0; + ptrdiff_t delta = 0; + unsigned mult = 1; + + if ( *(p+2) == 'x' ) + { + // Hexadecimal. + if ( !*(p+3) ) return 0; + + const char* q = p+3; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != 'x' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else if ( *q >= 'a' && *q <= 'f' ) + ucs += mult * (*q - 'a' + 10); + else if ( *q >= 'A' && *q <= 'F' ) + ucs += mult * (*q - 'A' + 10 ); + else + return 0; + mult *= 16; + --q; + } + } + else + { + // Decimal. + if ( !*(p+2) ) return 0; + + const char* q = p+2; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != '#' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else + return 0; + mult *= 10; + --q; + } + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // convert the UCS to UTF-8 + ConvertUTF32ToUTF8( ucs, value, length ); + } + else + { + *value = (char)ucs; + *length = 1; + } + return p + delta + 1; + } + + // Now try to match it. + for( i=0; iappend( cArr, len ); + } + } + else + { + bool whitespace = false; + + // Remove leading white space: + p = SkipWhiteSpace( p, encoding ); + while ( p && *p + && !StringEqual( p, endTag, caseInsensitive, encoding ) ) + { + if ( *p == '\r' || *p == '\n' ) + { + whitespace = true; + ++p; + } + else if ( IsWhiteSpace( *p ) ) + { + whitespace = true; + ++p; + } + else + { + // If we've found whitespace, add it before the + // new character. Any whitespace just becomes a space. + if ( whitespace ) + { + (*text) += ' '; + whitespace = false; + } + int len; + char cArr[4] = { 0, 0, 0, 0 }; + p = GetChar( p, cArr, &len, encoding ); + if ( len == 1 ) + (*text) += cArr[0]; // more efficient + else + text->append( cArr, len ); + } + } + } + if ( p && *p ) + p += strlen( endTag ); + return ( p && *p ) ? p : 0; +} + +#ifdef TIXML_USE_STL + +void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + // The basic issue with a document is that we don't know what we're + // streaming. Read something presumed to be a tag (and hope), then + // identify it, and call the appropriate stream method on the tag. + // + // This "pre-streaming" will never read the closing ">" so the + // sub-tag can orient itself. + + if ( !StreamTo( in, '<', tag ) ) + { + SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + while ( in->good() ) + { + int tagIndex = (int) tag->length(); + while ( in->good() && in->peek() != '>' ) + { + int c = in->get(); + if ( c <= 0 ) + { + SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + break; + } + (*tag) += (char) c; + } + + if ( in->good() ) + { + // We now have something we presume to be a node of + // some sort. Identify it, and call the node to + // continue streaming. + TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); + + if ( node ) + { + node->StreamIn( in, tag ); + bool isElement = node->ToElement() != 0; + delete node; + node = 0; + + // If this is the root element, we're done. Parsing will be + // done by the >> operator. + if ( isElement ) + { + return; + } + } + else + { + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + } + } + // We should have returned sooner. + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); +} + +#endif + +const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) +{ + ClearError(); + + // Parse away, at the document level. Since a document + // contains nothing but other tags, most of what happens + // here is skipping white space. + if ( !p || !*p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + // Note that, for a document, this needs to come + // before the while space skip, so that parsing + // starts from the pointer we are given. + location.Clear(); + if ( prevData ) + { + location.row = prevData->cursor.row; + location.col = prevData->cursor.col; + } + else + { + location.row = 0; + location.col = 0; + } + TiXmlParsingData data( p, TabSize(), location.row, location.col ); + location = data.Cursor(); + + if ( encoding == TIXML_ENCODING_UNKNOWN ) + { + // Check for the Microsoft UTF-8 lead bytes. + const unsigned char* pU = (const unsigned char*)p; + if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 + && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 + && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) + { + encoding = TIXML_ENCODING_UTF8; + useMicrosoftBOM = true; + } + } + + p = SkipWhiteSpace( p, encoding ); + if ( !p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + while ( p && *p ) + { + TiXmlNode* node = Identify( p, encoding ); + if ( node ) + { + p = node->Parse( p, &data, encoding ); + LinkEndChild( node ); + } + else + { + break; + } + + // Did we get encoding info? + if ( encoding == TIXML_ENCODING_UNKNOWN + && node->ToDeclaration() ) + { + TiXmlDeclaration* dec = node->ToDeclaration(); + const char* enc = dec->Encoding(); + assert( enc ); + + if ( *enc == 0 ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice + else + encoding = TIXML_ENCODING_LEGACY; + } + + p = SkipWhiteSpace( p, encoding ); + } + + // Was this empty? + if ( !firstChild ) { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); + return 0; + } + + // All is well. + return p; +} + +void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + // The first error in a chain is more accurate - don't set again! + if ( error ) + return; + + assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); + error = true; + errorId = err; + errorDesc = errorString[ errorId ]; + + errorLocation.Clear(); + if ( pError && data ) + { + data->Stamp( pError, encoding ); + errorLocation = data->Cursor(); + } +} + + +TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) +{ + TiXmlNode* returnNode = 0; + + p = SkipWhiteSpace( p, encoding ); + if( !p || !*p || *p != '<' ) + { + return 0; + } + + p = SkipWhiteSpace( p, encoding ); + + if ( !p || !*p ) + { + return 0; + } + + // What is this thing? + // - Elements start with a letter or underscore, but xml is reserved. + // - Comments: "; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // [ 1475201 ] TinyXML parses entities in comments + // Oops - ReadText doesn't work, because we don't want to parse the entities. + // p = ReadText( p, &value, false, endTag, false, encoding ); + // + // from the XML spec: + /* + [Definition: Comments may appear anywhere in a document outside other markup; in addition, + they may appear within the document type declaration at places allowed by the grammar. + They are not part of the document's character data; an XML processor MAY, but need not, + make it possible for an application to retrieve the text of comments. For compatibility, + the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity + references MUST NOT be recognized within comments. + + An example of a comment: + + + */ + + value = ""; + // Keep all the white space. + while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) + { + value.append( p, 1 ); + ++p; + } + if ( p && *p ) + p += strlen( endTag ); + + return p; +} + + +const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) return 0; + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + // Read the name, the '=' and the value. + const char* pErr = p; + p = ReadName( p, &name, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); + return 0; + } + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p || *p != '=' ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + ++p; // skip '=' + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + const char* end; + const char SINGLE_QUOTE = '\''; + const char DOUBLE_QUOTE = '\"'; + + if ( *p == SINGLE_QUOTE ) + { + ++p; + end = "\'"; // single quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else if ( *p == DOUBLE_QUOTE ) + { + ++p; + end = "\""; // double quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else + { + // All attribute values should be in single or double quotes. + // But this is such a common error that the parser will try + // its best, even without them. + value = ""; + while ( p && *p // existence + && !IsWhiteSpace( *p ) // whitespace + && *p != '/' && *p != '>' ) // tag end + { + if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { + // [ 1451649 ] Attribute values with trailing quotes not handled correctly + // We did not have an opening quote but seem to have a + // closing one. Give up and throw an error. + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + value += *p; + ++p; + } + } + return p; +} + +#ifdef TIXML_USE_STL +void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->peek(); + if ( !cdata && (c == '<' ) ) + { + return; + } + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + (*tag) += (char) c; + in->get(); // "commits" the peek made above + + if ( cdata && c == '>' && tag->size() >= 3 ) { + size_t len = tag->size(); + if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { + // terminator of cdata. + return; + } + } + } +} +#endif + +const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + value = ""; + TiXmlDocument* document = GetDocument(); + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + + const char* const startTag = ""; + + if ( cdata || StringEqual( p, startTag, false, encoding ) ) + { + cdata = true; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // Keep all the white space, ignore the encoding, etc. + while ( p && *p + && !StringEqual( p, endTag, false, encoding ) + ) + { + value += *p; + ++p; + } + + TIXML_STRING dummy; + p = ReadText( p, &dummy, false, endTag, false, encoding ); + return p; + } + else + { + bool ignoreWhite = true; + + const char* end = "<"; + p = ReadText( p, &value, ignoreWhite, end, false, encoding ); + if ( p && *p ) + return p-1; // don't truncate the '<' + return 0; + } +} + +#ifdef TIXML_USE_STL +void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->get(); + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + (*tag) += (char) c; + + if ( c == '>' ) + { + // All is well. + return; + } + } +} +#endif + +const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) +{ + p = SkipWhiteSpace( p, _encoding ); + // Find the beginning, find the end, and look for + // the stuff in-between. + TiXmlDocument* document = GetDocument(); + if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); + return 0; + } + if ( data ) + { + data->Stamp( p, _encoding ); + location = data->Cursor(); + } + p += 5; + + version = ""; + encoding = ""; + standalone = ""; + + while ( p && *p ) + { + if ( *p == '>' ) + { + ++p; + return p; + } + + p = SkipWhiteSpace( p, _encoding ); + if ( StringEqual( p, "version", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + version = attrib.Value(); + } + else if ( StringEqual( p, "encoding", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + encoding = attrib.Value(); + } + else if ( StringEqual( p, "standalone", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + standalone = attrib.Value(); + } + else + { + // Read over whatever it is. + while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) + ++p; + } + } + return 0; +} + +bool TiXmlText::Blank() const +{ + for ( unsigned i=0; i + + The world has many languages + Мир имеет много Ñзыков + el mundo tiene muchos idiomas + 世界有很多语言 + <РуÑÑкий название="name" ценноÑÑ‚ÑŒ="value"><имеет> + <汉语 åå­—="name" 价值="value">世界有很多语言 + "Mëtæl!" + <ä>Umlaut Element + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml new file mode 100644 index 0000000..91a319d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml @@ -0,0 +1,11 @@ + + + The world has many languages + Мир имеет много Ñзыков + el mundo tiene muchos idiomas + 世界有很多语言 + <РуÑÑкий название="name" ценноÑÑ‚ÑŒ="value"><имеет> + <汉语 åå­—="name" 价值="value">世界有很多语言 + "Mëtæl!" + <ä>Umlaut Element + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h new file mode 100644 index 0000000..4086469 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h @@ -0,0 +1,27 @@ +#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H +#define BOOST_REPLACEMENT_LEXICAL_CAST_H + +#include + +namespace boost +{ + + template T lexical_cast(const char* txt) + { + double result = atof(txt); + return result; + }; + + struct bad_lexical_cast + { + const char* what() + { + return ("bad lexical cast\n"); + } + + }; + +} //namespace boost + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp new file mode 100644 index 0000000..ed38915 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp @@ -0,0 +1,28 @@ +#include "printf_console.h" +#include + +#include + + +void logError(const char* msg, const char* arg0, const char* arg1, const char* arg2) +{ + LOG << msg << " " << arg0 << " " << arg1 << " " << arg2 << std::endl; +} + +void logDebug(const char* msg, float v0, float v1) +{ + LOG << msg << " " << v0 << " " << v1 << std::endl; +}; +void logDebug(const char* msg, const char* msg1, const char* arg1) +{ + LOG << msg << " " << msg1 << " " << arg1 << std::endl; +} + +void logInform(const char* msg, const char* arg0) +{ + LOG << msg << " " << arg0 << std::endl; +} +void logWarn(const char* msg,int id, const char* arg0) +{ + LOG << msg << " " << id << " " << arg0 << std::endl; +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h new file mode 100644 index 0000000..247aab2 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h @@ -0,0 +1,13 @@ +#ifndef PRINTF_CONSOLE_H +#define PRINTF_CONSOLE_H + + +void logError(const char* msg="", const char* arg0="", const char* arg1="", const char* arg2=""); +void logDebug(const char* msg, float v0, float v1); +void logDebug(const char* msg, const char* msg1="", const char* arg1=""); +void logInform(const char* msg, const char* arg0=""); +void logWarn(const char* msg,int id, const char* arg0=""); + +#endif + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h new file mode 100644 index 0000000..5d29732 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h @@ -0,0 +1,210 @@ +/* +Bullet Continuous Collision Detection and Physics Library Maya Plugin +Copyright (c) 2008 Walt Disney Studios + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising +from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must +not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +Written by: Nicola Candussi + +Modified by Francisco Gochez +Dec 2011 - Added deferencing operator +*/ + +//my_shared_ptr + +#ifndef DYN_SHARED_PTR_H +#define DYN_SHARED_PTR_H + +#define DYN_SHARED_PTR_THREAD_SAFE + + +#include + +#ifdef _WIN32 +#include + +class my_shared_count { +public: + my_shared_count(): m_count(1) { } + ~my_shared_count() { } + + long increment() + { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + return InterlockedIncrement(&m_count); +#else + return ++m_count; +#endif + } + + long decrement() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + return InterlockedDecrement(&m_count); +#else + return ++m_count; +#endif + } + + long use_count() { return m_count; } + +private: + long m_count; +}; +#else //ifdef WIN32 + + +#include + +class my_shared_count { +public: + my_shared_count(): m_count(1) { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_init(&m_mutex, 0); +#endif + } + ~my_shared_count() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_destroy(&m_mutex); +#endif + } + + long increment() + { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_lock(&m_mutex); +#endif + long c = ++m_count; +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_unlock(&m_mutex); +#endif + return c; + } + + long decrement() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_lock(&m_mutex); +#endif + long c = --m_count; +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_unlock(&m_mutex); +#endif + return c; + } + + long use_count() { return m_count; } + +private: + long m_count; + mutable pthread_mutex_t m_mutex; +}; + +#endif + + +template +class my_shared_ptr +{ +public: + my_shared_ptr(): m_ptr(NULL), m_count(NULL) { } + my_shared_ptr(my_shared_ptr const& other): + m_ptr(other.m_ptr), + m_count(other.m_count) + { + if(other.m_count != NULL) other.m_count->increment(); + } + + template + my_shared_ptr(my_shared_ptr const& other): + m_ptr(other.m_ptr), + m_count(other.m_count) + { + if(other.m_count != NULL) other.m_count->increment(); + } + + my_shared_ptr(T const* other): m_ptr(const_cast(other)), m_count(NULL) + { + if(other != NULL) m_count = new my_shared_count; + } + + ~my_shared_ptr() + { + giveup_ownership(); + } + + void reset(T const* other) + { + if(m_ptr == other) return; + giveup_ownership(); + m_ptr = const_cast(other); + if(other != NULL) m_count = new my_shared_count; + else m_count = NULL; + } + + T* get() { return m_ptr; } + T const* get() const { return m_ptr; } + T* operator->() { return m_ptr; } + T const* operator->() const { return m_ptr; } + operator bool() const { return m_ptr != NULL; } + T& operator*() const + { + assert(m_ptr != 0); + return *m_ptr; + } + + bool operator<(my_shared_ptr const& rhs) const { return m_ptr < rhs.m_ptr; } + + my_shared_ptr& operator=(my_shared_ptr const& other) { + if(m_ptr == other.m_ptr) return *this; + giveup_ownership(); + m_ptr = other.m_ptr; + m_count = other.m_count; + if(other.m_count != NULL) m_count->increment(); + return *this; + } + + template + my_shared_ptr& operator=(my_shared_ptr& other) { + if(m_ptr == other.m_ptr) return *this; + giveup_ownership(); + m_ptr = other.m_ptr; + m_count = other.m_count; + if(other.m_count != NULL) m_count->increment(); + return *this; + } + +protected: + + template friend class my_shared_ptr; + void giveup_ownership() + { + if(m_count != NULL) { + if( m_count->decrement() == 0) { + delete m_ptr; + m_ptr = NULL; + delete m_count; + m_count = NULL; + } + } + } + +protected: + T *m_ptr; + my_shared_count *m_count; + +}; + + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp new file mode 100644 index 0000000..6c8a294 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp @@ -0,0 +1,253 @@ + + +#include +//#include +#include +#include +#include + +#include "string_split.h" + +namespace boost +{ + void split( std::vector&pieces, const std::string& vector_str, std::vector separators) + { + assert(separators.size()==1); + if (separators.size()==1) + { + char** strArray = str_split(vector_str.c_str(),separators[0].c_str()); + int numSubStr = str_array_len(strArray); + for (int i=0;i is_any_of(const char* seps) + { + std::vector strArray; + + int numSeps = strlen(seps); + for (int i=0;i +#include +#include + +namespace boost +{ + void split( std::vector&pieces, const std::string& vector_str, std::vector separators); + std::vector is_any_of(const char* seps); +}; + +///The string split C code is by Lars Wirzenius +///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char + + +/* Split a string into substrings. Return dynamic array of dynamically + allocated substrings, or NULL if there was an error. Caller is + expected to free the memory, for example with str_array_free. */ +char** str_split(const char* input, const char* sep); + +/* Free a dynamic array of dynamic strings. */ +void str_array_free(char** array); + +/* Return length of a NULL-delimited array of strings. */ +size_t str_array_len(char** array); + +#endif //STRING_SPLIT_H + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE new file mode 100644 index 0000000..e80920e --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE @@ -0,0 +1,15 @@ +Software License Agreement (Apache License) + +Copyright 2011 John Hsu + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt new file mode 100644 index 0000000..4e3bff6 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt @@ -0,0 +1,7 @@ +The URDF (U-Robot Description Format) library + provides core data structures and a simple XML parsers + for populating the class data structures from an URDF file. + +For now, the details of the URDF specifications reside on + http://ros.org/wiki/urdf + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h new file mode 100644 index 0000000..336af0f --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h @@ -0,0 +1,63 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_PARSER_URDF_PARSER_H +#define URDF_PARSER_URDF_PARSER_H + +#include + +#include +#include "tinyxml/tinyxml.h" + +//#include + +#ifndef M_PI +#define M_PI 3.1415925438 +#endif //M_PI + + +#include + + + + +namespace urdf{ + + my_shared_ptr parseURDF(const std::string &xml_string); + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp new file mode 100644 index 0000000..67e9eb1 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp @@ -0,0 +1,137 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" +#include +#include + +using namespace urdf; + +void printTree(my_shared_ptr link,int level = 0) +{ + level+=2; + int count = 0; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + for(int j=0;jname << std::endl; + // first grandchild + printTree(*child,level); + } + else + { + for(int j=0;jname << " has a null child!" << *child << std::endl; + } + } + +} + + +#define MSTRINGIFY(A) #A + + +const char* urdf_char = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + +); + + +int main(int argc, char** argv) +{ + + std::string xml_string; + + if (argc < 2){ + std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; + + xml_string = std::string(urdf_char); + + } else + { + + + std::fstream xml_file(argv[1], std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + } + + my_shared_ptr robot = parseURDF(xml_string); + if (!robot){ + std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + return -1; + } + std::cout << "robot name is: " << robot->getName() << std::endl; + + // get info from parser + std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; + // get root link + my_shared_ptr root_link=robot->getRoot(); + if (!root_link) return -1; + + std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; + + + // print entire tree + printTree(root_link); + return 0; +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp new file mode 100644 index 0000000..bde0c5f --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp @@ -0,0 +1,579 @@ +/********************************************************************* +* Software Ligcense Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#include +#include +#ifdef URDF_USE_BOOST +#include +#else +#include +#endif +#include + +#ifdef URDF_USE_CONSOLE_BRIDGE + #include +#else + #include "urdf/boost_replacement/printf_console.h" +#endif + +#include +#include + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) +{ + jd.clear(); + + // Get joint damping + const char* damping_str = config->Attribute("damping"); + if (damping_str == NULL){ + logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); + jd.damping = 0; + } + else + { + try + { + jd.damping = boost::lexical_cast(damping_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("damping value (%s) is not a float: %s",damping_str, e.what()); + return false; + } + } + + // Get joint friction + const char* friction_str = config->Attribute("friction"); + if (friction_str == NULL){ + logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); + jd.friction = 0; + } + else + { + try + { + jd.friction = boost::lexical_cast(friction_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("friction value (%s) is not a float: %s",friction_str, e.what()); + return false; + } + } + + if (damping_str == NULL && friction_str == NULL) + { + logError("joint dynamics element specified with no damping and no friction"); + return false; + } + else{ + logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); + return true; + } +} + +bool parseJointLimits(JointLimits &jl, TiXmlElement* config) +{ + jl.clear(); + + // Get lower joint limit + const char* lower_str = config->Attribute("lower"); + if (lower_str == NULL){ + logDebug("urdfdom.joint_limit: no lower, defaults to 0"); + jl.lower = 0; + } + else + { + try + { + jl.lower = boost::lexical_cast(lower_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("lower value (%s) is not a float: %s", lower_str, e.what()); + return false; + } + } + + // Get upper joint limit + const char* upper_str = config->Attribute("upper"); + if (upper_str == NULL){ + logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); + jl.upper = 0; + } + else + { + try + { + jl.upper = boost::lexical_cast(upper_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("upper value (%s) is not a float: %s",upper_str, e.what()); + return false; + } + } + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str == NULL){ + logError("joint limit: no effort"); + return false; + } + else + { + try + { + jl.effort = boost::lexical_cast(effort_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("effort value (%s) is not a float: %s",effort_str, e.what()); + return false; + } + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str == NULL){ + logError("joint limit: no velocity"); + return false; + } + else + { + try + { + jl.velocity = boost::lexical_cast(velocity_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointSafety(JointSafety &js, TiXmlElement* config) +{ + js.clear(); + + // Get soft_lower_limit joint limit + const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); + if (soft_lower_limit_str == NULL) + { + logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); + js.soft_lower_limit = 0; + } + else + { + try + { + js.soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); + return false; + } + } + + // Get soft_upper_limit joint limit + const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); + if (soft_upper_limit_str == NULL) + { + logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); + js.soft_upper_limit = 0; + } + else + { + try + { + js.soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); + return false; + } + } + + // Get k_position_ safety "position" gain - not exactly position gain + const char* k_position_str = config->Attribute("k_position"); + if (k_position_str == NULL) + { + logDebug("urdfdom.joint_safety: no k_position, using default value"); + js.k_position = 0; + } + else + { + try + { + js.k_position = boost::lexical_cast(k_position_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); + return false; + } + } + // Get k_velocity_ safety velocity gain + const char* k_velocity_str = config->Attribute("k_velocity"); + if (k_velocity_str == NULL) + { + logError("joint safety: no k_velocity"); + return false; + } + else + { + try + { + js.k_velocity = boost::lexical_cast(k_velocity_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) +{ + jc.clear(); + + // Get rising edge position + const char* rising_position_str = config->Attribute("rising"); + if (rising_position_str == NULL) + { + logDebug("urdfdom.joint_calibration: no rising, using default value"); + jc.rising.reset(0); + } + else + { + try + { + jc.rising.reset(new double(boost::lexical_cast(rising_position_str))); + } + catch (boost::bad_lexical_cast &e) + { + logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); + return false; + } + } + + // Get falling edge position + const char* falling_position_str = config->Attribute("falling"); + if (falling_position_str == NULL) + { + logDebug("urdfdom.joint_calibration: no falling, using default value"); + jc.falling.reset(0); + } + else + { + try + { + jc.falling.reset(new double(boost::lexical_cast(falling_position_str))); + } + catch (boost::bad_lexical_cast &e) + { + logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointMimic(JointMimic &jm, TiXmlElement* config) +{ + jm.clear(); + + // Get name of joint to mimic + const char* joint_name_str = config->Attribute("joint"); + + if (joint_name_str == NULL) + { + logError("joint mimic: no mimic joint specified"); + return false; + } + else + jm.joint_name = joint_name_str; + + // Get mimic multiplier + const char* multiplier_str = config->Attribute("multiplier"); + + if (multiplier_str == NULL) + { + logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); + jm.multiplier = 1; + } + else + { + try + { + jm.multiplier = boost::lexical_cast(multiplier_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); + return false; + } + } + + + // Get mimic offset + const char* offset_str = config->Attribute("offset"); + if (offset_str == NULL) + { + logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); + jm.offset = 0; + } + else + { + try + { + jm.offset = boost::lexical_cast(offset_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("offset value (%s) is not a float: %s",offset_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJoint(Joint &joint, TiXmlElement* config) +{ + joint.clear(); + + // Get Joint Name + const char *name = config->Attribute("name"); + if (!name) + { + logError("unnamed joint found"); + return false; + } + joint.name = name; + + // Get transform from Parent Link to Joint Frame + TiXmlElement *origin_xml = config->FirstChildElement("origin"); + if (!origin_xml) + { + logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); + joint.parent_to_joint_origin_transform.clear(); + } + else + { + if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) + { + joint.parent_to_joint_origin_transform.clear(); + logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); + return false; + } + } + + // Get Parent Link + TiXmlElement *parent_xml = config->FirstChildElement("parent"); + if (parent_xml) + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); + } + else + { + joint.parent_link_name = std::string(pname); + } + } + + // Get Child Link + TiXmlElement *child_xml = config->FirstChildElement("child"); + if (child_xml) + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); + } + else + { + joint.child_link_name = std::string(pname); + } + } + + // Get Joint type + const char* type_char = config->Attribute("type"); + if (!type_char) + { + logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); + return false; + } + + std::string type_str = type_char; + if (type_str == "planar") + joint.type = Joint::PLANAR; + else if (type_str == "floating") + joint.type = Joint::FLOATING; + else if (type_str == "revolute") + joint.type = Joint::REVOLUTE; + else if (type_str == "continuous") + joint.type = Joint::CONTINUOUS; + else if (type_str == "prismatic") + joint.type = Joint::PRISMATIC; + else if (type_str == "fixed") + joint.type = Joint::FIXED; + else + { + logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); + return false; + } + + // Get Joint Axis + if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); + joint.axis = Vector3(1.0, 0.0, 0.0); + } + else{ + if (axis_xml->Attribute("xyz")){ + try { + joint.axis.init(axis_xml->Attribute("xyz")); + } + catch (ParseError &e) { + joint.axis.clear(); + logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); + return false; + } + } + } + } + + // Get limit + TiXmlElement *limit_xml = config->FirstChildElement("limit"); + if (limit_xml) + { + joint.limits.reset(new JointLimits()); + if (!parseJointLimits(*joint.limits, limit_xml)) + { + logError("Could not parse limit element for joint [%s]", joint.name.c_str()); + joint.limits.reset(0); + return false; + } + } + else if (joint.type == Joint::REVOLUTE) + { + logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); + return false; + } + else if (joint.type == Joint::PRISMATIC) + { + logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); + return false; + } + + // Get safety + TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); + if (safety_xml) + { + joint.safety.reset(new JointSafety()); + if (!parseJointSafety(*joint.safety, safety_xml)) + { + logError("Could not parse safety element for joint [%s]", joint.name.c_str()); + joint.safety.reset(0); + return false; + } + } + + // Get calibration + TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); + if (calibration_xml) + { + joint.calibration.reset(new JointCalibration()); + if (!parseJointCalibration(*joint.calibration, calibration_xml)) + { + logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); + joint.calibration.reset(0); + return false; + } + } + + // Get Joint Mimic + TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); + if (mimic_xml) + { + joint.mimic.reset(new JointMimic()); + if (!parseJointMimic(*joint.mimic, mimic_xml)) + { + logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); + joint.mimic.reset(0); + return false; + } + } + + // Get Dynamics + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + joint.dynamics.reset(new JointDynamics()); + if (!parseJointDynamics(*joint.dynamics, prop_xml)) + { + logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); + joint.dynamics.reset(0); + return false; + } + } + + return true; +} + + + + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp new file mode 100644 index 0000000..a224146 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp @@ -0,0 +1,505 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + + +#include +#include +//#include +//#include +#ifdef URDF_USE_BOOST +#include +#else +#include +#endif + +#include +#include +#ifdef URDF_USE_CONSOLE_BRIDGE +#include +#else +#include "urdf/boost_replacement/printf_console.h" +#endif + + + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) +{ + bool has_rgb = false; + bool has_filename = false; + + material.clear(); + + if (!config->Attribute("name")) + { + logError("Material must contain a name attribute"); + return false; + } + + material.name = config->Attribute("name"); + + // texture + TiXmlElement *t = config->FirstChildElement("texture"); + if (t) + { + if (t->Attribute("filename")) + { + material.texture_filename = t->Attribute("filename"); + has_filename = true; + } + } + + // color + TiXmlElement *c = config->FirstChildElement("color"); + if (c) + { + if (c->Attribute("rgba")) { + + try { + material.color.init(c->Attribute("rgba")); + has_rgb = true; + } + catch (ParseError &e) { + material.color.clear(); + logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); + } + } + } + + if (!has_rgb && !has_filename) { + if (!only_name_is_ok) // no need for an error if only name is ok + { + if (!has_rgb) logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); + if (!has_filename) logError(std::string("Material ["+material.name+"] not defined in file").c_str()); + } + return false; + } + return true; +} + + +bool parseSphere(Sphere &s, TiXmlElement *c) +{ + s.clear(); + + s.type = Geometry::SPHERE; + if (!c->Attribute("radius")) + { + logError("Sphere shape must have a radius attribute"); + return false; + } + + try + { + s.radius = boost::lexical_cast(c->Attribute("radius")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what(); + // logError(stm.str().c_str()); + logError("radius issue"); + return false; + } + + return true; +} + +bool parseBox(Box &b, TiXmlElement *c) +{ + b.clear(); + + b.type = Geometry::BOX; + if (!c->Attribute("size")) + { + logError("Box shape has no size attribute"); + return false; + } + try + { + b.dim.init(c->Attribute("size")); + } + catch (ParseError &e) + { + b.dim.clear(); + logError(e.what()); + return false; + } + return true; +} + +bool parseCylinder(Cylinder &y, TiXmlElement *c) +{ + y.clear(); + + y.type = Geometry::CYLINDER; + if (!c->Attribute("length") || + !c->Attribute("radius")) + { + logError("Cylinder shape must have both length and radius attributes"); + return false; + } + + try + { + y.length = boost::lexical_cast(c->Attribute("length")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "length [" << c->Attribute("length") << "] is not a valid float"; + //logError(stm.str().c_str()); + logError("length"); + return false; + } + + try + { + y.radius = boost::lexical_cast(c->Attribute("radius")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + //logError(stm.str().c_str()); + logError("radius"); + return false; + } + return true; +} + + +bool parseMesh(Mesh &m, TiXmlElement *c) +{ + m.clear(); + + m.type = Geometry::MESH; + if (!c->Attribute("filename")) { + logError("Mesh must contain a filename attribute"); + return false; + } + + m.filename = c->Attribute("filename"); + + if (c->Attribute("scale")) { + try { + m.scale.init(c->Attribute("scale")); + } + catch (ParseError &e) { + m.scale.clear(); + logError("Mesh scale was specified, but could not be parsed: %s", e.what()); + return false; + } + } + else + { + m.scale.x = m.scale.y = m.scale.z = 1; + } + return true; +} + +my_shared_ptr parseGeometry(TiXmlElement *g) +{ + my_shared_ptr geom; + if (!g) return geom; + + TiXmlElement *shape = g->FirstChildElement(); + if (!shape) + { + logError("Geometry tag contains no child element."); + return geom; + } + + const std::string type_name = shape->ValueTStr().c_str(); + if (type_name == "sphere") + { + Sphere *s = new Sphere(); + geom.reset(s); + if (parseSphere(*s, shape)) + return geom; + } + else if (type_name == "box") + { + Box *b = new Box(); + geom.reset(b); + if (parseBox(*b, shape)) + return geom; + } + else if (type_name == "cylinder") + { + Cylinder *c = new Cylinder(); + geom.reset(c); + if (parseCylinder(*c, shape)) + return geom; + } + else if (type_name == "mesh") + { + Mesh *m = new Mesh(); + geom.reset(m); + if (parseMesh(*m, shape)) + return geom; + } + else + { + logError("Unknown geometry type '%s'", type_name.c_str()); + return geom; + } + + return my_shared_ptr(); +} + +bool parseInertial(Inertial &i, TiXmlElement *config) +{ + i.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(i.origin, o)) + return false; + } + + TiXmlElement *mass_xml = config->FirstChildElement("mass"); + if (!mass_xml) + { + logError("Inertial element must have a mass element"); + return false; + } + if (!mass_xml->Attribute("value")) + { + logError("Inertial: mass element must have value attribute"); + return false; + } + + try + { + i.mass = boost::lexical_cast(mass_xml->Attribute("value")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "Inertial: mass [" << mass_xml->Attribute("value") + // << "] is not a float"; + //logError(stm.str().c_str()); + logError("Inertial mass issue"); + return false; + } + + TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); + if (!inertia_xml) + { + logError("Inertial element must have inertia element"); + return false; + } + if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && + inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && + inertia_xml->Attribute("izz"))) + { + logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); + return false; + } + try + { + i.ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); + i.ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); + i.ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); + i.iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); + i.iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); + i.izz = boost::lexical_cast(inertia_xml->Attribute("izz")); + } + catch (boost::bad_lexical_cast &e) + { + /* std::stringstream stm; + stm << "Inertial: one of the inertia elements is not a valid double:" + << " ixx [" << inertia_xml->Attribute("ixx") << "]" + << " ixy [" << inertia_xml->Attribute("ixy") << "]" + << " ixz [" << inertia_xml->Attribute("ixz") << "]" + << " iyy [" << inertia_xml->Attribute("iyy") << "]" + << " iyz [" << inertia_xml->Attribute("iyz") << "]" + << " izz [" << inertia_xml->Attribute("izz") << "]"; + logError(stm.str().c_str()); + */ + logError("Inertia error"); + + return false; + } + return true; +} + +bool parseVisual(Visual &vis, TiXmlElement *config) +{ + vis.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(vis.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + vis.geometry = parseGeometry(geom); + if (!vis.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + vis.name = name_char; + + // Material + TiXmlElement *mat = config->FirstChildElement("material"); + if (mat) { + // get material name + if (!mat->Attribute("name")) { + logError("Visual material must contain a name attribute"); + return false; + } + vis.material_name = mat->Attribute("name"); + + // try to parse material element in place + vis.material.reset(new Material()); + if (!parseMaterial(*vis.material, mat, true)) + { + logDebug("urdfdom: material has only name, actual material definition may be in the model"); + } + } + + return true; +} + +bool parseCollision(Collision &col, TiXmlElement* config) +{ + col.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(col.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + col.geometry = parseGeometry(geom); + if (!col.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + col.name = name_char; + + return true; +} + +bool parseLink(Link &link, TiXmlElement* config) +{ + + link.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the link."); + return false; + } + link.name = std::string(name_char); + + // Inertial (optional) + TiXmlElement *i = config->FirstChildElement("inertial"); + if (i) + { + link.inertial.reset(new Inertial()); + if (!parseInertial(*link.inertial, i)) + { + logError("Could not parse inertial element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Multiple Visuals (optional) + for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + { + + my_shared_ptr vis; + vis.reset(new Visual()); + if (parseVisual(*vis, vis_xml)) + { + link.visual_array.push_back(vis); + } + else + { + vis.reset(0); + logError("Could not parse visual element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Visual (optional) + // Assign the first visual to the .visual ptr, if it exists + if (!link.visual_array.empty()) + link.visual = link.visual_array[0]; + + // Multiple Collisions (optional) + for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + { + my_shared_ptr col; + col.reset(new Collision()); + if (parseCollision(*col, col_xml)) + { + link.collision_array.push_back(col); + } + else + { + col.reset(0); + logError("Could not parse collision element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Collision (optional) + // Assign the first collision to the .collision ptr, if it exists + if (!link.collision_array.empty()) + link.collision = link.collision_array[0]; + return true; + +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp new file mode 100644 index 0000000..e8562d0 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp @@ -0,0 +1,240 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ +//#include +#include +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" +#ifdef URDF_USE_CONSOLE_BRIDGE + #include +#else + #include "urdf/boost_replacement/printf_console.h" +#endif + +namespace urdf{ + +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); +bool parseLink(Link &link, TiXmlElement *config); +bool parseJoint(Joint &joint, TiXmlElement *config); + + +my_shared_ptr parseURDF(const std::string &xml_string) +{ + my_shared_ptr model(new ModelInterface); + model->clear(); + + TiXmlDocument xml_doc; + xml_doc.Parse(xml_string.c_str()); + if (xml_doc.Error()) + { + logError(xml_doc.ErrorDesc()); + xml_doc.ClearError(); + model.reset(0); + return model; + } + + TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); + if (!robot_xml) + { + logError("Could not find the 'robot' element in the xml file"); + model.reset(0); + return model; + } + + // Get robot name + const char *name = robot_xml->Attribute("name"); + if (!name) + { + logError("No name given for the robot."); + model.reset(0); + return model; + } + model->name_ = std::string(name); + + // Get all Material elements + for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + { + my_shared_ptr material; + material.reset(new Material); + + try { + parseMaterial(*material, material_xml, false); // material needs to be fully defined here + if (model->getMaterial(material->name)) + { + logError("material '%s' is not unique.", material->name.c_str()); + material.reset(0); + model.reset(0); + return model; + } + else + { + model->materials_.insert(make_pair(material->name,material)); + logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); + } + } + catch (ParseError &e) { + logError("material xml is not initialized correctly"); + material.reset(0); + model.reset(0); + return model; + } + } + + // Get all Link elements + for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + { + my_shared_ptr link; + link.reset(new Link); + model->m_numLinks++; + + try { + parseLink(*link, link_xml); + if (model->getLink(link->name)) + { + logError("link '%s' is not unique.", link->name.c_str()); + model.reset(0); + return model; + } + else + { + // set link visual material + logDebug("urdfdom: setting link '%s' material", link->name.c_str()); + if (link->visual) + { + if (!link->visual->material_name.empty()) + { + if (model->getMaterial(link->visual->material_name)) + { + logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); + link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); + } + else + { + if (link->visual->material) + { + logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); + model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); + } + else + { + logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); + model.reset(0); + return model; + } + } + } + } + + model->links_.insert(make_pair(link->name,link)); + logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); + } + } + catch (ParseError &e) { + logError("link xml is not initialized correctly"); + model.reset(0); + return model; + } + } + if (model->links_.empty()){ + logError("No link elements found in urdf file"); + model.reset(0); + return model; + } + + // Get all Joint elements + for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + my_shared_ptr joint; + joint.reset(new Joint); + model->m_numJoints++; + + if (parseJoint(*joint, joint_xml)) + { + if (model->getJoint(joint->name)) + { + logError("joint '%s' is not unique.", joint->name.c_str()); + model.reset(0); + return model; + } + else + { + model->joints_.insert(make_pair(joint->name,joint)); + logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); + } + } + else + { + logError("joint xml is not initialized correctly"); + model.reset(0); + return model; + } + } + + + // every link has children links and joints, but no parents, so we create a + // local convenience data structure for keeping child->parent relations + std::map parent_link_tree; + parent_link_tree.clear(); + + // building tree: name mapping + try + { + model->initTree(parent_link_tree); + } + catch(ParseError &e) + { + logError("Failed to build tree: %s", e.what()); + model.reset(0); + return model; + } + + // find the root link + try + { + model->initRoot(parent_link_tree); + } + catch(ParseError &e) + { + logError("Failed to find root link: %s", e.what()); + model.reset(0); + return model; + } + + return model; +} + + + +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp new file mode 100644 index 0000000..e90247c --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp @@ -0,0 +1,91 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen, John Hsu */ + + +#include +#include +#include +//#include +#include + +#ifdef URDF_USE_CONSOLE_BRIDGE +#include +#else +#include "urdf/boost_replacement/printf_console.h" +#endif + +#include +#include + + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml) +{ + pose.clear(); + if (xml) + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str != NULL) + { + try { + pose.position.init(xyz_str); + } + catch (ParseError &e) { + logError(e.what()); + return false; + } + } + + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str != NULL) + { + try { + pose.rotation.init(rpy_str); + } + catch (ParseError &e) { + logError(e.what()); + return false; + } + } + } + return true; +} + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp new file mode 100644 index 0000000..4980e17 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp @@ -0,0 +1,85 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseTwist(Twist &twist, TiXmlElement* xml) +{ + twist.clear(); + if (xml) + { + const char* linear_char = xml->Attribute("linear"); + if (linear_char != NULL) + { + try { + twist.linear.init(linear_char); + } + catch (ParseError &e) { + twist.linear.clear(); + logError("Malformed linear string [%s]: %s", linear_char, e.what()); + return false; + } + } + + const char* angular_char = xml->Attribute("angular"); + if (angular_char != NULL) + { + try { + twist.angular.init(angular_char); + } + catch (ParseError &e) { + twist.angular.clear(); + logError("Malformed angular [%s]: %s", angular_char, e.what()); + return false; + } + } + } + return true; +} + +} + + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp new file mode 100644 index 0000000..f30b845 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp @@ -0,0 +1,154 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseModelState(ModelState &ms, TiXmlElement* config) +{ + ms.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the model_state."); + return false; + } + ms.name = std::string(name_char); + + const char *time_stamp_char = config->Attribute("time_stamp"); + if (time_stamp_char) + { + try { + double sec = boost::lexical_cast(time_stamp_char); + ms.time_stamp.set(sec); + } + catch (boost::bad_lexical_cast &e) { + logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); + return false; + } + } + + TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); + if (joint_state_elem) + { + boost::shared_ptr joint_state; + joint_state.reset(new JointState()); + + const char *joint_char = joint_state_elem->Attribute("joint"); + if (joint_char) + joint_state->joint = std::string(joint_char); + else + { + logError("No joint name given for the model_state."); + return false; + } + + // parse position + const char *position_char = joint_state_elem->Attribute("position"); + if (position_char) + { + + std::vector pieces; + boost::split( pieces, position_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->position.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("position element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // parse velocity + const char *velocity_char = joint_state_elem->Attribute("velocity"); + if (velocity_char) + { + + std::vector pieces; + boost::split( pieces, velocity_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->velocity.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // parse effort + const char *effort_char = joint_state_elem->Attribute("effort"); + if (effort_char) + { + + std::vector pieces; + boost::split( pieces, effort_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->effort.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // add to vector + ms.joint_states.push_back(joint_state); + } +}; + + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp new file mode 100644 index 0000000..85a886d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp @@ -0,0 +1,364 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseCamera(Camera &camera, TiXmlElement* config) +{ + camera.clear(); + camera.type = VisualSensor::CAMERA; + + TiXmlElement *image = config->FirstChildElement("image"); + if (image) + { + const char* width_char = image->Attribute("width"); + if (width_char) + { + try + { + camera.width = boost::lexical_cast(width_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image width attribute"); + return false; + } + + const char* height_char = image->Attribute("height"); + if (height_char) + { + try + { + camera.height = boost::lexical_cast(height_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image height attribute"); + return false; + } + + const char* format_char = image->Attribute("format"); + if (format_char) + camera.format = std::string(format_char); + else + { + logError("Camera sensor needs an image format attribute"); + return false; + } + + const char* hfov_char = image->Attribute("hfov"); + if (hfov_char) + { + try + { + camera.hfov = boost::lexical_cast(hfov_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image hfov attribute"); + return false; + } + + const char* near_char = image->Attribute("near"); + if (near_char) + { + try + { + camera.near = boost::lexical_cast(near_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image near attribute"); + return false; + } + + const char* far_char = image->Attribute("far"); + if (far_char) + { + try + { + camera.far = boost::lexical_cast(far_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image far attribute"); + return false; + } + + } + else + { + logError("Camera sensor has no element"); + return false; + } + return true; +} + +bool parseRay(Ray &ray, TiXmlElement* config) +{ + ray.clear(); + ray.type = VisualSensor::RAY; + + TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + if (horizontal) + { + const char* samples_char = horizontal->Attribute("samples"); + if (samples_char) + { + try + { + ray.horizontal_samples = boost::lexical_cast(samples_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = horizontal->Attribute("resolution"); + if (resolution_char) + { + try + { + ray.horizontal_resolution = boost::lexical_cast(resolution_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); + return false; + } + } + + const char* min_angle_char = horizontal->Attribute("min_angle"); + if (min_angle_char) + { + try + { + ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); + return false; + } + } + + const char* max_angle_char = horizontal->Attribute("max_angle"); + if (max_angle_char) + { + try + { + ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); + return false; + } + } + } + + TiXmlElement *vertical = config->FirstChildElement("vertical"); + if (vertical) + { + const char* samples_char = vertical->Attribute("samples"); + if (samples_char) + { + try + { + ray.vertical_samples = boost::lexical_cast(samples_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = vertical->Attribute("resolution"); + if (resolution_char) + { + try + { + ray.vertical_resolution = boost::lexical_cast(resolution_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); + return false; + } + } + + const char* min_angle_char = vertical->Attribute("min_angle"); + if (min_angle_char) + { + try + { + ray.vertical_min_angle = boost::lexical_cast(min_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); + return false; + } + } + + const char* max_angle_char = vertical->Attribute("max_angle"); + if (max_angle_char) + { + try + { + ray.vertical_max_angle = boost::lexical_cast(max_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); + return false; + } + } + } +} + +boost::shared_ptr parseVisualSensor(TiXmlElement *g) +{ + boost::shared_ptr visual_sensor; + + // get sensor type + TiXmlElement *sensor_xml; + if (g->FirstChildElement("camera")) + { + Camera *camera = new Camera(); + visual_sensor.reset(camera); + sensor_xml = g->FirstChildElement("camera"); + if (!parseCamera(*camera, sensor_xml)) + visual_sensor.reset(); + } + else if (g->FirstChildElement("ray")) + { + Ray *ray = new Ray(); + visual_sensor.reset(ray); + sensor_xml = g->FirstChildElement("ray"); + if (!parseRay(*ray, sensor_xml)) + visual_sensor.reset(); + } + else + { + logError("No know sensor types [camera|ray] defined in block"); + } + return visual_sensor; +} + + +bool parseSensor(Sensor &sensor, TiXmlElement* config) +{ + sensor.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the sensor."); + return false; + } + sensor.name = std::string(name_char); + + // parse parent_link_name + const char *parent_link_name_char = config->Attribute("parent_link_name"); + if (!parent_link_name_char) + { + logError("No parent_link_name given for the sensor."); + return false; + } + sensor.parent_link_name = std::string(parent_link_name_char); + + // parse origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(sensor.origin, o)) + return false; + } + + // parse sensor + sensor.sensor = parseVisualSensor(config); + return true; +} + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp new file mode 100644 index 0000000..ddc27c5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp @@ -0,0 +1,71 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseWorld(World &world, TiXmlElement* config) +{ + + // to be implemented + + return true; +} + +bool exportWorld(World &world, TiXmlElement* xml) +{ + TiXmlElement * world_xml = new TiXmlElement("world"); + world_xml->SetAttribute("name", world.name); + + // to be implemented + // exportModels(*world.models, world_xml); + + xml->LinkEndChild(world_xml); + + return true; +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp new file mode 100644 index 0000000..d835eb3 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp @@ -0,0 +1,20 @@ +#include "urdf_parser/urdf_parser.h" +#include +#include + +int main(int argc, char** argv){ + while (true){ + std::string xml_string; + std::fstream xml_file(argv[1], std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + + + urdf::parseURDF(xml_string); + } +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE new file mode 100644 index 0000000..e80920e --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE @@ -0,0 +1,15 @@ +Software License Agreement (Apache License) + +Copyright 2011 John Hsu + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt new file mode 100644 index 0000000..6a841d5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt @@ -0,0 +1,6 @@ +The URDF (U-Robot Description Format) headers + provides core data structure headers for URDF. + +For now, the details of the URDF specifications reside on + http://ros.org/wiki/urdf + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h new file mode 100644 index 0000000..24222f1 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h @@ -0,0 +1,53 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +// URDF exceptions +#ifndef URDF_INTERFACE_EXCEPTION_H_ +#define URDF_INTERFACE_EXCEPTION_H_ + +#include +#include + +namespace urdf +{ + +class ParseError: public std::runtime_error +{ +public: + ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h new file mode 100644 index 0000000..9c15dd7 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h @@ -0,0 +1,101 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Josh Faust */ + +#ifndef URDF_INTERFACE_COLOR_H +#define URDF_INTERFACE_COLOR_H + +#include +#include +#include +//#include +//#include + +namespace urdf +{ + +class Color +{ +public: + Color() {this->clear();}; + float r; + float g; + float b; + float a; + + void clear() + { + r = g = b = 0.0f; + a = 1.0f; + } + bool init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector rgba; + + boost::split( pieces, vector_str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + try + { + rgba.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + return false; + } + } + } + + if (rgba.size() != 4) + { + return false; + } + this->r = rgba[0]; + this->g = rgba[1]; + this->b = rgba[2]; + this->a = rgba[3]; + + return true; + }; +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h new file mode 100644 index 0000000..cd889dc --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h @@ -0,0 +1,234 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_JOINT_H +#define URDF_INTERFACE_JOINT_H + +#include +#include +#ifdef URDF_USE_BOOST +#include +#define my_shared_ptr my_shared_ptr +#else +#include +#endif + +#include + + +namespace urdf{ + +class Link; + +class JointDynamics +{ +public: + JointDynamics() { this->clear(); }; + double damping; + double friction; + + void clear() + { + damping = 0; + friction = 0; + }; +}; + +class JointLimits +{ +public: + JointLimits() { this->clear(); }; + double lower; + double upper; + double effort; + double velocity; + + void clear() + { + lower = 0; + upper = 0; + effort = 0; + velocity = 0; + }; +}; + +/// \brief Parameters for Joint Safety Controllers +class JointSafety +{ +public: + /// clear variables on construction + JointSafety() { this->clear(); }; + + /// + /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. + /// + /// Basic safety controller operation is as follows + /// + /// current safety controllers will take effect on joints outside the position range below: + /// + /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, + /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] + /// + /// if (joint_position is outside of the position range above) + /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) + /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) + /// else + /// velocity_limit_min = -JointLimits::velocity + /// velocity_limit_max = JointLimits::velocity + /// + /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, + /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] + /// + /// if (joint_velocity is outside of the velocity range above) + /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) + /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) + /// else + /// effort_limit_min = -JointLimits::effort + /// effort_limit_max = JointLimits::effort + /// + /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] + /// + /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits + /// + double soft_upper_limit; + double soft_lower_limit; + double k_position; + double k_velocity; + + void clear() + { + soft_upper_limit = 0; + soft_lower_limit = 0; + k_position = 0; + k_velocity = 0; + }; +}; + + +class JointCalibration +{ +public: + JointCalibration() { this->clear(); }; + double reference_position; + my_shared_ptr rising, falling; + + void clear() + { + reference_position = 0; + }; +}; + +class JointMimic +{ +public: + JointMimic() { this->clear(); }; + double offset; + double multiplier; + std::string joint_name; + + void clear() + { + offset = 0.0; + multiplier = 0.0; + joint_name.clear(); + }; +}; + + +class Joint +{ +public: + + Joint() { this->clear(); }; + + std::string name; + enum + { + UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED + } type; + + /// \brief type_ meaning of axis_ + /// ------------------------------------------------------ + /// UNKNOWN unknown type + /// REVOLUTE rotation axis + /// PRISMATIC translation axis + /// FLOATING N/A + /// PLANAR plane normal axis + /// FIXED N/A + Vector3 axis; + + /// child Link element + /// child link frame is the same as the Joint frame + std::string child_link_name; + + /// parent Link element + /// origin specifies the transform from Parent Link to Joint Frame + std::string parent_link_name; + /// transform from Parent Link frame to Joint frame + Pose parent_to_joint_origin_transform; + + /// Joint Dynamics + my_shared_ptr dynamics; + + /// Joint Limits + my_shared_ptr limits; + + /// Unsupported Hidden Feature + my_shared_ptr safety; + + /// Unsupported Hidden Feature + my_shared_ptr calibration; + + /// Option to Mimic another Joint + my_shared_ptr mimic; + + void clear() + { + this->axis.clear(); + this->child_link_name.clear(); + this->parent_link_name.clear(); + this->parent_to_joint_origin_transform.clear(); + this->dynamics.reset(0); + this->limits.reset(0); + this->safety.reset(0); + this->calibration.reset(0); + this->type = UNKNOWN; + }; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h new file mode 100644 index 0000000..22e64cf --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h @@ -0,0 +1,262 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_LINK_H +#define URDF_INTERFACE_LINK_H + +#include +#include +#include + + +#ifdef URDF_USE_BOOST + #include + #include +#else + #include +#endif + +#include "joint.h" +#include "color.h" +#include "pose.h" + +namespace urdf{ + +class Geometry +{ +public: + enum {SPHERE, BOX, CYLINDER, MESH} type; + + virtual ~Geometry(void) + { + } +}; + +class Sphere : public Geometry +{ +public: + Sphere() { this->clear(); type = SPHERE; }; + double radius; + + void clear() + { + radius = 0; + }; +}; + +class Box : public Geometry +{ +public: + Box() { this->clear(); type = BOX; } + Vector3 dim; + + void clear() + { + this->dim.clear(); + }; +}; + +class Cylinder : public Geometry +{ +public: + Cylinder() { this->clear(); type = CYLINDER; }; + double length; + double radius; + + void clear() + { + length = 0; + radius = 0; + }; +}; + +class Mesh : public Geometry +{ +public: + Mesh() { this->clear(); type = MESH; }; + std::string filename; + Vector3 scale; + + void clear() + { + filename.clear(); + // default scale + scale.x = 1; + scale.y = 1; + scale.z = 1; + }; +}; + +class Material +{ +public: + Material() { this->clear(); }; + std::string name; + std::string texture_filename; + Color color; + + void clear() + { + color.clear(); + texture_filename.clear(); + name.clear(); + }; +}; + +class Inertial +{ +public: + Inertial() { this->clear(); }; + Pose origin; + double mass; + double ixx,ixy,ixz,iyy,iyz,izz; + + void clear() + { + origin.clear(); + mass = 0; + ixx = ixy = ixz = iyy = iyz = izz = 0; + }; +}; + +class Visual +{ +public: + Visual() { this->clear(); }; + Pose origin; + my_shared_ptr geometry; + + std::string material_name; + my_shared_ptr material; + + void clear() + { + origin.clear(); + material_name.clear(); + material.reset(0); + geometry.reset(0); + name.clear(); + }; + + std::string name; +}; + +class Collision +{ +public: + Collision() { this->clear(); }; + Pose origin; + my_shared_ptr geometry; + + void clear() + { + origin.clear(); + geometry.reset(0); + name.clear(); + }; + + std::string name; + +}; + + +class Link +{ +public: + Link() { this->clear(); }; + + std::string name; + + /// inertial element + my_shared_ptr inertial; + + /// visual element + my_shared_ptr visual; + + /// collision element + my_shared_ptr collision; + + /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) + std::vector > collision_array; + + /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array) + std::vector > visual_array; + + /// Parent Joint element + /// explicitly stating "parent" because we want directional-ness for tree structure + /// every link can have one parent + my_shared_ptr parent_joint; + + std::vector > child_joints; + std::vector > child_links; + + mutable int m_link_index; + + const Link* getParent() const + {return parent_link_;} + + void setParent(const my_shared_ptr &parent) + { + parent_link_ = parent.get(); + } + + void clear() + { + this->name.clear(); + this->inertial.reset(0); + this->visual.reset(0); + this->collision.reset(0); + this->parent_joint.reset(0); + this->child_joints.clear(); + this->child_links.clear(); + this->collision_array.clear(); + this->visual_array.clear(); + this->m_link_index=-1; + this->parent_link_ = NULL; + }; + +private: +// boost::weak_ptr parent_link_; + const Link* parent_link_; + +}; + + + + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h new file mode 100644 index 0000000..8e93d94 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h @@ -0,0 +1,220 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_MODEL_H +#define URDF_INTERFACE_MODEL_H + +#include +#include +//#include +#include +#include //printf +#include + +namespace urdf { + +class ModelInterface +{ +public: + my_shared_ptr getRoot(void) const{return this->root_link_;}; + my_shared_ptr getLink(const std::string& name) const + { + my_shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(0); + else + ptr = this->links_.find(name)->second; + return ptr; + }; + + my_shared_ptr getJoint(const std::string& name) const + { + my_shared_ptr ptr; + if (this->joints_.find(name) == this->joints_.end()) + ptr.reset(0); + else + ptr = this->joints_.find(name)->second; + return ptr; + }; + + + const std::string& getName() const {return name_;}; + void getLinks(std::vector >& links) const + { + for (std::map >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) + { + links.push_back(link->second); + } + }; + + void clear() + { + m_numLinks=0; + m_numJoints = 0; + name_.clear(); + this->links_.clear(); + this->joints_.clear(); + this->materials_.clear(); + this->root_link_.reset(0); + }; + + /// non-const getLink() + void getLink(const std::string& name,my_shared_ptr &link) const + { + my_shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(0); + else + ptr = this->links_.find(name)->second; + link = ptr; + }; + + /// non-const getMaterial() + my_shared_ptr getMaterial(const std::string& name) const + { + my_shared_ptr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(0); + else + ptr = this->materials_.find(name)->second; + return ptr; + }; + + void initTree(std::map &parent_link_tree) + { + // loop through all joints, for every link, assign children links and children joints + for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) + { + std::string parent_link_name = joint->second->parent_link_name; + std::string child_link_name = joint->second->child_link_name; + + if (parent_link_name.empty() || child_link_name.empty()) + { + assert(0); + + // throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); + } + else + { + // find child and parent links + my_shared_ptr child_link, parent_link; + this->getLink(child_link_name, child_link); + if (!child_link) + { + printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() ); + assert(0); +// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); + } + this->getLink(parent_link_name, parent_link); + if (!parent_link) + { + assert(0); + +/* throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" to your urdf file."); + + */} + + //set parent link for child link + child_link->setParent(parent_link); + + //set parent joint for child link + child_link->parent_joint = joint->second; + + //set child joint for parent link + parent_link->child_joints.push_back(joint->second); + + //set child link for parent link + parent_link->child_links.push_back(child_link); + + // fill in child/parent string map + parent_link_tree[child_link->name] = parent_link_name; + } + } + } + + void initRoot(const std::map &parent_link_tree) + { + this->root_link_.reset(0); + + // find the links that have no parent in the tree + for (std::map >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::const_iterator parent = parent_link_tree.find(l->first); + if (parent == parent_link_tree.end()) + { + // store root link + if (!this->root_link_) + { + getLink(l->first, this->root_link_); + } + // we already found a root link + else + { + assert(0); + // throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); + } + } + } + if (!this->root_link_) + { + assert(0); + //throw ParseError("No root link found. The robot xml is not a valid tree."); + } + } + + + /// \brief complete list of Links + std::map > links_; + /// \brief complete list of Joints + std::map > joints_; + /// \brief complete list of Materials + std::map > materials_; + + /// \brief The name of the robot model + std::string name_; + + /// \brief The root is always a link (the parent of the tree describing the robot) + my_shared_ptr root_link_; + + int m_numLinks;//includes parent + int m_numJoints; + + +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h new file mode 100644 index 0000000..93183c8 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h @@ -0,0 +1,265 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_POSE_H +#define URDF_INTERFACE_POSE_H + +#include +//#include +#include +#include +#ifndef M_PI +#define M_PI 3.141592538 +#endif //M_PI + +#ifdef URDF_USE_BOOST + #include + #include +#else + #include + #include +#endif //URDF_USE_BOOST + +#include +#include + +namespace urdf{ + +class Vector3 +{ +public: + Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; + Vector3() {this->clear();}; + double x; + double y; + double z; + + void clear() {this->x=this->y=this->z=0.0;}; + void init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector xyz; + boost::split( pieces, vector_str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + xyz.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + assert(0); + // throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); + } + } + } + + + + if (xyz.size() != 3) + { + assert(0); + // throw ParseError("Parser found " + boost::lexical_cast(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); + } + this->x = xyz[0]; + this->y = xyz[1]; + this->z = xyz[2]; + } + + Vector3 operator+(Vector3 vec) + { + return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); + }; +}; + +class Rotation +{ +public: + Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; + Rotation() {this->clear();}; + void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const + { + quat_x = this->x; + quat_y = this->y; + quat_z = this->z; + quat_w = this->w; + }; + void getRPY(double &roll,double &pitch,double &yaw) const + { + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = this->x * this->x; + sqy = this->y * this->y; + sqz = this->z * this->z; + sqw = this->w * this->w; + + roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); + double sarg = -2 * (this->x*this->z - this->w*this->y); + pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); + yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); + + }; + void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) + { + this->x = quat_x; + this->y = quat_y; + this->z = quat_z; + this->w = quat_w; + this->normalize(); + }; + void setFromRPY(double roll, double pitch, double yaw) + { + double phi, the, psi; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); + this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); + this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); + this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); + + this->normalize(); + }; + + double x,y,z,w; + + void init(const std::string &rotation_str) + { + this->clear(); + Vector3 rpy; + rpy.init(rotation_str); + setFromRPY(rpy.x, rpy.y, rpy.z); + } + + void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } + + void normalize() + { + double s = sqrt(this->x * this->x + + this->y * this->y + + this->z * this->z + + this->w * this->w); + if (s == 0.0) + { + this->x = 0.0; + this->y = 0.0; + this->z = 0.0; + this->w = 1.0; + } + else + { + this->x /= s; + this->y /= s; + this->z /= s; + this->w /= s; + } + }; + + // Multiplication operator (copied from gazebo) + Rotation operator*( const Rotation &qt ) const + { + Rotation c; + + c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; + c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; + c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; + c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; + + return c; + }; + /// Rotate a vector using the quaternion + Vector3 operator*(Vector3 vec) const + { + Rotation tmp; + Vector3 result; + + tmp.w = 0.0; + tmp.x = vec.x; + tmp.y = vec.y; + tmp.z = vec.z; + + tmp = (*this) * (tmp * this->GetInverse()); + + result.x = tmp.x; + result.y = tmp.y; + result.z = tmp.z; + + return result; + }; + // Get the inverse of this quaternion + Rotation GetInverse() const + { + Rotation q; + + double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; + + if (norm > 0.0) + { + q.w = this->w / norm; + q.x = -this->x / norm; + q.y = -this->y / norm; + q.z = -this->z / norm; + } + + return q; + }; + + +}; + +class Pose +{ +public: + Pose() { this->clear(); }; + + Vector3 position; + Rotation rotation; + + void clear() + { + this->position.clear(); + this->rotation.clear(); + }; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h new file mode 100644 index 0000000..5560de3 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h @@ -0,0 +1,68 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#ifndef URDF_TWIST_H +#define URDF_TWIST_H + +#include +#include +#include +#include +#include + +namespace urdf{ + + +class Twist +{ +public: + Twist() { this->clear(); }; + + Vector3 linear; + // Angular velocity represented by Euler angles + Vector3 angular; + + void clear() + { + this->linear.clear(); + this->angular.clear(); + }; +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h new file mode 100644 index 0000000..b132719 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h @@ -0,0 +1,141 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#ifndef URDF_MODEL_STATE_H +#define URDF_MODEL_STATE_H + +#include +#include +#include +#include +#include + +#include "urdf_model/pose.h" +#include + + +namespace urdf{ + +class Time +{ +public: + Time() { this->clear(); }; + + void set(double _seconds) + { + this->sec = (int32_t)(floor(_seconds)); + this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); + this->Correct(); + }; + + operator double () + { + return (static_cast(this->sec) + + static_cast(this->nsec)*1e-9); + }; + + int32_t sec; + int32_t nsec; + + void clear() + { + this->sec = 0; + this->nsec = 0; + }; +private: + void Correct() + { + // Make any corrections + if (this->nsec >= 1e9) + { + this->sec++; + this->nsec = (int32_t)(this->nsec - 1e9); + } + else if (this->nsec < 0) + { + this->sec--; + this->nsec = (int32_t)(this->nsec + 1e9); + } + }; +}; + + +class JointState +{ +public: + JointState() { this->clear(); }; + + /// joint name + std::string joint; + + std::vector position; + std::vector velocity; + std::vector effort; + + void clear() + { + this->joint.clear(); + this->position.clear(); + this->velocity.clear(); + this->effort.clear(); + } +}; + +class ModelState +{ +public: + ModelState() { this->clear(); }; + + /// state name must be unique + std::string name; + + Time time_stamp; + + void clear() + { + this->name.clear(); + this->time_stamp.set(0); + this->joint_states.clear(); + }; + + std::vector > joint_states; + +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h new file mode 100644 index 0000000..05f1917 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h @@ -0,0 +1,42 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef URDF_MODEL_STATE_TWIST_ +#define URDF_MODEL_STATE_TWIST_ + +#warning "Please Use #include " + +#include + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h new file mode 100644 index 0000000..3b99695 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h @@ -0,0 +1,176 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +/* example + + + + + 1.5708 + + + + + + + + + + + + + + +*/ + + + +#ifndef URDF_SENSOR_H +#define URDF_SENSOR_H + +#include +#include +#include +#include +#include +#include "urdf_model/pose.h" +#include "urdf_model/joint.h" +#include "urdf_model/link.h" + +namespace urdf{ + +class VisualSensor +{ +public: + enum {CAMERA, RAY} type; + virtual ~VisualSensor(void) + { + } +}; + +class Camera : public VisualSensor +{ +public: + Camera() { this->clear(); }; + unsigned int width, height; + /// format is optional: defaults to R8G8B8), but can be + /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + std::string format; + double hfov; + double near; + double far; + + void clear() + { + hfov = 0; + width = 0; + height = 0; + format.clear(); + near = 0; + far = 0; + }; +}; + +class Ray : public VisualSensor +{ +public: + Ray() { this->clear(); }; + unsigned int horizontal_samples; + double horizontal_resolution; + double horizontal_min_angle; + double horizontal_max_angle; + unsigned int vertical_samples; + double vertical_resolution; + double vertical_min_angle; + double vertical_max_angle; + + void clear() + { + // set defaults + horizontal_samples = 1; + horizontal_resolution = 1; + horizontal_min_angle = 0; + horizontal_max_angle = 0; + vertical_samples = 1; + vertical_resolution = 1; + vertical_min_angle = 0; + vertical_max_angle = 0; + }; +}; + + +class Sensor +{ +public: + Sensor() { this->clear(); }; + + /// sensor name must be unique + std::string name; + + /// update rate in Hz + double update_rate; + + /// transform from parent frame to optical center + /// with z-forward and x-right, y-down + Pose origin; + + /// sensor + boost::shared_ptr sensor; + + + /// Parent link element name. A pointer is stored in parent_link_. + std::string parent_link_name; + + boost::shared_ptr getParent() const + {return parent_link_.lock();}; + + void setParent(boost::shared_ptr parent) + { this->parent_link_ = parent; } + + void clear() + { + this->name.clear(); + this->sensor.reset(); + this->parent_link_name.clear(); + this->parent_link_.reset(); + }; + +private: + boost::weak_ptr parent_link_; + +}; +} +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h new file mode 100644 index 0000000..eb13fc4 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h @@ -0,0 +1,114 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +/* encapsulates components in a world + see http://ros.org/wiki/usdf/XML/urdf_world and + for details +*/ +/* example world XML + + + + + ... + + + + + + + + + + + + + + + + + + + +*/ + +#ifndef USDF_STATE_H +#define USDF_STATE_H + +#include +#include +#include +#include +#include +#include + +#include "urdf_model/model.h" +#include "urdf_model/pose.h" +#include "urdf_model/twist.h" + +namespace urdf{ + +class Entity +{ +public: + boost::shared_ptr model; + Pose origin; + Twist twist; +}; + +class World +{ +public: + World() { this->clear(); }; + + /// world name must be unique + std::string name; + + std::vector models; + + void initXml(TiXmlElement* config); + + void clear() + { + this->name.clear(); + }; +}; +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/urdfreader.cc b/3rdparty/rbdl/addons/urdfreader/urdfreader.cc new file mode 100644 index 0000000..70cc424 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/urdfreader.cc @@ -0,0 +1,315 @@ +#include + +#include "urdfreader.h" + +#include +#include +#include +#include +#include + +#ifdef RBDL_USE_ROS_URDF_LIBRARY +#include +#include + +typedef boost::shared_ptr LinkPtr; +typedef const boost::shared_ptr ConstLinkPtr; +typedef boost::shared_ptr JointPtr; +typedef boost::shared_ptr ModelPtr; + +#else +#include +#include + +typedef my_shared_ptr LinkPtr; +typedef const my_shared_ptr ConstLinkPtr; +typedef my_shared_ptr JointPtr; +typedef my_shared_ptr ModelPtr; + +#endif + +using namespace std; + +namespace RigidBodyDynamics { + +namespace Addons { + +using namespace Math; + +typedef vector URDFLinkVector; +typedef vector URDFJointVector; +typedef map URDFLinkMap; +typedef map URDFJointMap; + +bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose) { + LinkPtr urdf_root_link; + + URDFLinkMap link_map; + link_map = urdf_model->links_; + + URDFJointMap joint_map; + joint_map = urdf_model->joints_; + + vector joint_names; + + stack link_stack; + stack joint_index_stack; + + // add the bodies in a depth-first order of the model tree + link_stack.push (link_map[(urdf_model->getRoot()->name)]); + + // add the root body + ConstLinkPtr& root = urdf_model->getRoot (); + Vector3d root_inertial_rpy; + Vector3d root_inertial_position; + Matrix3d root_inertial_inertia; + double root_inertial_mass; + + if (root->inertial) { + root_inertial_mass = root->inertial->mass; + + root_inertial_position.set ( + root->inertial->origin.position.x, + root->inertial->origin.position.y, + root->inertial->origin.position.z); + + root_inertial_inertia(0,0) = root->inertial->ixx; + root_inertial_inertia(0,1) = root->inertial->ixy; + root_inertial_inertia(0,2) = root->inertial->ixz; + + root_inertial_inertia(1,0) = root->inertial->ixy; + root_inertial_inertia(1,1) = root->inertial->iyy; + root_inertial_inertia(1,2) = root->inertial->iyz; + + root_inertial_inertia(2,0) = root->inertial->ixz; + root_inertial_inertia(2,1) = root->inertial->iyz; + root_inertial_inertia(2,2) = root->inertial->izz; + + root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]); + + Body root_link = Body (root_inertial_mass, + root_inertial_position, + root_inertial_inertia); + + Joint root_joint (JointTypeFixed); + if (floating_base) { + root_joint = JointTypeFloatingBase; + } + + SpatialTransform root_joint_frame = SpatialTransform (); + + if (verbose) { + cout << "+ Adding Root Body " << endl; + cout << " joint frame: " << root_joint_frame << endl; + if (floating_base) { + cout << " joint type : floating" << endl; + } else { + cout << " joint type : fixed" << endl; + } + cout << " body inertia: " << endl << root_link.mInertia << endl; + cout << " body mass : " << root_link.mMass << endl; + cout << " body name : " << root->name << endl; + } + + rbdl_model->AppendBody(root_joint_frame, + root_joint, + root_link, + root->name); + } + + if (link_stack.top()->child_joints.size() > 0) { + joint_index_stack.push(0); + } + + while (link_stack.size() > 0) { + LinkPtr cur_link = link_stack.top(); + unsigned int joint_idx = joint_index_stack.top(); + + if (joint_idx < cur_link->child_joints.size()) { + JointPtr cur_joint = cur_link->child_joints[joint_idx]; + + // increment joint index + joint_index_stack.pop(); + joint_index_stack.push (joint_idx + 1); + + link_stack.push (link_map[cur_joint->child_link_name]); + joint_index_stack.push(0); + + if (verbose) { + for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) { + cout << " "; + } + cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl; + } + + joint_names.push_back(cur_joint->name); + } else { + link_stack.pop(); + joint_index_stack.pop(); + } + } + + unsigned int j; + for (j = 0; j < joint_names.size(); j++) { + JointPtr urdf_joint = joint_map[joint_names[j]]; + LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name]; + LinkPtr urdf_child = link_map[urdf_joint->child_link_name]; + + // determine where to add the current joint and child body + unsigned int rbdl_parent_id = 0; + + if (urdf_parent->name != "base_joint" && rbdl_model->mBodies.size() != 1) + rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str()); + + if (rbdl_parent_id == std::numeric_limits::max()) + cerr << "Error while processing joint '" << urdf_joint->name + << "': parent link '" << urdf_parent->name + << "' could not be found." << endl; + + //cout << "joint: " << urdf_joint->name << "\tparent = " << urdf_parent->name << " child = " << urdf_child->name << " parent_id = " << rbdl_parent_id << endl; + + // create the joint + Joint rbdl_joint; + if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) { + rbdl_joint = Joint (SpatialVector (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.)); + } else if (urdf_joint->type == urdf::Joint::PRISMATIC) { + rbdl_joint = Joint (SpatialVector (0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z)); + } else if (urdf_joint->type == urdf::Joint::FIXED) { + rbdl_joint = Joint (JointTypeFixed); + } else if (urdf_joint->type == urdf::Joint::FLOATING) { + // todo: what order of DoF should be used? + rbdl_joint = Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.)); + } else if (urdf_joint->type == urdf::Joint::PLANAR) { + // todo: which two directions should be used that are perpendicular + // to the specified axis? + cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl; + return false; + } + + // compute the joint transformation + Vector3d joint_rpy; + Vector3d joint_translation; + urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]); + joint_translation.set ( + urdf_joint->parent_to_joint_origin_transform.position.x, + urdf_joint->parent_to_joint_origin_transform.position.y, + urdf_joint->parent_to_joint_origin_transform.position.z + ); + SpatialTransform rbdl_joint_frame = + Xrot (joint_rpy[0], Vector3d (1., 0., 0.)) + * Xrot (joint_rpy[1], Vector3d (0., 1., 0.)) + * Xrot (joint_rpy[2], Vector3d (0., 0., 1.)) + * Xtrans (Vector3d ( + joint_translation + )); + + // assemble the body + Vector3d link_inertial_position; + Vector3d link_inertial_rpy; + Matrix3d link_inertial_inertia = Matrix3d::Zero(); + double link_inertial_mass = 0.; + + // but only if we actually have inertial data + if (urdf_child->inertial) { + link_inertial_mass = urdf_child->inertial->mass; + + link_inertial_position.set ( + urdf_child->inertial->origin.position.x, + urdf_child->inertial->origin.position.y, + urdf_child->inertial->origin.position.z + ); + urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]); + + link_inertial_inertia(0,0) = urdf_child->inertial->ixx; + link_inertial_inertia(0,1) = urdf_child->inertial->ixy; + link_inertial_inertia(0,2) = urdf_child->inertial->ixz; + + link_inertial_inertia(1,0) = urdf_child->inertial->ixy; + link_inertial_inertia(1,1) = urdf_child->inertial->iyy; + link_inertial_inertia(1,2) = urdf_child->inertial->iyz; + + link_inertial_inertia(2,0) = urdf_child->inertial->ixz; + link_inertial_inertia(2,1) = urdf_child->inertial->iyz; + link_inertial_inertia(2,2) = urdf_child->inertial->izz; + + if (link_inertial_rpy != Vector3d (0., 0., 0.)) { + cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl; + return false; + } + } + + Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia); + + if (verbose) { + cout << "+ Adding Body " << endl; + cout << " parent_id : " << rbdl_parent_id << endl; + cout << " joint frame: " << rbdl_joint_frame << endl; + cout << " joint dofs : " << rbdl_joint.mDoFCount << endl; + for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) { + cout << " " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl; + } + cout << " body inertia: " << endl << rbdl_body.mInertia << endl; + cout << " body mass : " << rbdl_body.mMass << endl; + cout << " body name : " << urdf_child->name << endl; + } + + if (urdf_joint->type == urdf::Joint::FLOATING) { + Matrix3d zero_matrix = Matrix3d::Zero(); + Body null_body (0., Vector3d::Zero(3), zero_matrix); + Joint joint_txtytz(JointTypeTranslationXYZ); + string trans_body_name = urdf_child->name + "_Translate"; + rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body, trans_body_name); + + Joint joint_euler_zyx (JointTypeEulerXYZ); + rbdl_model->AppendBody (SpatialTransform(), joint_euler_zyx, rbdl_body, urdf_child->name); + } else { + rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name); + } + } + + return true; +} + +RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose) { + ifstream model_file (filename); + if (!model_file) { + cerr << "Error opening file '" << filename << "'." << endl; + abort(); + } + + // reserve memory for the contents of the file + string model_xml_string; + model_file.seekg(0, std::ios::end); + model_xml_string.reserve(model_file.tellg()); + model_file.seekg(0, std::ios::beg); + model_xml_string.assign((std::istreambuf_iterator(model_file)), std::istreambuf_iterator()); + + model_file.close(); + + return URDFReadFromString (model_xml_string.c_str(), model, floating_base, verbose); +} + +RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose) { + assert (model); + + ModelPtr urdf_model = urdf::parseURDF (model_xml_string); + + if (!construct_model (model, urdf_model, floating_base, verbose)) { + cerr << "Error constructing model from urdf file." << endl; + return false; + } + + model->gravity.set (0., 0., -9.81); + + return true; +} + +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/urdfreader.h b/3rdparty/rbdl/addons/urdfreader/urdfreader.h new file mode 100644 index 0000000..19d9653 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/urdfreader.h @@ -0,0 +1,18 @@ +#ifndef RBDL_URDFREADER_H +#define RBDL_URDFREADER_H + +#include + +namespace RigidBodyDynamics { + +struct Model; + +namespace Addons { + RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose = false); + RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose = false); +} + +} + +/* _RBDL_URDFREADER_H */ +#endif diff --git a/3rdparty/rbdl/doc/Mainpage.h b/3rdparty/rbdl/doc/Mainpage.h new file mode 100644 index 0000000..dffd789 --- /dev/null +++ b/3rdparty/rbdl/doc/Mainpage.h @@ -0,0 +1,170 @@ +/** \file Mainpage.h + * \mainpage Mainpage + * \image html rbdl_logo.png + * + * This is the documentation of RBDL, the Rigid Body Dynamics Library. The + * library contains highly efficient code for both forward and inverse + * dynamics for kinematic chains and branched models. It includes: + * + * \li Recursive Newton Euler Algorithm (RNEA) + * \li Composite Rigid Body Algorithm (CRBA) + * \li Articulated Body Algorithm (ABA). + * + * Furthermore it contains code for forward and inverse kinematics, + * computations of Jacobians, contact handling. \link + * RigidBodyDynamics::Model Models \endlink can be loaded from Lua scripts + * or URDF files. + * + * The code is developed by Martin Felis + * at the research group Optimization in Robotics and + * Biomechanics (ORB) of the Interdisciplinary Center for + * Scientific Computing (IWR) at Heidelberg University. The code + * is heavily inspired by the pseudo code of the book "Rigid Body Dynamics + * Algorithms" of Roy + * Featherstone. + * + * The code has no external dependencies but for optimal performance it is + * advised to use version 3 of the Eigen math library. More information about it can + * be found here: http://eigen.tuxfamily.org/. The Eigen3 library + * must be obtained and installed separately. + * + * \section download Download + * + * You can download the most recent stable version as zip file from + * here:
+ * https://bitbucket.org/rbdl/rbdl/get/default.zip + * + * All development takes place on Bitbucket and you can follow RBDL's + * development here:
+ * https://bitbucket.org/rbdl/rbdl + * + * \section recent_changes Recent Changes + *
    + *
  • 28 April 2016: New release 2.5.0: + *
      + *
    • Added an experimental Cython based Python wrapper of RBDL. The API is + * very close to the C++ API. For a brief glimpse of the API see + * \ref PythonExample "Python Example"
      • + *
    • Matthew Millard added CustomJoints which allow to create different joint + * types completely by user code. They are implemented as proxy joints for + * which their behaviour is specified using virtual functions.
      • + *
    • Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + * the joint space inertia matrix with a vector in O(n) time.
      • + *
    • Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + * the floating base joint.
      • + *
    • Loading of floating base URDF models must now be specified as a third + * parameter to URDFReadFromFile() and URDFReadFromString()
      • + *
    • Added the URDF code from Bullet3 which gets used when ROS is not found. + * Otherwise use the URDF libraries found via Catkin.
      • + *
    • Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + * that compute both linear and angular quantities
      • + *
    • Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + * JointTypeFloatingBase instead.
      • + *
    • Fixed building issues when building DLL with MSVC++.
      • + *
    + *
    • + *
  • 20 March 2016: New bugfix version 2.4.1: + *
      + *
    • critical: fixed termination criterion for + * InverseKinematics(). The termination criterion would be evaluated too + * early and thus report convergence too early. This was reported + * independently by Kevin Stein, Yun Fei, and Davide Corradi. Thanks + * for the reports!
      • + *
    • critical: fixed CompositeRigidBodyAlgorithm() when using + * spherical joints (thanks to Sébastien Barthélémy for + * reporting!)
      • + * + *
    + *
    • + *
  • 23 February 2015: New version 2.4.0: + *
      + *
    • Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() and ComputeContactImpulsesRangeSpaceSparse()
      • + *
    • Added null-space method ForwardDynamicsContactsNullSpace() and ComputeContactImpulsesNullSpace()
      • + *
    • Renamed ForwardDynamicsContactsLagrangian() to ForwardDynamicsContactsDirect() and ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect()
      • + *
    • Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis()
      • + *
    • Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The functionality has been integrated into CalcCenterOfMass().
      • + *
    • CalcPointJacobian() does not clear the argument of the result anymore. Caller has to ensure that the matrix was set to zero before using this function.
      • + *
    • Added optional workspace parameters for ForwardDynamicsLagrangian() to optionally reduce memory allocations
      • + *
    • Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ which are equivalent to the emulated multidof joints but faster.
      • + *
    • Added optional parameter to CalcCenterOfMass() to compute angular momentum.
      • + *
    • Added CalcBodySpatialJacobian()
      • + *
    • Added CalcContactJacobian()
      • + *
    • Added NonlinearEffects()
      • + *
    • Added solving of linear systems using standard Householder QR
      • + *
    • LuaModel: Added LuaModelReadFromLuaState()
      • + *
    • URDFReader: Fixed various issues and using faster joints for floating base models
      • + *
    • Various performance improvements
      • + *
    + *
    • + *
+ * + * See \subpage api_version_checking_page for a complete version history. + * + * \section Example Examples + * + * A simple example for creation of a model and computation of the forward + * dynamics using the C++ API can be found \ref SimpleExample "here". + * + * Another example that uses the \ref addon_luamodel_page "LuaModel Addon" can be found \ref + * LuaModelExample "here". + * + * An example of the Python wrapper can be found at \ref PythonExample + * "Python Example". + * + * \section ModuleOverview API reference separated by functional modules + * + * \li \subpage modeling_page + * \li \subpage joint_description + * \li \subpage kinematics_page + * \li \subpage dynamics_page + * \li \subpage contacts_page + * \li \subpage addon_luamodel_page + * + * The page \subpage api_version_checking_page contains information about + * incompatibilities of the existing versions and how to migrate. + * + * \section Licensing Licensing + * + * The library is published under the very permissive zlib free software + * license which should allow you to use the software wherever you need. + * Here is the full license text: + * \verbatim +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2014 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. +\endverbatim + + * \section Acknowledgements + * + * Work on this library was funded by the Heidelberg + * Graduate School of Mathematical and Computational Methods for the + * Sciences (HGS) and the European FP7 projects ECHORD (grant number 231143) and Koroibot (grant number 611909). + * + */ diff --git a/3rdparty/rbdl/doc/api_changes.txt b/3rdparty/rbdl/doc/api_changes.txt new file mode 100644 index 0000000..c033405 --- /dev/null +++ b/3rdparty/rbdl/doc/api_changes.txt @@ -0,0 +1,179 @@ +2.5.0 -> next +- Replaced Contacts API by new Constraints API. The new API allows to + compute dynamics of models with kinematic loops but otherwise uses a very + similar API. Loop constraints can be stabilized using Baumgarte + stabilization. Special thanks to Davide Corradi for this contribution! +- New inverse kinematics algorithm and API. It now uses the IK method + described by Sugihara which is faster and more robust than the + previously implemented damped Levenberg-Marquardt method. The new API + allows to specify both point and orientation (and mixed) constraints. + Thanks to Kevin Stein for implementing it! +- Changed Quaternion multiplication behaviour for a more standard + convention: multiplying q1 (1,0,0,0) with q2 (0,1,0,0) results now in + (0,0,1,0) instead of the previous (0,0,-1,0). +- New joint type JointTypeHelical that can be used for screwing motions + (translations and simultaneous rotations), contributed by Stuart Anderson. + +2.4.1 -> 2.5.0 +- Added an experimental Cython based Python wrapper of RBDL. The API is + very close to the C++ API. For a brief glimpse of the API see the file + python/test_wrapper.py. +- Matthew Millard added CustomJoints which allow to create different joint + types completely by user code. They are implemented as proxy joints for + which their behaviour is specified using virtual functions. +- Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + the joint space inertia matrix with a vector in O(n) time. +- Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + the floating base joint. +- Loading of floating base URDF models must now be specified as a third + parameter to URDFReadFromFile() and URDFReadFromString() +- Added the URDF code from Bullet3 which gets used when ROS is not found. + Otherwise use the URDF libraries found via Catkin. +- Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + that compute both linear and angular quantities +- Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + JointTypeFloatingBase instead. +- Fixed building issues when building DLL with MSVC++. + +2.4.0 -> 2.4.1 (20 April 2016) +This is a bugfix release that maintains binary compatibility and only fixes +erroneous behaviour. +- critical: fixed termination criterion for InverseKinematics. The termination + criterion would be evaluated too early and thus report convergence too + early. This was reported independently by Kevin Stein, Yun Fei, and Davide + Corradi. Thanks for the reports! +- critical: fixed CompositeRigidBodyAlgorithm when using spherical joints + (thanks to Sébastien Barthélémy for reporting!) + +2.3.3 -> 2.4.0 (23 February 2015) +- Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() + and ComputeContactImpulsesRangeSpaceSparse() +- Added null-space method ForwardDynamicsContactsNullSpace() + and ComputeContactImpulsesNullSpace() +- Renamed ForwardDynamicsContactsLagrangian() to + ForwardDynamicsContactsDirect() and + ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect() +- Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis() +- Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The + functionality has been integrated into CalcCenterOfMass(). +- CalcPointJacobian() does not clear the argument of the result anymore. + Caller has to ensure that the matrix was set to zero before using this + function. +- Added optional workspace parameters for ForwardDynamicsLagrangian() to + optionally reduce memory allocations +- Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ + which are equivalent to the emulated multidof joints but faster. +- Added optional parameter to CalcCenterOfMass to compute angular momentum. +- Added CalcBodySpatialJacobian() +- Added CalcContactJacobian() +- Added NonlinearEffects() +- Added solving of linear systems using standard Householder QR +- LuaModel: Added LuaModelReadFromLuaState() +- URDFReader: Fixed various issues and using faster joints for floating + base models +- Various performance improvements + +2.3.2 -> 2.3.3 (21 October 2014) +- critical: fixed ForwardDynamicsContacts with constraints on a body + hat is attached with a fixed joint. Previous versions simply crashed. + Thanks to Yue Hu for reporting! +- rbdl_print_version() now properly prints whether URDFReader was enabled + at build time +- build system: fixed roblems especially building of the URDFreader +- build system: all CMake variables for RBDL are now prefixed with RBDL_ +- FindRBDL.cmake now can use components to search for the LuaModel or + URDFReader addon + +2.3.1 -> 2.3.2 (29 August 2014) +- critical: fixed ForwardDynamicsLagrangian which used uninitialized values for the joint space inertia matrix +- critical: fixed ForwardDynamicsContacts when using 3-dof joints +- critical: fixed CalcBodyWorldOrientation for fixed joints (thanks to Hilaro Tome!) +- critical: fixed CompositeRigidBodyDynamics when using 3-dof joints (thanks to Henning Koch!) + +2.3.0 -> 2.3.1 (13 July 2014) +- critical: fixed angular momentum computation. Version 2.3.0 produced wrong + results. Thanks to Hilario Tome and Benjamin Michaud for reporting! +- critical: fixed JointTypeEulerZYX. Previous versions produce wrong + results! +- fixed library version number for the LuaModel addon. It now uses version + 2.3 instead of the wrong 2.2. + +2.2.2 -> 2.3.0 (14 March 2014) +- disabled clearing of joint space inertia matrix in CRBA. + It is expected that the matrix is cleared by the user when neccessary. +- Added experimental joint type JointTypeEulerZYX. It does not emulate + multiple degrees of freedom using virtual bodies instead it uses a 3 DoF + motion subspace. Performance is better for the + CompositeRigidBodyAlgorithm but worse for other algorithms. +- Using Eigen3's default column-major ordering for matrices when using + Eigen3. This should have no effect for the user unless matrix elements + are accessed using the .data()[i] operator of Eigen3's matrix class. However + if .data()[i] is used the access indices have to be adjusted. +- added functions to compute kinetic and potential energy and the + computation of the center of mass and its linear velocity: + RigidBodyDynamics::Utils::CalcCenterOfMass + RigidBodyDynamics::Utils::CalcPotentialEnergy + RigidBodyDynamics::Utils::CalcKineticEnergy + RigidBodyDynamics::Utils::CalcAngularMomentum + +2.2.1 -> 2.2.2 (06 November 2013) +- adjusted default constructor for Body. It now has the identity matrix as + inertia, instead of a zero matrix. +- LuaModel: made sure that the Body value is optional and uses the default + Body constructor if not defined. + +2.2.0 -> 2.2.1 04 (November 2013) +- properly exporting LuaTables++ functions when using LuaModel addon + Fixes linking +- fixed exported library version (now at 2.2 as expected) + +2.1.0 -> 2.2.0 (28 October 2013) +- added spherical joints that do not suffer from singularities: + Use joint type JointTypeSpherical +- added Model::q_size, and Model::qdot_size that report the sizes including + all 4 Quaternion parameters when spherical joints are used. + User is advised to use Model::q_size and Model::qdot_size instead of + Model::dof_count. +- removed "constraint_" prefix from constraint impulses, forces and + accelerations from the ConstraintSets: + renaming required if values are queried +- Contact impulses: specification of a contact velocity after a collision: + added ConstraintSet::v_plus which can be set for the desired constraint + velocity after a collision. Previously it used the values stored in + ConstraintSet::constraint_acceleration. + User has to store desired exit velocities manually in CS::v_plus + +2.0.1 -> 2.1.0 (29 September 2013) +- made codebase compatible to Debian + Binary symbol export was changed. No change in user code required, + however everything needs to be recompiled and linked. +- Removed Lua 5.2 source + When building the addon LuaModel, one hast to have it installed on the + system already. +- Removed UnitTest++ sources + When building tests one has to have it installed on the system already. + +2.0.0 -> 2.0.1 (05 September 2013) +- fixed compiler errors on some older compilers + No change required when using RBDL version 2.0.0. +- fixed CMake configurations for examples + No change required when using RBDL version 2.0.0. + +1.X.Y -> 2.0.0 (18 July 2013) +- removed Model::Init(): + All initialization is now done in the default constructor in Model(). To + be compatible with the new API simply remove any calls to Model::Init(). +- removed Eigen3 sources: + Eigen3 is no more included in RBDL instead it uses the Eigen3 library + that is installed on the system. If you want to use RBDL with Eigen3 + you have to install it on your system yourself. +- inverted sign of contact forces/impulses: + ConstraintSet::constraint_force and ConstraintSet::constraint_impulse are + now the forces or impulses that are acting on the constrained body by the + constraint. + +1.0.0 -> 1.1.0 (20 February 2013) +- removed constructor Body (mass, com, length, gyration_radii) + This constructor did some erroneous calculations to compute the real + radii of gyration. It was removed as the two remaining constructors are + properly tested and are more general. diff --git a/3rdparty/rbdl/doc/example.h b/3rdparty/rbdl/doc/example.h new file mode 100644 index 0000000..33c66b5 --- /dev/null +++ b/3rdparty/rbdl/doc/example.h @@ -0,0 +1,25 @@ +/** \file example.h + * \page SimpleExample A simple example + * + * Here is a simple example how one can create a meaningless model and + * compute the forward dynamics for it: + * + * \include example.cc + * + * If the library itself is already created, one can compile this example + * with CMake. In the example folder is a CMakeLists.txt file, that can be + * used to automatically create the makefiles by using CMake. It uses the script + * FindRBDL.cmake which can be used to find the library and include + * directory of the headers. + * + * The FindRBDL.cmake script can use the environment variables RBDL_PATH, + * RBDL_INCLUDE_PATH, and RBDL_LIBRARY_PATH to find the required files. + * + * To build it manually you have to specify the following compiler and + * linking switches: + * \code + * g++ example.cc -I/src -I -lrbdl -L -o example + * \endcode + * + */ diff --git a/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png b/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png new file mode 100644 index 0000000..c626d85 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png new file mode 100644 index 0000000..9bf7c48 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png new file mode 100644 index 0000000..f2f2dd8 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png new file mode 100644 index 0000000..430e399 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png new file mode 100644 index 0000000..efbab57 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png new file mode 100644 index 0000000..49cfaa6 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png new file mode 100644 index 0000000..43710de Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png new file mode 100644 index 0000000..45a3847 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png new file mode 100644 index 0000000..b40f6c0 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png new file mode 100644 index 0000000..82ee9c8 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png new file mode 100644 index 0000000..7ef78ab Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png new file mode 100644 index 0000000..fbe8f67 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png new file mode 100644 index 0000000..5d123f9 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png new file mode 100644 index 0000000..7d1475f Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png new file mode 100644 index 0000000..6743091 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png new file mode 100644 index 0000000..25ec23f Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png new file mode 100644 index 0000000..c13f8ba Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png new file mode 100644 index 0000000..f879122 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png new file mode 100644 index 0000000..e7db07a Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png new file mode 100644 index 0000000..5c39517 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png new file mode 100644 index 0000000..696cd0e Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png new file mode 100644 index 0000000..9a8bcb5 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png new file mode 100644 index 0000000..fe17415 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo.png b/3rdparty/rbdl/doc/logo/rbdl_logo.png new file mode 100644 index 0000000..5d0a755 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo.svg b/3rdparty/rbdl/doc/logo/rbdl_logo.svg new file mode 100644 index 0000000..68280da --- /dev/null +++ b/3rdparty/rbdl/doc/logo/rbdl_logo.svg @@ -0,0 +1,104 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png b/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png new file mode 100644 index 0000000..18146d7 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png b/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png new file mode 100644 index 0000000..eac696e Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png b/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png new file mode 100644 index 0000000..ea92ae7 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png differ diff --git a/3rdparty/rbdl/doc/luamodel_example.h b/3rdparty/rbdl/doc/luamodel_example.h new file mode 100644 index 0000000..2b2465f --- /dev/null +++ b/3rdparty/rbdl/doc/luamodel_example.h @@ -0,0 +1,43 @@ +/** \file example.h + * \page LuaModelExample LuaModel example + * + * Here is an example on how to use the \ref luamodel_introduction Addon both + * for loading the models from C++ code and also how the model file looks + * like. + * + * Using Lua as a description format for models instead of C++ code has + * numerous advantages such as simplified modeling, less C++ compilations, + * and easier exchange of models between multiple users. As the file format + * uses Lua code it is easy to create parameterized models. + * + * To be able to use the addon one has to enable the addon in CMake by + * setting BUILD_ADDON_LUAMODEL to true. + * + * Here is an example how \ref luamodel_introduction can be loaded by the RBDL: + * + * \include example_luamodel.cc + * + * If the library itself is already created, one can compile this example + * with CMake. In the example folder is a CMakeLists.txt file, that can be + * used to automatically create the makefiles by using CMake. It uses the script + * FindRBDL.cmake which can be used to find the library and include + * directory of the headers. + * + * The documentation about how to write the lua models can be found + * \ref luamodel_introduction "here". + * + * Here is an example: + * + * \include samplemodel.lua + * + * The FindRBDL.cmake script can use the environment variables RBDL_PATH, + * RBDL_INCLUDE_PATH, and RBDL_LIBRARY_PATH to find the required files. + * + * To build it manually you have to specify the following compiler and + * linking switches: + * \code + * g++ example.cc -I/src -I -lrbdl -L -o example + * \endcode + * + */ diff --git a/3rdparty/rbdl/doc/notes/Makefile b/3rdparty/rbdl/doc/notes/Makefile new file mode 100644 index 0000000..f9f53ae --- /dev/null +++ b/3rdparty/rbdl/doc/notes/Makefile @@ -0,0 +1,2 @@ +all: point_velocity_acceleration.tex + rubber -d point_velocity_acceleration.tex diff --git a/3rdparty/rbdl/doc/notes/acceleration_visualization.pdf b/3rdparty/rbdl/doc/notes/acceleration_visualization.pdf new file mode 100644 index 0000000..c0517ca Binary files /dev/null and b/3rdparty/rbdl/doc/notes/acceleration_visualization.pdf differ diff --git a/3rdparty/rbdl/doc/notes/acceleration_visualization.svg b/3rdparty/rbdl/doc/notes/acceleration_visualization.svg new file mode 100644 index 0000000..45cdde3 --- /dev/null +++ b/3rdparty/rbdl/doc/notes/acceleration_visualization.svg @@ -0,0 +1,2003 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex b/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex new file mode 100644 index 0000000..25aee06 --- /dev/null +++ b/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex @@ -0,0 +1,157 @@ +% File: computation_notes.tex +% Created: Mon Dec 20 02:00 PM 2010 C +% Last Change: Mon Dec 20 02:00 PM 2010 C +% +\documentclass[a4paper]{article} +\usepackage[utf8]{inputenc} +\usepackage{graphicx} + +\newcommand{\Spa}[1]{\mathbf{\hat{#1}}} +\newcommand{\Nspa}[1]{\mathbf{\underline{#1}}} +\newcommand{\Vec}[1]{\mathbf{#1}} +\newcommand{\figref}[1]{\figurename~\ref{#1}} + +\begin{document} + +\section{Computation of the velocity of a point on a rigid body using spatial +algebra} + +The ABA computes automatically the spatial velocity of body $i$ in the +reference frame of the body such that computation of joint velocities are +easier. Now we want to compute the (linear) velocity in base coordinates of a +point $\Vec{p}$ that is attached to body $i$ with body coordinates +${}^i\Vec{r}_p$, i.e. we want to compute ${}^0 \Vec{\dot{r}}_p = {}^0 +\Vec{r}_p$. + +First of all we compute the spatial velocities ${}^0\Spa{v}$ of all bodies in +base coordinates (which can be done during the first loop of the ABA): + +\begin{equation} + {}^0\Spa{v}_i = {}^0\Spa{v}_{\lambda(i)} + {}^{0}\Spa{X}_{i} \Spa{v}_{Ji} +\end{equation} + +where $\Spa{v}_{Ji} = \Spa{S}_i \dot{q}_i $ which is the velocity that is +propagated by joint $i$ from body $\lambda(i)$ to body $i$ along joint axis +$\Spa{S}_i$ (see also RBDA p. 80). + +This is now the velocity of the body $i$ in base coordinates. What is now left +todo is to transform this velocity into the velocity of the point $\Vec{p}$. +Before we can do that we have to compute the coordinates of $P$ in base +coordinates which can be done by: + +\begin{equation} + {}^0\Vec{r}_p = {}^0 \Vec{r}_i + {}^0\Vec{R}_i {}^i \Vec{r}_p +\end{equation} + +for which ${}^0 \Vec{r}_i$ is the origin of the bodies in base coordinates and +${}^0 \Vec{R}_i$ the orientation of the base relative to the body. + +Now we can compute the velocity of point $\Vec{r}_p$ with the following +formulation: +\begin{equation} + {}^0\Spa{v}_p = \textit{xlt}({}^0\Vec{r}_p) {}^0\Spa{v}_i = + \left[ + \begin{array}{cc} + \Vec{1} & 0 \\ + -{}^0\Vec{r}_p \times & \Vec{1} + \end{array} + \right] + {}^0\Spa{v}_i +\end{equation} + +By doing so, the linear part of the spatial velocity ${}^0\Spa{v}_p$ has the +following entries: +\begin{equation} + {}^0\Spa{v}_p = \left[ + \begin{array}{c} + \Nspa{\omega} \\ + -{}^0\Nspa{r}_p \times \Nspa{\omega} + {}^0 \Nspa{\Vec{v}}_i + \end{array} + \right] + = + \left[ + \begin{array}{c} + \Nspa{\omega} \\ + {}^0 \Nspa{\Vec{v}}_i + \Nspa{\omega} \times {}^0 \Nspa{r}_p + \end{array} + \right] +\end{equation} + +For which the bottom line is the term for linear velocity in the standard 3D +notation. + +\section{Computation of the acceleration of a point on a rigid body using +spatial algebra} + +The acceleration of a point depends on three quantities: the position of the +point, velocity of the body and the acceleration of the body. We therefore +assume that we already computed the velocity as described in the previous +section. + +To compute the acceleration of a point we have to compute the spatial +acceleration of a body in base coordinates. This can be expressed as: +\begin{equation} + \Spa{a}_i = {}^0\Spa{a}_{\lambda(i)} + + {}^{0}\Spa{X}_{i} \Spa{a}_{Ji} +\end{equation} +for which $\Spa{a}_{Ji} = \Spa{S}_i \ddot{q}_i + \dot{\Spa{S}}_i \dot{q}_i$. +The last term can be written as: $\dot{\Spa{S}} = \Spa{v}_i \times \Spa{S}_i$. + +\begin{figure}[h!] + \begin{center} + \includegraphics[width=0.9\textwidth]{acceleration_visualization} + \end{center} + \caption{Visualization of the acceleration of a point} + \label{fig:acceleration_visualization} +\end{figure} + +There are three coordinate systems involved in the computation of the +acceleration of point p which are shown in +\figref{fig:acceleration_visualization}. The \emph{base coordinate system} +which is the global reference frame, \emph{link $i$ coordinate system} +which is the coordinate system of body $i$, and the \emph{point coordinate +system} that is locate at the current position of $p$ and has the same +orientation as the base coordinate system. + +First of all we build the transformation from the body coordinate system to +the point coordinate system: +\begin{equation} + {^p}\Vec{X}_i = + \left[ + \begin{array}{cc} + {^0}\Vec{E}_i & \Vec{0} \\ + \Vec{0} & {^0}\Vec{E}_i + \end{array} + \right] + \left[ + \begin{array}{cc} + \Vec{1} & \Vec{0} \\ + -{^i}\Nspa{r}_p \times & \Vec{1} + \end{array} + \right]. +\end{equation} +This can now be used to express the velocity and acceleration of the body at +the point p: +\begin{equation} + {^p}\Spa{v}_i = {^p}\Vec{X}_i \Spa{v}_i +\end{equation} +\begin{equation} + {^p}\Spa{a}_i = {^p}\Vec{X}_i \Spa{a}_i +\end{equation} + +We now want to retrieve the \emph{classical acceleration}. It is expressed in a +coordinate frame that has a pure linear velocity of ${^0}\Nspa{\dot{p}}$. +\begin{equation} + {^p}\Spa{a}'_i = {^p}\Spa{a}_i + + \left[ + \begin{array}{c} + \Vec{0}\\ + {^0}\Nspa{\omega}_i \times {^0}\Nspa{\dot{p}} + \end{array} + \right] +\end{equation} + +The linear part of this vector is then the acceleration of the point in base +coordinates. + +\end{document} diff --git a/3rdparty/rbdl/doc/python_example.h b/3rdparty/rbdl/doc/python_example.h new file mode 100644 index 0000000..1af93ff --- /dev/null +++ b/3rdparty/rbdl/doc/python_example.h @@ -0,0 +1,32 @@ +/** \file python_example.h + * \page PythonExample Python API example + * + * Here is a simple example of the Python API that showcases a subset of + * the wrapped functions and how to access them from python: + * + * \include example.py + * + * To build the wrapper one needs both Cython and the development libraries for + * NumPy installed on the system. If + * this is the case you can build the wrapper by enabling the CMake option + * RBDL_BUILD_PYTHON_WRAPPER when configuring RBDL. You can find the + * wrapper in the subdirectory python/ of your build directory. + * By running the python interpreter in this directory you can load it + * within the python shell using + * + * \code + * Python 2.7.11+ (default, Apr 17 2016, 14:00:29) + * [GCC 5.3.1 20160413] on linux2 + * Type "help", "copyright", "credits" or "license" for more information. + * >>> import rbdl + * \endcode + * + * To install the wrapper you can use the python/setup.py script: + * \code + * sudo ./setup.py install + * \endcode + * + * This installs the RBDL python module globally and allows you to import + * it from any python script. + */ diff --git a/3rdparty/rbdl/examples/luamodel/CMakeLists.txt b/3rdparty/rbdl/examples/luamodel/CMakeLists.txt new file mode 100644 index 0000000..9a0b71c --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/CMakeLists.txt @@ -0,0 +1,27 @@ +PROJECT (RBDLEXAMPLE CXX) + +CMAKE_POLICY(SET CMP0048 NEW) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +# We need to add the project source path to the CMake module path so that +# the FindRBDL.cmake script can be found. +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL COMPONENTS LuaModel REQUIRED) +FIND_PACKAGE (Eigen3 3.0.0 REQUIRED) +FIND_PACKAGE (Lua51 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${LUA_INCLUDE_DIR}) + +# Create an executable +ADD_EXECUTABLE (example_luamodel example_luamodel.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES ( example_luamodel + ${RBDL_LIBRARY} + ${RBDL_LuaModel_LIBRARY} + ${LUA_LIBRARIES} + ) diff --git a/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake b/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - 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 + +# 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 + 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) diff --git a/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake b/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/luamodel/example_luamodel.cc b/3rdparty/rbdl/examples/luamodel/example_luamodel.cc new file mode 100644 index 0000000..3d90c05 --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/example_luamodel.cc @@ -0,0 +1,47 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include + +#ifndef RBDL_BUILD_ADDON_LUAMODEL + #error "Error: RBDL addon LuaModel not enabled." +#endif + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = NULL; + + model = new Model(); + + if (!Addons::LuaModelReadFromFile ("./samplemodel.lua", model, false)) { + std::cerr << "Error loading model ./samplemodel.lua" << std::endl; + abort(); + } + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << Q.transpose() << std::endl; + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua b/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua new file mode 100644 index 0000000..8a93abd --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua @@ -0,0 +1,247 @@ +--[[ +-- This is an example model for the RBDL addon luamodel. You need to +-- enable RBDL_BUILD_ADDON_LUAMODEL to be able to use this file. +--]] + + +m1 = 2 +l1 = 2 +r1 = 0.2 +Izz1 = m1 * l1 * l1 / 3 + +m2 = 2 +l2 = 2 +r2 = 0.2 +Izz2 = m2 * l2 * l2 / 3 + +bodies = { + + virtual = { + mass = 0, + com = {0, 0, 0}, + inertia = { + {0, 0, 0}, + {0, 0, 0}, + {0, 0, 0}, + }, + }, + + link1 = { + mass = m1, + com = {l1/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz1}, + }, + }, + + link2 = { + mass = m2, + com = {l2/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz2}, + }, + }, + +} + +joints = { + + revZ = { + {0, 0, 1, 0, 0, 0}, + }, + + trnXYZ = { + {0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + }, + +} + +model = { + + gravity = {0, 0, 0}, + + configuration = { + axis_front = { 1., 0., 0.}, + axis_right = { 0., -1., 0.}, + axis_up = { 0., 0., 1.}, + }, + + frames = { + + { + name = 'base', + parent = 'ROOT', + body = bodies.virtual, + joint = joints.trnXYZ, + }, + { + name = 'l11', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + }, + { + name = 'l12', + parent = 'l11', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + }, + { + name = 'l21', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + }, + { + name = 'l22', + parent = 'l21', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + }, + + }, + + constraint_sets = { + loop_constraints = { + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + + all_constraints = { + { + constraint_type = 'contact', + body = 'base', + point = {0, 0, 0}, + normal = {1, 0, 0}, + name = 'baseTX', + normal_acceleration = 0, + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 1, 0}, + name = 'baseTY', + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 0, 1}, + name = 'baseTZ', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + }, + +} + +return model \ No newline at end of file diff --git a/3rdparty/rbdl/examples/luamodel/samplemodel.lua b/3rdparty/rbdl/examples/luamodel/samplemodel.lua new file mode 100644 index 0000000..756ed3d --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/samplemodel.lua @@ -0,0 +1,122 @@ +--[[ +-- This is an example model for the RBDL addon luamodel. You need to +-- enable RBDL_BUILD_ADDON_LUAMODEL to be able to use this file. +--]] + +print ("This is some output from the model file") + +inertia = { + {1.1, 0.1, 0.2}, + {0.3, 1.2, 0.4}, + {0.5, 0.6, 1.3} +} + +pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } +thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia } +shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } +foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } + +bodies = { + pelvis = pelvis, + thigh_right = thigh, + shank_right = shank, + thigh_left = thigh, + shank_left = shank +} + +joints = { + freeflyer = { + { 0., 0., 0., 1., 0., 0.}, + { 0., 0., 0., 0., 1., 0.}, + { 0., 0., 0., 0., 0., 1.}, + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + spherical_zyx = { + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + rotational_y = { + { 0., 1., 0., 0., 0., 0.} + }, + fixed = {} +} + +model = { + gravity = {0., 0., -9.81}, + + frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.spherical_zyx, + joint_frame = { + r = {0.0, -0.15, 0.}, + E = { + {1., 0., 0.}, + {0., 1., 0.}, + {0., 0., 0.} + } + } + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y, + joint_frame = { + r = {0.0, 0., -0.42}, + }, + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.thigh_right, + joint_frame = { + r = {0.0, 0., -0.41} + }, + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.spherical_zyx, + joint_frame = { + r = {0.0, 0.15, 0.}, + E = { + {1., 0., 0.}, + {0., 1., 0.}, + {0., 0., 0.} + } + } + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.thigh_left, + joint = joints.rotational_y, + joint_frame = { + r = {0.0, 0., -0.42}, + }, + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.thigh_left, + joint_frame = { + r = {0.0, 0., -0.41}, + }, + }, + } +} + +return model diff --git a/3rdparty/rbdl/examples/python/example.py b/3rdparty/rbdl/examples/python/example.py new file mode 100644 index 0000000..fce073f --- /dev/null +++ b/3rdparty/rbdl/examples/python/example.py @@ -0,0 +1,55 @@ +import numpy as np +import rbdl + +# Create a new model +model = rbdl.Model() + +# Create a joint from joint type +joint_rot_y = rbdl.Joint.fromJointType ("JointTypeRevoluteY") + +# Create a body for given mass, center of mass, and inertia at +# the CoM +body = rbdl.Body.fromMassComInertia ( + 1., + np.array([0., 0.5, 0.]), + np.eye(3) * 0.05) +xtrans= rbdl.SpatialTransform() +xtrans.r = np.array([0., 1., 0.]) + +# You can print all types +print (joint_rot_y) +print (model) +print (body) +print (body.mInertia) +print (xtrans) + +# Construct the model +body_1 = model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, body) +body_2 = model.AppendBody (xtrans, joint_rot_y, body) +body_3 = model.AppendBody (xtrans, joint_rot_y, body) + +# Create numpy arrays for the state +q = np.zeros (model.q_size) +qdot = np.zeros (model.qdot_size) +qddot = np.zeros (model.qdot_size) +tau = np.zeros (model.qdot_size) + +# Modify the state +q[0] = 1.3 +q[1] = -0.5 +q[2] = 3.2 + +# Transform coordinates from local to global coordinates +point_local = np.array([1., 2., 3.]) +point_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_3, point_local) +point_local_2 = rbdl.CalcBaseToBodyCoordinates (model, q, body_3, point_base) + +# Perform forward dynamics and print the result +rbdl.ForwardDynamics (model, q, qdot, tau, qddot) +print ("qddot = " + str(qddot.transpose())) + +# Compute and print the jacobian (note: the output parameter +# of the Jacobian G must have proper size!) +G = np.zeros([3,model.qdot_size]) +rbdl.CalcPointJacobian (model, q, body_3, point_local, G) +print ("G = \n" + str(G)) diff --git a/3rdparty/rbdl/examples/simple/CMakeLists.txt b/3rdparty/rbdl/examples/simple/CMakeLists.txt new file mode 100644 index 0000000..35672f0 --- /dev/null +++ b/3rdparty/rbdl/examples/simple/CMakeLists.txt @@ -0,0 +1,24 @@ +PROJECT (RBDLEXAMPLE CXX) + +CMAKE_POLICY(SET CMP0048 NEW) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +# We need to add the project source path to the CMake module path so that +# the FindRBDL.cmake script can be found. +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL REQUIRED) +FIND_PACKAGE (Eigen3 3.0.0 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) + +# Create an executable +ADD_EXECUTABLE (example example.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES (example + ${RBDL_LIBRARY} + ) diff --git a/3rdparty/rbdl/examples/simple/FindEigen3.cmake b/3rdparty/rbdl/examples/simple/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/simple/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - 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 + +# 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 + 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) diff --git a/3rdparty/rbdl/examples/simple/FindRBDL.cmake b/3rdparty/rbdl/examples/simple/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/simple/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/simple/example.cc b/3rdparty/rbdl/examples/simple/example.cc new file mode 100644 index 0000000..15df04d --- /dev/null +++ b/3rdparty/rbdl/examples/simple/example.cc @@ -0,0 +1,65 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = NULL; + + unsigned int body_a_id, body_b_id, body_c_id; + Body body_a, body_b, body_c; + Joint joint_a, joint_b, joint_c; + + model = new Model(); + + model->gravity = Vector3d (0., -9.81, 0.); + + body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.)); + joint_a = Joint( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.)); + joint_c = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt b/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt new file mode 100644 index 0000000..e170b76 --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt @@ -0,0 +1,25 @@ +PROJECT (RBDLEXAMPLE CXX) + +CMAKE_POLICY(SET CMP0048 NEW) + +CMAKE_MINIMUM_REQUIRED(VERSION 2.6) + +# We need to add the project source path to the CMake module path so that +# the FindRBDL.cmake script can be found. +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL COMPONENTS URDFReader REQUIRED) +FIND_PACKAGE (Eigen3 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) + +# Create an executable +ADD_EXECUTABLE (example_urdfreader example_urdfreader.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES ( example_urdfreader + ${RBDL_LIBRARY} + ${RBDL_URDFReader_LIBRARY} + ) diff --git a/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake b/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - 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 + +# 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 + 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) diff --git a/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake b/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc b/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc new file mode 100644 index 0000000..106c283 --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc @@ -0,0 +1,45 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include + +#ifndef RBDL_BUILD_ADDON_URDFREADER + #error "Error: RBDL addon URDFReader not enabled." +#endif + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = new Model(); + + if (!Addons::URDFReadFromFile ("./samplemodel.urdf", model, false)) { + std::cerr << "Error loading model ./samplemodel.urdf" << std::endl; + abort(); + } + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << Q.transpose() << std::endl; + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/include/rbdl/Body.h b/3rdparty/rbdl/include/rbdl/Body.h new file mode 100644 index 0000000..7d36b81 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Body.h @@ -0,0 +1,218 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_BODY_H +#define RBDL_BODY_H + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +/** \brief Describes all properties of a single body + * + * A Body contains information about mass, the location of its center of + * mass, and the ineria tensor in the center of mass. This class is + * designed to use the given information and transform it such that it can + * directly be used by the spatial algebra. + */ +struct RBDL_DLLAPI Body { + Body() : + mMass (0.), + mCenterOfMass (0., 0., 0.), + mInertia (Math::Matrix3d::Zero(3,3)), + mIsVirtual (false) + { }; + Body(const Body &body) : + mMass (body.mMass), + mCenterOfMass (body.mCenterOfMass), + mInertia (body.mInertia), + mIsVirtual (body.mIsVirtual) + {}; + Body& operator= (const Body &body) { + if (this != &body) { + mMass = body.mMass; + mInertia = body.mInertia; + mCenterOfMass = body.mCenterOfMass; + mIsVirtual = body.mIsVirtual; + } + + return *this; + } + + /** \brief Constructs a body from mass, center of mass and radii of gyration + * + * This constructor eases the construction of a new body as all the + * required parameters can be specified as parameters to the + * constructor. These are then used to generate the spatial inertia + * matrix which is expressed at the origin. + * + * \param mass the mass of the body + * \param com the position of the center of mass in the bodies coordinates + * \param gyration_radii the radii of gyration at the center of mass of the body + */ + Body(const double &mass, + const Math::Vector3d &com, + const Math::Vector3d &gyration_radii) : + mMass (mass), + mCenterOfMass(com), + mIsVirtual (false) { + mInertia = Math::Matrix3d ( + gyration_radii[0], 0., 0., + 0., gyration_radii[1], 0., + 0., 0., gyration_radii[2] + ); + } + + /** \brief Constructs a body from mass, center of mass, and a 3x3 inertia matrix + * + * This constructor eases the construction of a new body as all the + * required parameters can simply be specified as parameters to the + * constructor. These are then used to generate the spatial inertia + * matrix which is expressed at the origin. + * + * \param mass the mass of the body + * \param com the position of the center of mass in the bodies coordinates + * \param inertia_C the inertia at the center of mass + */ + Body(const double &mass, + const Math::Vector3d &com, + const Math::Matrix3d &inertia_C) : + mMass (mass), + mCenterOfMass(com), + mInertia (inertia_C), + mIsVirtual (false) { } + + /** \brief Joins inertial parameters of two bodies to create a composite + * body. + * + * This function can be used to joint inertial parameters of two bodies + * to create a composite body that has the inertial properties as if the + * two bodies were joined by a fixed joint. + * + * \note Both bodies have to have their inertial parameters expressed in + * the same orientation. + * + * \param transform The frame transformation from the origin of the + * original body to the origin of the added body + * \param other_body The other body that will be merged with *this. + */ + void Join (const Math::SpatialTransform &transform, const Body &other_body) { + // nothing to do if we join a massles body to the current. + if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) { + return; + } + + double other_mass = other_body.mMass; + double new_mass = mMass + other_mass; + + if (new_mass == 0.) { + std::cerr << "Error: cannot join bodies as both have zero mass!" << std::endl; + assert (false); + abort(); + } + + Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r; + Math::Vector3d new_com = (1 / new_mass ) * (mMass * mCenterOfMass + other_mass * other_com); + + LOG << "other_com = " << std::endl << other_com.transpose() << std::endl; + LOG << "rotation = " << std::endl << transform.E << std::endl; + + // We have to transform the inertia of other_body to the new COM. This + // is done in 4 steps: + // + // 1. Transform the inertia from other origin to other COM + // 2. Rotate the inertia that it is aligned to the frame of this body + // 3. Transform inertia of other_body to the origin of the frame of + // this body + // 4. Sum the two inertias + // 5. Transform the summed inertia to the new COM + + Math::SpatialRigidBodyInertia other_rbi = Math::SpatialRigidBodyInertia::createFromMassComInertiaC (other_body.mMass, other_body.mCenterOfMass, other_body.mInertia); + Math::SpatialRigidBodyInertia this_rbi = Math::SpatialRigidBodyInertia::createFromMassComInertiaC (mMass, mCenterOfMass, mInertia); + + Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3,3>(0,0); + LOG << "inertia_other = " << std::endl << inertia_other << std::endl; + + // 1. Transform the inertia from other origin to other COM + Math::Matrix3d other_com_cross = Math::VectorCrossMatrix(other_body.mCenterOfMass); + Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose(); + LOG << "inertia_other_com = " << std::endl << inertia_other_com << std::endl; + + // 2. Rotate the inertia that it is aligned to the frame of this body + Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E; + LOG << "inertia_other_com_rotated = " << std::endl << inertia_other_com_rotated << std::endl; + + // 3. Transform inertia of other_body to the origin of the frame of this body + Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis (inertia_other_com_rotated, other_mass, other_com); + LOG << "inertia_other_com_rotated_this_origin = " << std::endl << inertia_other_com_rotated_this_origin << std::endl; + + // 4. Sum the two inertias + Math::Matrix3d inertia_summed = Math::Matrix3d (this_rbi.toMatrix().block<3,3>(0,0)) + inertia_other_com_rotated_this_origin; + LOG << "inertia_summed = " << std::endl << inertia_summed << std::endl; + + // 5. Transform the summed inertia to the new COM + Math::Matrix3d new_inertia = inertia_summed - new_mass * Math::VectorCrossMatrix (new_com) * Math::VectorCrossMatrix(new_com).transpose(); + + LOG << "new_mass = " << new_mass << std::endl; + LOG << "new_com = " << new_com.transpose() << std::endl; + LOG << "new_inertia = " << std::endl << new_inertia << std::endl; + + *this = Body (new_mass, new_com, new_inertia); + } + + ~Body() {}; + + /// \brief The mass of the body + double mMass; + /// \brief The position of the center of mass in body coordinates + Math::Vector3d mCenterOfMass; + /// \brief Inertia matrix at the center of mass + Math::Matrix3d mInertia; + + bool mIsVirtual; +}; + +/** \brief Keeps the information of a body and how it is attached to another body. + * + * When using fixed bodies, i.e. a body that is attached to anothe via a + * fixed joint, the attached body is merged onto its parent. By doing so + * adding fixed joints do not have an impact on runtime. + */ +struct RBDL_DLLAPI FixedBody { + /// \brief The mass of the body + double mMass; + /// \brief The position of the center of mass in body coordinates + Math::Vector3d mCenterOfMass; + /// \brief The spatial inertia that contains both mass and inertia information + Math::Matrix3d mInertia; + + /// \brief Id of the movable body that this fixed body is attached to. + unsigned int mMovableParent; + /// \brief Transforms spatial quantities expressed for the parent to the + // fixed body. + Math::SpatialTransform mParentTransform; + Math::SpatialTransform mBaseTransform; + + static FixedBody CreateFromBody (const Body& body) { + FixedBody fbody; + + fbody.mMass = body.mMass; + fbody.mCenterOfMass = body.mCenterOfMass; + fbody.mInertia = body.mInertia; + + return fbody; + } +}; + +} + +/* RBDL_BODY_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Constraints.h b/3rdparty/rbdl/include/rbdl/Constraints.h new file mode 100644 index 0000000..1a4f7c6 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Constraints.h @@ -0,0 +1,936 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2015 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_CONSTRAINTS_H +#define RBDL_CONSTRAINTS_H + +#include +#include + +namespace RigidBodyDynamics { + +/** \page contacts_page External Constraints + * + * All functions related to contacts are specified in the \ref + * constraints_group "Constraints Module". + + * \defgroup constraints_group Constraints + * + * Constraints are handled by specification of a \link + * RigidBodyDynamics::ConstraintSet + * ConstraintSet \endlink which contains all informations about the + * current constraints and workspace memory. + * + * Separate constraints can be specified by calling + * ConstraintSet::AddContactConstraint() and ConstraintSet::AddLoopConstraint(). + * After all constraints have been specified, this \link + * RigidBodyDynamics::ConstraintSet + * ConstraintSet \endlink has to be bound to the model via + * ConstraintSet::Bind(). This initializes workspace memory that is + * later used when calling one of the contact functions, such as + * ForwardDynamicsContactsDirects(). + * + * The values in the vectors ConstraintSet::force and + * ConstraintSet::impulse contain the computed force or + * impulse values for each constraint when returning from one of the + * contact functions. + * + * \section solution_constraint_system Solution of the Constraint System + * + * \subsection constraint_system Linear System of the Constrained Dynamics + * + * In the presence of constraints, to compute the + * acceleration one has to solve a linear system of the form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + - \lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ is the constraint jacobian, + * \f$C\f$ the bias force (sometimes called "non-linear + * effects"), and \f$\gamma\f$ the generalized acceleration independent + * part of the constraints. + * + * \subsection collision_system Linear System of the Contact Collision + * + * Similarly to compute the response of the model to a contact gain one has + * to solve a system of the following form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \dot{q}^{+} \\ + \Lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + H \dot{q}^{-} \\ + v^{+} + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the + * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ + * the generalized velocity before the impact, and \f$v^{+}\f$ the desired + * velocity of each constraint after the impact (known beforehand, usually + * 0). The value of \f$v^{+}\f$ can is specified via the variable + * ConstraintSet::v_plus and defaults to 0. + * + * \subsection solution_methods Solution Methods + * + * There are essentially three different approaches to solve these systems: + * -# \b Direct: solve the full system to simultaneously compute + * \f$\ddot{q}\f$ and \f$\lambda\f$. This may be slow for large systems + * and many constraints. + * -# \b Range-Space: solve first for \f$\lambda\f$ and then for + * \f$\ddot{q}\f$. + * -# \b Null-Space: solve furst for \f$\ddot{q}\f$ and then for + * \f$\lambda\f$ + * The methods are the same for the contact gaints just with different + * variables on the right-hand-side. + * + * RBDL provides methods for all approaches. The implementation for the + * range-space method also exploits sparsities in the joint space inertia + * matrix using a sparse structure preserving \f$L^TL\f$ decomposition as + * described in Chapter 8.5 of "Rigid Body Dynamics Algorithms". + * + * None of the methods is generally superior to the others and each has + * different trade-offs as factors such as model topology, number of + * constraints, constrained bodies, numerical stability, and performance + * vary and evaluation has to be made on a case-by-case basis. + * + * \subsection solving_constraints_dynamics Methods for Solving Constrained + * Dynamics + * + * RBDL provides the following methods to compute the acceleration of a + * constrained system: + * + * - ForwardDynamicsConstraintsDirect() + * - ForwardDynamicsConstraintsRangeSpaceSparse() + * - ForwardDynamicsConstraintsNullSpace() + * + * \subsection solving_constraints_collisions Methods for Computing Collisions + * + * RBDL provides the following methods to compute the collision response on + * new contact gains: + * + * - ComputeConstraintImpulsesDirect() + * - ComputeConstraintImpulsesRangeSpaceSparse() + * - ComputeConstraintImpulsesNullSpace() + * + * \subsection assembly_q_qdot Computing generalized joint positions and velocities satisfying the constraint equations + * + * When considering a model subject position level constraints expressed by the + * equation \f$\phi (q) = 0\f$, it is often necessary to comput generalized joint + * position and velocities which satisfy the constraints. + * + * In order to compute a vector of generalized joint positions that satisfy + * the constraints it is necessary to solve the following optimization problem: + * \f{eqnarray*}{ + * \text{minimize} && \sum_{i = 0}^{n} (q - q_{0})^T W (q - q_{0}) \\ + * \text{over} && q \\ + * \text{subject to} && \phi (q) = 0 + * \f} + * + * In order to compute a vector of generalized joint velocities that satisfy + * the constraints it is necessary to solve the following optimization problem: + * \f{eqnarray*}{ + * \text{minimize} && \sum_{i = 0}^{n} (\dot{q} - \dot{q}_{0})^T W (\dot{q} - \dot{q}_{0}) \\ + * \text{over} && \dot{q} \\ + * \text{subject to} && \dot{\phi} (q) = \phi _{q}(q) \dot{q} + \phi _{t}(q) = 0 + * \f} + * + * \f$q_{0}\f$ and \f$\dot{q}_{0}\f$ are initial guesses, \f$\phi _{q}\f$ is the + * constraints jacobian (partial derivative of \f$\phi\f$ with respect to \f$q\f$), + * and \f$\phi _{t}(q)\f$ is the partial derivative of \f$\phi\f$ with respect + * to time. \f$W\f$ is a diagonal weighting matrix, which can be exploited + * to prioritize changes in the position/ velocity of specific joints. + * With a solver capable of handling singular matrices, it is possible to set to + * 1 the weight of the joint positions/ velocities that should not be changed + * from the initial guesses, and to 0 those corresponding to the values that + * can be changed. + * + * These problems are solved using the Lagrange multipliers method. For the + * velocity problem the solution is exact. For the position problem the + * constraints are linearized in the form + * \f$ \phi (q_{0}) + \phi _{t}(q0) + \phi _{q_0}(q) (q - q_{0}) \f$ + * and the linearized problem is solved iteratively until the constraint position + * errors are smaller than a given threshold. + * + * RBDL provides two functions to compute feasible joint position and velocities: + * - CalcAssemblyQ() + * - CalcAssemblyQDot() + * + * \subsection baumgarte_stabilization Baumgarte stabilization + * + * The constrained dynamic equations are correct in theory, but are not stable + * during numeric integration. RBDL implements Baumgarte stabilization to avoid + * the accumulation of position and velocity errors. + * + * The dynamic equations are changed to the following form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + - \lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \gamma _{stab} + \end{array} + \right) + * \f] A term \f$\gamma _{stab}\f$ is added to the right hand side of the + * equation, defined in the following way: \f[ + \gamma _{stab} = + - \frac{2}{T_{stab}} \dot{\phi} (q) + - \left(\frac{1}{T_{stab}} \right)^2 \phi (q) + * \f] where \f$\phi (q)\f$ are the position level constraint errors and + * \f$T_{stab}\f$ is a tuning coefficient. The value of \f$T_{stab}\f$ must + * be chosen carefully. If it is too large the stabilization will not be able + * to compensate the errors and the position and velocity errors will increase. + * If it is too small, the system of equations will become unnecessarily stiff. + * + * @{ + */ + +struct Model; + +/** \brief Structure that contains both constraint information and workspace memory. + * + * This structure is used to reduce the amount of memory allocations that + * are needed when computing constraint forces. + * + * The ConstraintSet has to be bound to a model using ConstraintSet::Bind() + * before it can be used in \link RigidBodyDynamics::ForwardDynamicsContacts + * ForwardDynamicsContacts \endlink. + */ +struct RBDL_DLLAPI ConstraintSet { + ConstraintSet() : + linear_solver (Math::LinearSolverColPivHouseholderQR), + bound (false) {} + + // Enum to describe the type of a constraint. + enum ConstraintType { + ContactConstraint, + LoopConstraint, + ConstraintTypeLast, + }; + + /** \brief Adds a contact constraint to the constraint set. + * + * This type of constraints ensures that the velocity and acceleration of a specified + * body point along a specified axis are null. This constraint does not act + * at the position level. + * + * \param body_id the body which is affected directly by the constraint + * \param body_point the point that is constrained relative to the + * contact body + * \param world_normal the normal along the constraint acts (in base + * coordinates) + * \param constraint_name a human readable name (optional, default: NULL) + * \param normal_acceleration the acceleration of the contact along the normal + * (optional, default: 0.) + */ + unsigned int AddContactConstraint ( + unsigned int body_id, + const Math::Vector3d &body_point, + const Math::Vector3d &world_normal, + const char *constraint_name = NULL, + double normal_acceleration = 0.); + + /** \brief Adds a loop constraint to the constraint set. + * + * This type of constraints ensures that the relative orientation and position, + * spatial velocity, and spatial acceleration between two frames in two bodies + * are null along a specified spatial constraint axis. + * + * \param id_predecessor the identifier of the predecessor body + * \param id_successor the identifier of the successor body + * \param X_predecessor a spatial transform localizing the constrained + * frames on the predecessor body, expressed with respect to the predecessor + * body frame + * \param X_successor a spatial transform localizing the constrained + * frames on the successor body, expressed with respect to the successor + * body frame + * \param axis a spatial vector indicating the axis along which the constraint + * acts + * \param T_stab_inv coefficient for Baumgarte stabilization. + * \param constraint_name a human readable name (optional, default: NULL) + * + */ + unsigned int AddLoopConstraint( + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + const Math::SpatialVector &axis, + double T_stab_inv, + const char *constraint_name = NULL + ); + + /** \brief Copies the constraints and resets its ConstraintSet::bound + * flag. + */ + ConstraintSet Copy() { + ConstraintSet result (*this); + result.bound = false; + + return result; + } + + /** \brief Specifies which method should be used for solving undelying linear systems. + */ + void SetSolver (Math::LinearSolver solver) { + linear_solver = solver; + } + + /** \brief Initializes and allocates memory for the constraint set. + * + * This function allocates memory for temporary values and matrices that + * are required for contact force computation. Both model and constraint + * set must not be changed after a binding as the required memory is + * dependent on the model size (i.e. the number of bodies and degrees of + * freedom) and the number of constraints in the Constraint set. + * + * The values of ConstraintSet::acceleration may still be + * modified after the set is bound to the model. + * + */ + bool Bind (const Model &model); + + /** \brief Returns the number of constraints. */ + size_t size() const { + return acceleration.size(); + } + + /** \brief Clears all variables in the constraint set. */ + void clear (); + + /// Method that should be used to solve internal linear systems. + Math::LinearSolver linear_solver; + /// Whether the constraint set was bound to a model (mandatory!). + bool bound; + + // Common constraints variables. + std::vector constraintType; + std::vector name; + std::vector contactConstraintIndices; + std::vector loopConstraintIndices; + + // Contact constraints variables. + std::vector body; + std::vector point; + std::vector normal; + + // Loop constraints variables. + std::vector body_p; + std::vector body_s; + std::vector X_p; + std::vector X_s; + std::vector constraintAxis; + /** Baumgarte stabilization parameter */ + std::vector T_stab_inv; + /** Position error for the Baumgarte stabilization */ + Math::VectorNd err; + /** Velocity error for the Baumgarte stabilization */ + Math::VectorNd errd; + + /** Enforced accelerations of the contact points along the contact + * normal. */ + Math::VectorNd acceleration; + /** Actual constraint forces along the contact normals. */ + Math::VectorNd force; + /** Actual constraint impulses along the contact normals. */ + Math::VectorNd impulse; + /** The velocities we want to have along the contact normals */ + Math::VectorNd v_plus; + + // Variables used by the Lagrangian methods + + /// Workspace for the joint space inertia matrix. + Math::MatrixNd H; + /// Workspace for the coriolis forces. + Math::VectorNd C; + /// Workspace of the lower part of b. + Math::VectorNd gamma; + Math::MatrixNd G; + /// Workspace for the Lagrangian left-hand-side matrix. + Math::MatrixNd A; + /// Workspace for the Lagrangian right-hand-side. + Math::VectorNd b; + /// Workspace for the Lagrangian solution. + Math::VectorNd x; + + /// Workspace when evaluating contact Jacobians + Math::MatrixNd Gi; + /// Workspace when evaluating loop Jacobians + Math::MatrixNd GSpi; + /// Workspace when evaluating loop Jacobians + Math::MatrixNd GSsi; + /// Workspace when evaluating loop Jacobians + Math::MatrixNd GSJ; + + /// Workspace for the QR decomposition of the null-space method +#ifdef RBDL_USE_SIMPLE_MATH + SimpleMath::HouseholderQR GT_qr; +#else + Eigen::HouseholderQR GT_qr; +#endif + + Math::MatrixNd GT_qr_Q; + Math::MatrixNd Y; + Math::MatrixNd Z; + Math::VectorNd qddot_y; + Math::VectorNd qddot_z; + + // Variables used by the IABI methods + + /// Workspace for the Inverse Articulated-Body Inertia. + Math::MatrixNd K; + /// Workspace for the accelerations of due to the test forces + Math::VectorNd a; + /// Workspace for the test accelerations. + Math::VectorNd QDDot_t; + /// Workspace for the default accelerations. + Math::VectorNd QDDot_0; + /// Workspace for the test forces. + std::vector f_t; + /// Workspace for the actual spatial forces. + std::vector f_ext_constraints; + /// Workspace for the default point accelerations. + std::vector point_accel_0; + + /// Workspace for the bias force due to the test force + std::vector d_pA; + /// Workspace for the acceleration due to the test force + std::vector d_a; + Math::VectorNd d_u; + + /// Workspace for the inertia when applying constraint forces + std::vector d_IA; + /// Workspace when applying constraint forces + std::vector d_U; + /// Workspace when applying constraint forces + Math::VectorNd d_d; + + std::vector d_multdof3_u; +}; + +/** \brief Computes the position errors for the given ConstraintSet. + * + * \param model the model + * \param Q the generalized positions of the joints + * \param CS the constraint set for which the error should be computed + * \param err (output) vector where the error will be stored in (should have + * the size of CS). + * \param update_kinematics whether the kinematics of the model should be + * updated from Q. + * + * \note the position error is always 0 for contact constraints. + * + */ +RBDL_DLLAPI +void CalcConstraintsPositionError( + Model& model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics = true +); + +/** \brief Computes the Jacobian for the given ConstraintSet + * + * \param model the model + * \param Q the generalized positions of the joints + * \param CS the constraint set for which the Jacobian should be computed + * \param G (output) matrix where the output will be stored in + * \param update_kinematics whether the kinematics of the model should be + * updated from Q + * + */ +RBDL_DLLAPI +void CalcConstraintsJacobian( + Model &model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::MatrixNd &G, + bool update_kinematics = true +); + +/** \brief Computes the velocity errors for the given ConstraintSet. + * + * + * \param model the model + * \param Q the generalized positions of the joints + * \param QDot the generalized velocities of the joints + * \param CS the constraint set for which the error should be computed + * \param err (output) vector where the error will be stored in (should have + * the size of CS). + * \param update_kinematics whether the kinematics of the model should be + * updated from Q. + * + * \note this is equivalent to multiplying the constraint jacobian by the + * generalized velocities of the joints. + * + */ +RBDL_DLLAPI +void CalcConstraintsVelocityError( + Model& model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics = true +); + +/** \brief Computes the terms \f$H\f$, \f$G\f$, and \f$\gamma\f$ of the + * constrained dynamic problem and stores them in the ConstraintSet. + * + * + * \param model the model + * \param Q the generalized positions of the joints + * \param QDot the generalized velocities of the joints + * \param Tau the generalized forces of the joints + * \param CS the constraint set for which the error should be computed + * + * \note This function is normally called automatically in the various + * constrained dynamics functions, the user normally does not have to call it. + * + */ +RBDL_DLLAPI +void CalcConstrainedSystemVariables ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS +); + +/** \brief Computes a feasible initial value of the generalized joint positions. + * + * \param model the model + * \param QInit initial guess for the generalized positions of the joints + * \param CS the constraint set for which the error should be computed + * \param Q (output) vector of the generalized joint positions. + * \param weights weighting coefficients for the different joint positions. + * \param tolerance the function will return successfully if the constraint + * position error norm is lower than this value. + * \param max_iter the funciton will return unsuccessfully after performing + * this number of iterations. + * + * \return true if the generalized joint positions were computed successfully, + * false otherwise. + * + */ +RBDL_DLLAPI +bool CalcAssemblyQ( + Model &model, + Math::VectorNd QInit, + ConstraintSet &CS, + Math::VectorNd &Q, + const Math::VectorNd &weights, + double tolerance = 1e-12, + unsigned int max_iter = 100 +); + +/** \brief Computes a feasible initial value of the generalized joint velocities. + * + * \param model the model + * \param Q the generalized joint position of the joints. It is assumed that + * this vector satisfies the position level assemblt constraints. + * \param QDotInit initial guess for the generalized velocities of the joints + * \param CS the constraint set for which the error should be computed + * \param QDot (output) vector of the generalized joint velocities. + * \param weights weighting coefficients for the different joint positions. + * + */ +RBDL_DLLAPI +void CalcAssemblyQDot( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotInit, + ConstraintSet &CS, + Math::VectorNd &QDot, + const Math::VectorNd &weights +); + +/** \brief Computes forward dynamics with contact by constructing and solving + * the full lagrangian equation + * + * This method builds and solves the linear system \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + -\lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$C\f$ the bias force (sometimes called "non-linear + * effects"), and \f$\gamma\f$ the generalized acceleration independent + * part of the contact point accelerations. + * + * \note So far, only constraints acting along cartesian coordinate axes + * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must + * not specify redundant constraints! + * + * \par + * + * \note To increase performance group constraints body and pointwise such + * that constraints acting on the same body point are sequentially in + * ConstraintSet. This can save computation of point jacobians \f$G\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param CS the description of all acting constraints + * \param QDDot accelerations of the internals joints (output) + * + * \note During execution of this function values such as + * ConstraintSet::force get modified and will contain the value + * of the force acting along the normal. + * + */ +RBDL_DLLAPI +void ForwardDynamicsConstraintsDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot +); + +RBDL_DLLAPI +void ForwardDynamicsConstraintsRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot +); + +RBDL_DLLAPI +void ForwardDynamicsConstraintsNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot +); + +/** \brief Computes forward dynamics that accounts for active contacts in + * ConstraintSet. + * + * The method used here is the one described by Kokkevis and Metaxas in the + * Paper "Practical Physics for Articulated Characters", Game Developers + * Conference, 2004. + * + * It does this by recursively computing the inverse articulated-body inertia (IABI) + * \f$\Phi_{i,j}\f$ which is then used to build and solve a system of the form: + \f[ + \left( + \begin{array}{c} + \dot{v}_1 \\ + \dot{v}_2 \\ + \vdots \\ + \dot{v}_n + \end{array} + \right) + = + \left( + \begin{array}{cccc} + \Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\ + \Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\ + \cdots & \cdots & \cdots & \vdots \\ + \Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n} + \end{array} + \right) + \left( + \begin{array}{c} + f_1 \\ + f_2 \\ + \vdots \\ + f_n + \end{array} + \right) + + + \left( + \begin{array}{c} + \phi_1 \\ + \phi_2 \\ + \vdots \\ + \phi_n + \end{array} + \right). + \f] + Here \f$n\f$ is the number of constraints and the method for building the system + uses the Articulated Body Algorithm to efficiently compute entries of the system. The + values \f$\dot{v}_i\f$ are the constraint accelerations, \f$f_i\f$ the constraint forces, + and \f$\phi_i\f$ are the constraint bias forces. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param CS a list of all contact points + * \param QDDot accelerations of the internals joints (output) + * + * \note During execution of this function values such as + * ConstraintSet::force get modified and will contain the value + * of the force acting along the normal. + * + * \note This function supports only contact constraints. + * + * \todo Allow for external forces + */ +RBDL_DLLAPI +void ForwardDynamicsContactsKokkevis ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot +); + +/** \brief Computes contact gain by constructing and solving the full lagrangian + * equation + * + * This method builds and solves the linear system \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \dot{q}^{+} \\ + \Lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + H \dot{q}^{-} \\ + v^{+} + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the + * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ + * the generalized velocity before the impact, and \f$v^{+}\f$ the desired + * velocity of each constraint after the impact (known beforehand, usually + * 0). The value of \f$v^{+}\f$ can is specified via the variable + * ConstraintSet::v_plus and defaults to 0. + * + * \note So far, only constraints acting along cartesian coordinate axes + * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must + * not specify redundant constraints! + * + * \par + * + * \note To increase performance group constraints body and pointwise such + * that constraints acting on the same body point are sequentially in + * ConstraintSet. This can save computation of point Jacobians \f$G\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDotMinus velocity vector of the internal joints before the impact + * \param CS the set of active constraints + * \param QDotPlus velocities of the internals joints after the impact (output) + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Resolves contact gain using SolveContactSystemRangeSpaceSparse() + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Resolves contact gain using SolveContactSystemNullSpace() + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Solves the full contact system directly, i.e. simultaneously for + * contact forces and joint accelerations. + * + * This solves a \f$ (n_\textit{dof} + + * n_c) \times (n_\textit{dof} + n_c\f$ linear system. + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param A work-space for the matrix of the linear system + * \param b work-space for the right-hand-side of the linear system + * \param x work-space for the solution of the linear system + * \param type of solver that should be used to solve the system + */ +RBDL_DLLAPI +void SolveConstrainedSystemDirect ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &A, + Math::VectorNd &b, + Math::VectorNd &x, + Math::LinearSolver &linear_solver +); + +/** \brief Solves the contact system by first solving for the the joint + * accelerations and then the contact forces using a sparse matrix + * decomposition of the joint space inertia matrix. + * + * This method exploits the branch-induced sparsity by the structure + * preserving \f$L^TL \f$ decomposition described in RBDL, Section 6.5. + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param K work-space for the matrix of the constraint force linear system + * \param a work-space for the right-hand-side of the constraint force linear + * system + * \param linear_solver type of solver that should be used to solve the + * constraint force system + */ +RBDL_DLLAPI +void SolveConstrainedSystemRangeSpaceSparse ( + Model &model, + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &K, + Math::VectorNd &a, + Math::LinearSolver linear_solver +); + +/** \brief Solves the contact system by first solving for the joint + * accelerations and then for the constraint forces. + * + * This methods requires a \f$n_\textit{dof} \times n_\textit{dof}\f$ + * matrix of the form \f$\left[ \ Y \ | Z \ \right]\f$ with the property + * \f$GZ = 0\f$ that can be computed using a QR decomposition (e.g. see + * code for ForwardDynamicsContactsNullSpace()). + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param Y basis for the range-space of the constraints + * \param Z basis for the null-space of the constraints + * \param qddot_y work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ + * \param qddot_z work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ + * \param linear_solver type of solver that should be used to solve the system + */ +RBDL_DLLAPI +void SolveConstrainedSystemNullSpace ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &Y, + Math::MatrixNd &Z, + Math::VectorNd &qddot_y, + Math::VectorNd &qddot_z, + Math::LinearSolver &linear_solver +); + +/** @} */ + +} /* namespace RigidBodyDynamics */ + +/* RBDL_CONSTRAINTS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Dynamics.h b/3rdparty/rbdl/include/rbdl/Dynamics.h new file mode 100644 index 0000000..af490e9 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Dynamics.h @@ -0,0 +1,182 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_DYNAMICS_H +#define RBDL_DYNAMICS_H + +#include +#include + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +struct Model; + +/** \page dynamics_page Dynamics + * + * All functions related to kinematics are specified in the \ref + * dynamics_group "Dynamics Module". + * + * \defgroup dynamics_group Dynamics + * @{ + */ + +/** \brief Computes inverse dynamics with the Newton-Euler Algorithm + * + * This function computes the generalized forces from given generalized + * states, velocities, and accelerations: + * \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$ + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot accelerations of the internals joints + * \param Tau actuations of the internal joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + */ +RBDL_DLLAPI void InverseDynamics ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + Math::VectorNd &Tau, + std::vector *f_ext = NULL + ); + +/** \brief Computes the coriolis forces + * + * This function computes the generalized forces from given generalized + * states, velocities, and accelerations: + * \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$ + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints (output) + */ +RBDL_DLLAPI void NonlinearEffects ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + Math::VectorNd &Tau + ); + +/** \brief Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm + * + * This function computes the joint space inertia matrix from a given model and + * the generalized state vector: + * \f$ M(q) \f$ + * + * \param model rigid body model + * \param Q state vector of the model + * \param H a matrix where the result will be stored in + * \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost!) + * + * \note This function only evaluates the entries of H that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling H.setZero(). + */ +RBDL_DLLAPI void CompositeRigidBodyAlgorithm ( + Model& model, + const Math::VectorNd &Q, + Math::MatrixNd &H, + bool update_kinematics = true + ); + +/** \brief Computes forward dynamics with the Articulated Body Algorithm + * + * This function computes the generalized accelerations from given + * generalized states, velocities and forces: + * \f$ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)\f$ + * It does this by using the recursive Articulated Body Algorithm that runs + * in \f$O(n_{dof})\f$ with \f$n_{dof}\f$ being the number of joints. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param QDDot accelerations of the internal joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + */ +RBDL_DLLAPI void ForwardDynamics ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + std::vector *f_ext = NULL + ); + +/** \brief Computes forward dynamics by building and solving the full Lagrangian equation + * + * This method builds and solves the linear system + * \f[ H \ddot{q} = -C + \tau \f] + * for \f$\ddot{q}\f$ where \f$H\f$ is the joint space inertia matrix + * computed with the CompositeRigidBodyAlgorithm(), \f$C\f$ the bias + * force (sometimes called "non-linear effects"). + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param QDDot accelerations of the internal joints (output) + * \param linear_solver specification which method should be used for solving the linear system + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + * \param H preallocated workspace area for the joint space inertia matrix of size dof_count x dof_count (optional, defaults to NULL and allocates temporary matrix) + * \param C preallocated workspace area for the right hand side vector of size dof_count x 1 (optional, defaults to NULL and allocates temporary vector) + */ +RBDL_DLLAPI void ForwardDynamicsLagrangian ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR, + std::vector *f_ext = NULL, + Math::MatrixNd *H = NULL, + Math::VectorNd *C = NULL + ); + +/** \brief Computes the effect of multiplying the inverse of the joint + * space inertia matrix with a vector in linear time. + * + * \param model rigid body model + * \param Q state vector of the generalized positions + * \param Tau the vector that should be multiplied with the inverse of + * the joint space inertia matrix + * \param QDDot vector where the result will be stored + * \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost) + * + * This function uses a reduced version of the Articulated %Body Algorithm + * to compute + * + * \f$ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)\f$ + * + * in \f$O(n_{\textit{dof}}\f$) time. + * + * \note When calling this function repeatedly for the same values of Q make sure + * to set the last parameter to false as this avoids expensive + * recomputations of transformations and articulated body inertias. + */ +RBDL_DLLAPI void CalcMInvTimesTau ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + bool update_kinematics=true + ); + +/** @} */ + +} + +/* RBDL_DYNAMICS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Joint.h b/3rdparty/rbdl/include/rbdl/Joint.h new file mode 100644 index 0000000..7dce1c0 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Joint.h @@ -0,0 +1,694 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_JOINT_H +#define RBDL_JOINT_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +struct Model; + +/** \page joint_description Joint Modeling + * + * \section joint_overview Overview + * + * The Rigid Body Dynamics Library supports a multitude of joints: + * revolute, planar, fixed, singularity-free spherical joints and joints + * with multiple degrees of freedom in any combinations. + * + * Fixed joints do not cause any overhead in RBDL as the bodies that are + * rigidly connected are merged into a single body. For details see \ref + * joint_models_fixed. + * + * Joints with multiple degrees of freedom are emulated by default which + * means that they are split up into multiple single degree of freedom + * joints which results in equivalent models. This has the benefit that it + * simplifies the required algebra and also code branching in RBDL. A + * special case are joints with three degrees of freedom for which specific + * joints are available that should be used for performance reasons + * whenever possible. See \ref joint_three_dof for details. + * + * Joints are defined by their motion subspace. For each degree of freedom + * a one dimensional motion subspace is specified as a Math::SpatialVector. + * This vector follows the following convention: \f[ (r_x, r_y, r_z, t_x, + * t_y, t_z) \f] + * + * To specify a planar joint with three degrees of freedom for which the + * first two are translations in \f$x\f$ and \f$y\f$ direction and the last + * is a rotation around \f$z\f$, the following joint definition can be + * used: + + * \code Joint planar_joint = Joint ( + * Math::SpatialVector (0., 0., 0., 1., 0., 0.), + * Math::SpatialVector (0., 0., 0., 0., 1., 0.), + * Math::SpatialVector (0., 0., 1., 0., 0., 0.) + * ); + * \endcode + + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + * + * \section joint_models_fixed Fixed Joints + * + * Fixed joints do not add an additional degree of freedom to the model. + * When adding a body that via a fixed joint (i.e. when the type is + * JointTypeFixed) then the dynamical parameters mass and inertia are + * merged onto its moving parent. By doing so fixed bodies do not add + * computational costs when performing dynamics computations. + + * To ensure a consistent API for the Kinematics such fixed bodies have a + * different range of ids. Where as the ids start at 1 get incremented for + * each added body, fixed bodies start at Model::fixed_body_discriminator + * which has a default value of std::numeric_limits::max() / + * 2. This means theoretical a maximum of each 2147483646 movable and fixed + * bodies are possible. + + * To check whether a body is connected by a fixed joint you can use the + * function Model::IsFixedBodyId(). + + * \section joint_three_dof 3-DoF Joints + * + * RBDL has highly efficient implementations for the following three degree + * of freedom joints: + *
    + *
  • \ref JointTypeTranslationXYZ which first translates along X, then + * Y, and finally Z.
    • + *
  • \ref JointTypeEulerZYX which first rotates around Z, then Y, and + * then X.
    • + *
  • \ref JointTypeEulerXYZ which first rotates around X, then Y, and + * then Z.
    • + *
  • \ref JointTypeEulerYXZ which first rotates around Y, then X, and + * then Z.
    • + *
  • \ref JointTypeSpherical which is a singularity free joint that + * uses a Quaternion and the bodies angular velocity (see \ref + * joint_singularities for details).
    • + *
+ * + * These joints can be created by providing the joint type as an argument + * to the Joint constructor, e.g.: + * + * \code Joint joint_rot_zyx = Joint ( JointTypeEulerZYX ); \endcode + * + * Using 3-Dof joints is always favourable over using their emulated + * counterparts as they are considerably faster and describe the same + * kinematics and dynamics. + + * \section joint_floatingbase Floating-Base Joint (a.k.a. Freeflyer Joint) + * + * RBDL has a special joint type for floating-base systems that uses the + * enum JointTypeFloatingBase. The first three DoF are translations along + * X,Y, and Z. For the rotational part it uses a JointTypeSpherical joint. + * It is internally modeled by a JointTypeTranslationXYZ and a + * JointTypeSpherical joint. It is recommended to only use this joint for + * the very first body added to the model. + * + * Positional variables are translations along X, Y, and Z, and for + * rotations it uses Quaternions. To set/get the orientation use + * Model::SetQuaternion () / Model::GetQuaternion() with the body id + * returned when adding the floating base (i.e. the call to + * Model::AddBody() or Model::AppendBody()). + + * \section joint_singularities Joint Singularities + + * Singularities in the models arise when a joint has three rotational + * degrees of freedom and the rotations are described by Euler- or + * Cardan-angles. The singularities present in these rotation + * parametrizations (e.g. for ZYX Euler-angles for rotations where a + * +/- 90 degrees rotation around the Y-axis) may cause problems in + * dynamics calculations, such as a rank-deficit joint-space inertia matrix + * or exploding accelerations in the forward dynamics calculations. + * + * For this case RBDL has the special joint type + * RigidBodyDynamics::JointTypeSpherical. When using this joint type the + * model does not suffer from singularities, however this also results in + * a change of interpretation for the values \f$\mathbf{q}, \mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, and \f$\mathbf{\tau}\f$: + * + *
    + *
  • The values in \f$\mathbf{q}\f$ for the joint parameterizes the orientation of a joint using a + * Quaternion \f$q_i\f$
    • + *
  • The values in \f$\mathbf{\dot{q}}\f$ for the joint describe the angular + * velocity \f$\omega\f$ of the joint in body coordinates
    • + *
  • The values in \f$\mathbf{\ddot{q}}\f$ for the joint describe the angular + * acceleration \f$\dot{\omega}\f$ of the joint in body coordinates
    • + *
  • The values in \f$\mathbf{\tau}\f$ for the joint describe the three couples + * acting on the body in body coordinates that are actuated by the joint.
    • + *
+ + * As a result, the dimension of the vector \f$\mathbf{q}\f$ is higher than + * of the vector of the velocity variables. Additionally, the values in + * \f$\mathbf{\dot{q}}\f$ are \b not the derivative of \f$q\f$ and are therefore + * denoted by \f$\mathbf{\bar{q}}\f$ (and similarly for the joint + * accelerations). + + * RBDL stores the Quaternions in \f$\mathbf{q}\f$ such that the 4th component of + * the joint is appended to \f$\mathbf{q}\f$. E.g. for a model with the joints: + * TX, Spherical, TY, Spherical, the values of \f$\mathbf{q},\mathbf{\bar{q}},\mathbf{\bar{\bar{q}}},\mathbf{\tau}\f$ are: + * + + \f{eqnarray*} + \mathbf{q} &=& ( q_{tx}, q_{q1,x}, q_{q1,y}, q_{q1,z}, q_{ty}, q_{q2,x}, q_{q2,y}, q_{q2,z}, q_{q1,w}, q_{q2,w})^T \\ + \mathbf{\bar{q}} &=& ( \dot{q}_{tx}, \omega_{1,x}, \omega_{1,y}, \omega_{1,z}, \dot{q}_{ty}, \omega_{2,x}, \omega_{2,y}, \omega_{2,z} )^T \\ + \mathbf{\bar{\bar{q}}} &=& ( \ddot{q}_{tx}, \dot{\omega}_{1,x}, \dot{\omega}_{1,y}, \dot{\omega}_{1,z}, \ddot{q}_{ty}, \dot{\omega}_{2,x}, \dot{\omega}_{2,y}, \dot{\omega}_{2,z} )^T \\ + \mathbf{\tau} &=& ( \tau_{tx}, \tau_{1,x}, \tau_{1,y}, \tau_{1,z}, \tau_{ty}, \tau_{2,x}, \tau_{2,y}, \tau_{2,z} )^T + \f} + + * \subsection spherical_integration Numerical Integration of Quaternions + * + * An additional consequence of this is, that special treatment is required + * when numerically integrating the angular velocities. One possibility is + * to interpret the angular velocity as an axis-angle pair scaled by the + * timestep and use it create a quaternion that is applied to the previous + * Quaternion. Another is to compute the quaternion rates from the angular + * velocity. For details see James Diebel "Representing Attitude: Euler + * Angles, Unit Quaternions, and Rotation Vectors", 2006, + * http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134. + * + */ + + /** \brief General types of joints + */ + enum JointType { + JointTypeUndefined = 0, + JointTypeRevolute, + JointTypePrismatic, + JointTypeRevoluteX, + JointTypeRevoluteY, + JointTypeRevoluteZ, + JointTypeSpherical, ///< 3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity variables. + JointTypeEulerZYX, ///< 3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints). + JointTypeEulerXYZ, ///< 3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints). + JointTypeEulerYXZ, ///< 3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints). + JointTypeTranslationXYZ, + JointTypeFloatingBase, ///< A 6-DoF joint for floating-base (or freeflyer) systems. + JointTypeFixed, ///< Fixed joint which causes the inertial properties to be merged with the parent body. + JointTypeHelical, //1 DoF joint with both rotational and translational motion + JointType1DoF, + JointType2DoF, ///< Emulated 2 DoF joint. + JointType3DoF, ///< Emulated 3 DoF joint. + JointType4DoF, ///< Emulated 4 DoF joint. + JointType5DoF, ///< Emulated 5 DoF joint. + JointType6DoF, ///< Emulated 6 DoF joint. + JointTypeCustom, ///< User defined joints of varying size + }; + +/** \brief Describes a joint relative to the predecessor body. + * + * This class contains all information required for one single joint. This + * contains the joint type and the axis of the joint. See \ref joint_description for detailed description. + * + */ +struct RBDL_DLLAPI Joint { + Joint() : + mJointAxes (NULL), + mJointType (JointTypeUndefined), + mDoFCount (0), + q_index (0) {}; + Joint (JointType type) : + mJointAxes (NULL), + mJointType (type), + mDoFCount (0), + custom_joint_index(-1), + q_index (0) { + if (type == JointTypeRevoluteX) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeRevoluteY) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + } else if (type == JointTypeRevoluteZ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeSpherical) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeEulerZYX) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeEulerXYZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeEulerYXZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeTranslationXYZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 0., 1., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 0., 0., 0., 1., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 0., 0., 0., 1.); + } else if (type >= JointType1DoF && type <= JointType6DoF) { + // create a joint and allocate memory for it. + // Warning: the memory does not get initialized by this function! + mDoFCount = type - JointType1DoF + 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + std::cerr << "Warning: uninitalized vector" << std::endl; + } else if (type == JointTypeCustom) { + //This constructor cannot be used for a JointTypeCustom because + //we have no idea what mDoFCount is. + std::cerr << "Error: Invalid use of Joint constructor Joint(JointType" + << " type). Only allowed when type != JointTypeCustom" + << std::endl; + assert(0); + abort(); + } else if (type != JointTypeFixed && type != JointTypeFloatingBase) { + std::cerr << "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed or JointTypeSpherical." << std::endl; + assert (0); + abort(); + } + } + Joint (JointType type, int degreesOfFreedom) : + mJointAxes (NULL), + mJointType (type), + mDoFCount (0), + custom_joint_index(-1), + q_index (0) { + if (type == JointTypeCustom) { + mDoFCount = degreesOfFreedom; + mJointAxes = new Math::SpatialVector[mDoFCount]; + for(int i=0; i 0) { + assert (mJointAxes); + delete[] mJointAxes; + } + mJointType = joint.mJointType; + mDoFCount = joint.mDoFCount; + custom_joint_index = joint.custom_joint_index; + mJointAxes = new Math::SpatialVector[mDoFCount]; + + for (unsigned int i = 0; i < mDoFCount; i++) + mJointAxes[i] = joint.mJointAxes[i]; + + q_index = joint.q_index; + } + return *this; + } + ~Joint() { + if (mJointAxes) { + assert (mJointAxes); + delete[] mJointAxes; + mJointAxes = NULL; + mDoFCount = 0; + custom_joint_index = -1; + } + } + + /** \brief Constructs a joint from the given cartesian parameters. + * + * This constructor creates all the required spatial values for the given + * cartesian parameters. + * + * \param joint_type whether the joint is revolute or prismatic + * \param joint_axis the axis of rotation or translation + */ + Joint ( + const JointType joint_type, + const Math::Vector3d &joint_axis + ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + + // Some assertions, as we concentrate on simple cases + + // Only rotation around the Z-axis + assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic ); + + mJointType = joint_type; + + if (joint_type == JointTypeRevolute) { + // make sure we have a unit axis + mJointAxes[0].set ( + joint_axis[0], + joint_axis[1], + joint_axis[2], + 0., 0., 0. + ); + + } else if (joint_type == JointTypePrismatic) { + // make sure we have a unit axis + assert (joint_axis.squaredNorm() == 1.); + + mJointAxes[0].set ( + 0., 0., 0., + joint_axis[0], + joint_axis[1], + joint_axis[2] + ); + } + } + + /** \brief Constructs a 1 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \param axis_0 Motion subspace for axis 0 + */ + Joint ( + const Math::SpatialVector &axis_0 + ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (axis_0); + if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.)) { + mJointType = JointTypeRevoluteX; + } else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.)) { + mJointType = JointTypeRevoluteY; + } else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.)) { + mJointType = JointTypeRevoluteZ; + } else if (axis_0[0] == 0 && + axis_0[1] == 0 && + axis_0[2] == 0) { + mJointType = JointTypePrismatic; + } else { + mJointType = JointTypeHelical; + } + validate_spatial_axis (mJointAxes[0]); + } + + /** \brief Constructs a 2 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1 + ) { + mJointType = JointType2DoF; + mDoFCount = 2; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + } + + /** \brief Constructs a 3 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2 + ) { + mJointType = JointType3DoF; + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + } + + /** \brief Constructs a 4 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3 + ) { + mJointType = JointType4DoF; + mDoFCount = 4; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + } + + /** \brief Constructs a 5 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + * \param axis_4 Motion subspace for axis 4 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3, + const Math::SpatialVector &axis_4 + ) { + mJointType = JointType5DoF; + mDoFCount = 5; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + mJointAxes[4] = axis_4; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + validate_spatial_axis (mJointAxes[4]); + } + + /** \brief Constructs a 6 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + * \param axis_4 Motion subspace for axis 4 + * \param axis_5 Motion subspace for axis 5 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3, + const Math::SpatialVector &axis_4, + const Math::SpatialVector &axis_5 + ) { + mJointType = JointType6DoF; + mDoFCount = 6; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + mJointAxes[4] = axis_4; + mJointAxes[5] = axis_5; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + validate_spatial_axis (mJointAxes[4]); + validate_spatial_axis (mJointAxes[5]); + } + + /** \brief Checks whether we have pure rotational or translational axis. + * + * This function is mainly used to print out warnings when specifying an + * axis that might not be intended. + */ + bool validate_spatial_axis (Math::SpatialVector &axis) { + + bool axis_rotational = false; + bool axis_translational = false; + + Math::Vector3d rotation (axis[0], axis[1], axis[2]); + Math::Vector3d translation (axis[3], axis[4], axis[5]); + + if (fabs(rotation.norm()) > 1.0e-8) + axis_rotational = true; + + if (fabs(translation.norm()) > 1.0e-8) + axis_translational = true; + + if(axis_rotational && rotation.norm() - 1.0 > 1.0e-8) { + std::cerr << "Warning: joint rotation axis is not unit!" << std::endl; + } + + if(axis_translational && translation.norm() - 1.0 > 1.0e-8) { + std::cerr << "Warning: joint translation axis is not unit!" << std::endl; + } + + return axis_rotational != axis_translational; + } + + /// \brief The spatial axes of the joint + Math::SpatialVector* mJointAxes; + /// \brief Type of joint + JointType mJointType; + /// \brief Number of degrees of freedom of the joint. Note: CustomJoints + // have here a value of 0 and their actual numbers of degrees of freedom + // can be obtained using the CustomJoint structure. + unsigned int mDoFCount; + unsigned int q_index; + unsigned int custom_joint_index; +}; + +/** \brief Computes all variables for a joint model + * + * By appropriate modification of this function all types of joints can be + * modeled. See RBDA Section 4.4 for details. + * + * \param model the rigid body model + * \param joint_id the id of the joint we are interested in. This will be used to determine the type of joint and also the entries of \f[ q, \dot{q} \f]. + * \param q joint state variables + * \param qdot joint velocity variables + */ +RBDL_DLLAPI +void jcalc ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ); + +RBDL_DLLAPI +Math::SpatialTransform jcalc_XJ ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q); + +RBDL_DLLAPI +void jcalc_X_lambda_S ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ); + +struct RBDL_DLLAPI CustomJoint { + CustomJoint() + { } + virtual ~CustomJoint() {}; + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ) = 0; + virtual void jcalc_X_lambda_S (Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ) = 0; + + unsigned int mDoFCount; + Math::SpatialTransform XJ; + Math::MatrixNd S; + Math::MatrixNd U; + Math::MatrixNd Dinv; + Math::VectorNd u; + Math::VectorNd d_u; +}; + +} + +/* RBDL_JOINT_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Kinematics.h b/3rdparty/rbdl/include/rbdl/Kinematics.h new file mode 100644 index 0000000..135fb5e --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Kinematics.h @@ -0,0 +1,415 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2015 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_KINEMATICS_H +#define RBDL_KINEMATICS_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +/** \page kinematics_page Kinematics + * All functions related to kinematics are specified in the \ref + * kinematics_group "Kinematics Module". + * + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + * + * \defgroup kinematics_group Kinematics + * @{ + * + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + */ + +/** \brief Updates and computes velocities and accelerations of the bodies + * + * This function updates the kinematic variables such as body velocities + * and accelerations in the model to reflect the variables passed to this function. + * + * \param model the model + * \param Q the positional variables of the model + * \param QDot the generalized velocities of the joints + * \param QDDot the generalized accelerations of the joints + */ +RBDL_DLLAPI void UpdateKinematics (Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot + ); + +/** \brief Selectively updates model internal states of body positions, velocities and/or accelerations. + * + * This function updates the kinematic variables such as body velocities and + * accelerations in the model to reflect the variables passed to this function. + * + * In contrast to UpdateKinematics() this function allows to update the model + * state with values one is interested and thus reduce computations (e.g. only + * positions, only positions + accelerations, only velocities, etc.). + + * \param model the model + * \param Q the positional variables of the model + * \param QDot the generalized velocities of the joints + * \param QDDot the generalized accelerations of the joints + */ +RBDL_DLLAPI void UpdateKinematicsCustom (Model &model, + const Math::VectorNd *Q, + const Math::VectorNd *QDot, + const Math::VectorNd *QDDot + ); + +/** \brief Returns the base coordinates of a point given in body coordinates. + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param body_point_position coordinates of the point in body coordinates + * \param update_kinematics whether UpdateKinematics() should be called + * or not (default: true) + * + * \returns a 3-D vector with coordinates of the point in base coordinates + */ +RBDL_DLLAPI Math::Vector3d CalcBodyToBaseCoordinates ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &body_point_position, + bool update_kinematics = true); + +/** \brief Returns the body coordinates of a point given in base coordinates. + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param base_point_position coordinates of the point in base coordinates + * \param update_kinematics whether UpdateKinematics() should be called or not + * (default: true). + * + * \returns a 3-D vector with coordinates of the point in body coordinates + */ +RBDL_DLLAPI Math::Vector3d CalcBaseToBodyCoordinates ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &base_point_position, + bool update_kinematics = true); + +/** \brief Returns the orientation of a given body as 3x3 matrix + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param update_kinematics whether UpdateKinematics() should be called or not + * (default: true). + * + * \returns An orthonormal 3x3 matrix that rotates vectors from base coordinates + * to body coordinates. + */ +RBDL_DLLAPI Math::Matrix3d CalcBodyWorldOrientation ( + Model &model, + const Math::VectorNd &Q, + const unsigned int body_id, + bool update_kinematics = true); + +/** \brief Computes the point jacobian for a point on a body + * + * If a position of a point is computed by a function \f$g(q(t))\f$ for which its + * time derivative is \f$\frac{d}{dt} g(q(t)) = G(q)\dot{q}\f$ then this + * function computes the jacobian matrix \f$G(q)\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param G a matrix of dimensions 3 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + * + */ +RBDL_DLLAPI void CalcPointJacobian (Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &point_position, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes a 6-D Jacobian for a point on a body + * + * Computes the 6-D Jacobian \f$G(q)\f$ that when multiplied with + * \f$\dot{q}\f$ gives a 6-D vector that has the angular velocity as the + * first three entries and the linear velocity as the last three entries. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param G a matrix of dimensions 6 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + * + */ +RBDL_DLLAPI void CalcPointJacobian6D (Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &point_position, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes the spatial jacobian for a body + * + * The spatial velocity of a body at the origin of the base coordinate + * system can be expressed as \f${}^0 \hat{v}_i = G(q) * \dot{q}\f$. The + * matrix \f$G(q)\f$ is called the spatial body jacobian of the body and + * can be computed using this function. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param G a matrix of size 6 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument and represents the + * body Jacobian expressed at the origin of the body. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + */ +RBDL_DLLAPI void CalcBodySpatialJacobian ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes the velocity of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The cartesian velocity of the point in global frame (output) + */ +RBDL_DLLAPI Math::Vector3d CalcPointVelocity ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes angular and linear velocity of a point that is fixed on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The a 6-D vector for which the first three elements are the + * angular velocity and the last three elements the linear velocity in the + * global reference system. + */ +RBDL_DLLAPI + Math::SpatialVector CalcPointVelocity6D ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes the linear acceleration of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The cartesian acceleration of the point in global frame (output) + * + * The kinematic state of the model has to be updated before valid + * values can be obtained. This can either be done by calling + * UpdateKinematics() or setting the last parameter update_kinematics to + * true (default). + * + * \warning If this function is called after ForwardDynamics() without + * an update of the kinematic state one has to add the gravity + * acceleration has to be added to the result. + */ +RBDL_DLLAPI + Math::Vector3d CalcPointAcceleration ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes linear and angular acceleration of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns A 6-D vector where the first three elements are the angular + * acceleration and the last three elements the linear accelerations of + * the point. + * + * The kinematic state of the model has to be updated before valid + * values can be obtained. This can either be done by calling + * UpdateKinematics() or setting the last parameter update_kinematics to + * true (default). + * + * \warning If this function is called after ForwardDynamics() without + * an update of the kinematic state one has to add the gravity + * acceleration has to be added to the result. + */ +RBDL_DLLAPI + Math::SpatialVector CalcPointAcceleration6D ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) + * + * \param model rigid body model + * \param Qinit initial guess for the state + * \param body_id a vector of all bodies for which we we have kinematic target positions + * \param body_point a vector of points in body local coordinates that are + * to be matched to target positions + * \param target_pos a vector of target positions + * \param Qres output of the computed inverse kinematics + * \param step_tol tolerance used for convergence detection + * \param lambda damping factor for the least squares function + * \param max_iter maximum number of steps that should be performed + * \returns true on success, false otherwise + * + * This function repeatedly computes + * \f[ Qres = Qres + \Delta \theta\f] + * \f[ \Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e \f] + * where \f$G = G(q) = \frac{d}{dt} g(q(t))\f$ and \f$e\f$ is the + * correction of the body points so that they coincide with the target + * positions. The function returns true when \f$||\Delta \theta||_2 \le\f$ + * step_tol or if the error between body points and target gets smaller + * than step_tol. Otherwise it returns false. + * + * The parameter \f$\lambda\f$ is the damping factor that has to + * be chosen carefully. In case of unreachable positions higher values (e.g + * 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might + * yield good results. See the literature for best practices. + * + * \warning The actual accuracy might be rather low (~1.0e-2)! Use this function with a + * grain of suspicion. + */ +RBDL_DLLAPI + bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + const std::vector& body_id, + const std::vector& body_point, + const std::vector& target_pos, + Math::VectorNd &Qres, + double step_tol = 1.0e-12, + double lambda = 0.01, + unsigned int max_iter = 50 + ); + +RBDL_DLLAPI Math::Vector3d CalcCalcAngularVelocityfromMatrix ( + const Math::Matrix3d &RotMat); + +struct RBDL_DLLAPI InverseKinematicsConstraintSet { + enum ConstraintType { + ConstraintTypePosition = 0, + ConstraintTypeOrientation, + ConstraintTypeFull + }; + + InverseKinematicsConstraintSet(); + + Math::MatrixNd J; /// the Jacobian of all constraints + Math::MatrixNd G; /// temporary storage of a single body Jacobian + Math::VectorNd e; /// Vector with all the constraint residuals. + + unsigned int num_constraints; //size of all constraints + double lambda; /// Damping factor, the default value of 1.0e-6 is reasonable for most problems + unsigned int num_steps; // The number of iterations performed + unsigned int max_steps; // Maximum number of steps (default 300), abort if more steps are performed. + double step_tol; // Step tolerance (default = 1.0e-12). If the computed step length is smaller than this value the algorithm terminates successfully (i.e. returns true). If error_norm is still larger than constraint_tol then this usually means that the target is unreachable. + double constraint_tol; // Constraint tolerance (default = 1.0e-12). If error_norm is smaller than this value the algorithm terminates successfully, i.e. all constraints are satisfied. + double error_norm; // Norm of the constraint residual vector. + + // everything to define a IKin constraint + std::vector constraint_type; + std::vector body_ids; + std::vector body_points; + std::vector target_positions; + std::vector target_orientations; + std::vector constraint_row_index; + + // Adds a point constraint that tries to get a body point close to a + // point described in base coordinates. + unsigned int AddPointConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos); + // Adds an orientation constraint that tries to align a body to the + // orientation specified as a rotation matrix expressed in base + // coordinates. + unsigned int AddOrientationConstraint (unsigned int body_id, const Math::Matrix3d &target_orientation); + // Adds a constraint on both location and orientation of a body. + unsigned int AddFullConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, const Math::Matrix3d &target_orientation); + // Clears all entries of the constraint setting + unsigned int ClearConstraints(); +}; + +RBDL_DLLAPI bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + InverseKinematicsConstraintSet &CS, + Math::VectorNd &Qres + ); + +/** @} */ + +} + +/* RBDL_KINEMATICS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Logging.h b/3rdparty/rbdl/include/rbdl/Logging.h new file mode 100644 index 0000000..8f63b3c --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Logging.h @@ -0,0 +1,74 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_LOGGING_H +#define RBDL_LOGGING_H + +#include +#include + +class LoggingGuard; + +/** \def RBDL_ENABLE_LOGGING + * + * Enables/Disables logging + * + * \warning Logging has a huge impact on performance. + */ +#ifndef RBDL_ENABLE_LOGGING +#define LOG if (false) LogOutput +#define SUPPRESS_LOGGING ; +#else +#define LOG LogOutput +#define SUPPRESS_LOGGING LoggingGuard _nolog +#endif + +extern RBDL_DLLAPI std::ostringstream LogOutput; +RBDL_DLLAPI void ClearLogOutput (); + +/** \brief Helper object to ignore any logs that happen during its lifetime + * + * If an instance of this class exists all logging gets suppressed. This + * allows to disable logging for a certain scope or a single function call, + * e.g. + * + * \code + * { + * // logging will be active + * do_some_stuff(); + * + * // now create a new scope in which a LoggingGuard instance exists + * { + * LoggingGuard ignore_logging; + * + * // as a _Nologging instance exists, all logging will be discarded + * do_some_crazy_stuff(); + * } + * + * // logging will be active again + * do_some_more_stuff(); + * } + * \endcode + * + */ +class RBDL_DLLAPI LoggingGuard { + public: + LoggingGuard() { + log_backup.str(""); + log_backup << LogOutput.str(); + } + ~LoggingGuard() { + LogOutput.str(""); + LogOutput << log_backup.str(); + } + + private: + std::ostringstream log_backup; +}; + +/* RBDL_LOGGING_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Model.h b/3rdparty/rbdl/include/rbdl/Model.h new file mode 100644 index 0000000..7159531 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Model.h @@ -0,0 +1,519 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MODEL_H +#define RBDL_MODEL_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include +#include +#include +#include + +#include "rbdl/Logging.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" + +// std::vectors containing any objects that have Eigen matrices or vectors +// as members need to have a special allocater. This can be achieved with +// the following macro. + +#ifdef EIGEN_CORE_H +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body); +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody); +#endif + +/** \brief Namespace for all structures of the RigidBodyDynamics library +*/ +namespace RigidBodyDynamics { + +/** \page modeling_page Model + * + * \section model_structure Model Structure + * + * RBDL stores the model internally in the \link RigidBodyDynamics::Model + * Model Structure\endlink. For each \link RigidBodyDynamics::Body Body + * \endlink it contains spatial velocities, accelerations and other + * variables that describe the state of the rigid body system. Furthermore + * it contains variables that are used as temporary variables in the + * algorithms. + * + * There are multiple ways of creating \link RigidBodyDynamics::Model Models\endlink for RBDL: + * + * \li Loading models from Lua files using the \ref luamodel_introduction + * "LuaModel" addon + * \li Loading models from URDF (the Unified Robot Description Format) xml + * files or strings using the URDFReader addon + * \li using the C++ interface. + * + * The first approach requires the addon \ref luamodel_introduction to be + * activated which is done by enabling BUILD_ADDON_LUAMODEL in CMake and is + * recommended when one is not interested in the details of RBDL and simply + * wants to create a model. + * + * \section modeling_cpp Modeling using C++ + * + * The construction of \link RigidBodyDynamics::Model Model Structures + * \endlink makes use of carefully designed constructors of the classes + * \link RigidBodyDynamics::Body Body \endlink and \link + * RigidBodyDynamics::Joint Joint \endlink to ease the process of + * creating articulated models. + * + * \link RigidBodyDynamics::Body Bodies \endlink are created by calling one + * of its constructors. Usually they are created by specifying the mass, + * center of mass and the inertia at the center of mass. + * \link RigidBodyDynamics::Joint Joints \endlink are similarly created and is + * described in detail in \ref joint_description. + * + * Adding bodies to the model is done by specifying the + * parent body by its id, the transformation from the parent origin to the + * joint origin, the joint specification as an object, and the body itself. + * These parameters are then fed to the function + * RigidBodyDynamics::Model::AddBody() or + * RigidBodyDynamics::Model::AppendBody(). + * + * To create a model with a floating base (a.k.a a model with a free-flyer + * joint) it is recommended to use \link + * RigidBodyDynamics::Model::SetFloatingBaseBody + * Model::SetFloatingBaseBody(...)\endlink. + * + * Once this is done, the model structure can be used with the functions of \ref + * kinematics_group, \ref dynamics_group, \ref contacts_page, to perform + * computations. + * + * A simple example can be found \ref SimpleExample "here". + * + * \section modeling_lua Using LuaModels + * + * For this see the documentation of \ref luamodel_introduction,\link + * RigidBodyDynamics::Addons::LuaModelReadFromFile \endlink, and \link + * RigidBodyDynamics::Addons::LuaModelReadFromFileWithConstraints \endlink. + + * \section modeling_urdf Using URDF + * + * For this see the documentation see \link + * RigidBodyDynamics::Addons::URDFReadFromFile \endlink. + */ + +/** \brief Contains all information about the rigid body model + * + * This class contains all information required to perform the forward + * dynamics calculation. The variables in this class are also used for + * storage of temporary values. It is designed for use of the Articulated + * Rigid Body Algorithm (which is implemented in ForwardDynamics()) and + * follows the numbering as described in Featherstones book. + * + * Please note that body 0 is the root body and the moving bodies start at + * index 1. This numbering scheme is very beneficial in terms of + * readability of the code as the resulting code is very similar to the + * pseudo-code in the RBDA book. The generalized variables q, qdot, qddot + * and tau however start at 0 such that the first entry (e.g. q[0]) always + * specifies the value for the first moving body. + * + * \note To query the number of degrees of freedom use Model::dof_count. + */ +struct RBDL_DLLAPI Model { + Model(); + + // Structural information + + /// \brief The id of the parents body + std::vector lambda; + /** \brief The index of the parent degree of freedom that is directly + influencing the current one*/ + std::vector lambda_q; + /// \brief Contains the ids of all the children of a given body + std::vector >mu; + + /** \brief number of degrees of freedoms of the model + * + * This value contains the number of entries in the generalized state (q) + * velocity (qdot), acceleration (qddot), and force (tau) vector. + */ + unsigned int dof_count; + + /** \brief The size of the \f$\mathbf{q}\f$-vector. + * For models without spherical joints the value is the same as + * Model::dof_count, otherwise additional values for the w-component of the + * Quaternion is stored at the end of \f$\mathbf{q}\f$. + * + * \sa \ref joint_description for more details. + */ + unsigned int q_size; + /** \brief The size of the + * + * (\f$\mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, + * and \f$\mathbf{\tau}\f$-vector. + * + * \sa \ref joint_description for more details. + */ + unsigned int qdot_size; + + /// \brief Id of the previously added body, required for Model::AppendBody() + unsigned int previously_added_body_id; + + /// \brief the cartesian vector of the gravity + Math::Vector3d gravity; + + // State information + /// \brief The spatial velocity of the bodies + std::vector v; + /// \brief The spatial acceleration of the bodies + std::vector a; + + //////////////////////////////////// + // Joints + + /// \brief All joints + + std::vector mJoints; + /// \brief The joint axis for joint i + std::vector S; + + // Joint state variables + std::vector X_J; + std::vector v_J; + std::vector c_J; + + std::vector mJointUpdateOrder; + + /// \brief Transformations from the parent body to the frame of the joint. + // It is expressed in the coordinate frame of the parent. + std::vector X_T; + /// \brief The number of fixed joints that have been declared before + /// each joint. + std::vector mFixedJointCount; + + //////////////////////////////////// + // Special variables for joints with 3 degrees of freedom + /// \brief Motion subspace for joints with 3 degrees of freedom + std::vector multdof3_S; + std::vector multdof3_U; + std::vector multdof3_Dinv; + std::vector multdof3_u; + std::vector multdof3_w_index; + + std::vector mCustomJoints; + + //////////////////////////////////// + // Dynamics variables + + /// \brief The velocity dependent spatial acceleration + std::vector c; + /// \brief The spatial inertia of the bodies + std::vector IA; + /// \brief The spatial bias force + std::vector pA; + /// \brief Temporary variable U_i (RBDA p. 130) + std::vector U; + /// \brief Temporary variable D_i (RBDA p. 130) + Math::VectorNd d; + /// \brief Temporary variable u (RBDA p. 130) + Math::VectorNd u; + /// \brief Internal forces on the body (used only InverseDynamics()) + std::vector f; + /// \brief The spatial inertia of body i (used only in + /// CompositeRigidBodyAlgorithm()) + std::vector I; + std::vector Ic; + std::vector hc; + + //////////////////////////////////// + // Bodies + + /** \brief Transformation from the parent body to the current body + * \f[ + * X_{\lambda(i)} = {}^{i} X_{\lambda(i)} + * \f] + */ + std::vector X_lambda; + /// \brief Transformation from the base to bodies reference frame + std::vector X_base; + + /// \brief All bodies that are attached to a body via a fixed joint. + std::vector mFixedBodies; + /** \brief Value that is used to discriminate between fixed and movable + * bodies. + * + * Bodies with id 1 .. (fixed_body_discriminator - 1) are moving bodies + * while bodies with id fixed_body_discriminator .. max (unsigned int) + * are fixed to a moving body. The value of max(unsigned int) is + * determined via std::numeric_limits::max() and the + * default value of fixed_body_discriminator is max (unsigned int) / 2. + * + * On normal systems max (unsigned int) is 4294967294 which means there + * could be a total of 2147483646 movable and / or fixed bodies. + */ + unsigned int fixed_body_discriminator; + + /** \brief All bodies 0 ... N_B, including the base + * + * mBodies[0] - base body
+ * mBodies[1] - 1st moveable body
+ * ...
+ * mBodies[N_B] - N_Bth moveable body
+ */ + std::vector mBodies; + + /// \brief Human readable names for the bodies + std::map mBodyNameMap; + + /** \brief Connects a given body to the model + * + * When adding a body there are basically informations required: + * - what kind of body will be added? + * - where is the new body to be added? + * - by what kind of joint should the body be added? + * + * The first information "what kind of body will be added" is contained + * in the Body class that is given as a parameter. + * + * The question "where is the new body to be added?" is split up in two + * parts: first the parent (or successor) body to which it is added and + * second the transformation to the origin of the joint that connects the + * two bodies. With these two informations one specifies the relative + * positions of the bodies when the joint is in neutral position.gk + * + * The last question "by what kind of joint should the body be added?" is + * again simply contained in the Joint class. + * + * \param parent_id id of the parent body + * \param joint_frame the transformation from the parent frame to the origin + * of the joint frame (represents X_T in RBDA) + * \param joint specification for the joint that describes the + * connection + * \param body specification of the body itself + * \param body_name human readable name for the body (can be used to + * retrieve its id with GetBodyId()) + * + * \returns id of the added body + */ + unsigned int AddBody ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + unsigned int AddBodySphericalJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + /** \brief Adds a Body to the model such that the previously added Body + * is the Parent. + * + * This function is basically the same as Model::AddBody() however the + * most recently added body (or body 0) is taken as parent. + */ + unsigned int AppendBody ( + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + unsigned int AddBodyCustomJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + CustomJoint *custom_joint, + const Body &body, + std::string body_name = "" + ); + + /** \brief Specifies the dynamical parameters of the first body and + * \brief assigns it a 6 DoF joint. + * + * The 6 DoF joint is simulated by adding 5 massless bodies at the base + * which are connected with joints. The body that is specified as a + * parameter of this function is then added by a 6th joint to the model. + * + * The floating base has the following order of degrees of freedom: + * + * \li translation X + * \li translation Y + * \li translation Z + * \li rotation Z + * \li rotation Y + * \li rotation X + * + * To specify a different ordering, it is recommended to create a 6 DoF + * joint. See \link RigidBodyDynamics::Joint Joint\endlink for more + * information. + * + * \param body Properties of the floating base body. + * + * \returns id of the body with 6 DoF + */ + unsigned int SetFloatingBaseBody (const Body &body); + + /** \brief Returns the id of a body that was passed to AddBody() + * + * Bodies can be given a human readable name. This function allows to + * resolve its name to the numeric id. + * + * \note Instead of querying this function repeatedly, it might be + * advisable to query it once and reuse the returned id. + * + * \returns the id of the body or \c std::numeric_limits\::max() if the id was not found. + */ + unsigned int GetBodyId (const char *body_name) const { + if (mBodyNameMap.count(body_name) == 0) { + return std::numeric_limits::max(); + } + + return mBodyNameMap.find(body_name)->second; + } + + /** \brief Returns the name of a body for a given body id */ + std::string GetBodyName (unsigned int body_id) const { + std::map::const_iterator iter + = mBodyNameMap.begin(); + + while (iter != mBodyNameMap.end()) { + if (iter->second == body_id) + return iter->first; + + iter++; + } + + return ""; + } + + /** \brief Checks whether the body is rigidly attached to another body. + */ + bool IsFixedBodyId (unsigned int body_id) { + if (body_id >= fixed_body_discriminator + && body_id < std::numeric_limits::max() + && body_id - fixed_body_discriminator < mFixedBodies.size()) { + return true; + } + return false; + } + + bool IsBodyId (unsigned int id) { + if (id > 0 && id < mBodies.size()) + return true; + if (id >= fixed_body_discriminator + && id < std::numeric_limits::max()) { + if (id - fixed_body_discriminator < mFixedBodies.size()) + return true; + } + return false; + } + + /** Determines id the actual parent body. + * + * When adding bodies using joints with multiple degrees of + * freedom, additional virtual bodies are added for each degree of + * freedom. This function returns the id of the actual + * non-virtual parent body. + */ + unsigned int GetParentBodyId (unsigned int id) { + if (id >= fixed_body_discriminator) { + return mFixedBodies[id - fixed_body_discriminator].mMovableParent; + } + + unsigned int parent_id = lambda[id]; + + while (mBodies[parent_id].mIsVirtual) { + parent_id = lambda[parent_id]; + } + + return parent_id; + } + + /** Returns the joint frame transformtion, i.e. the second argument to + Model::AddBody(). + */ + Math::SpatialTransform GetJointFrame (unsigned int id) { + if (id >= fixed_body_discriminator) { + return mFixedBodies[id - fixed_body_discriminator].mParentTransform; + } + + unsigned int child_id = id; + unsigned int parent_id = lambda[id]; + if (mBodies[parent_id].mIsVirtual) { + while (mBodies[parent_id].mIsVirtual) { + child_id = parent_id; + parent_id = lambda[child_id]; + } + return X_T[child_id]; + } else + return X_T[id]; + } + + /** Sets the joint frame transformtion, i.e. the second argument to + Model::AddBody(). + */ + void SetJointFrame (unsigned int id, + const Math::SpatialTransform &transform) { + if (id >= fixed_body_discriminator) { + std::cerr << "Error: setting of parent transform " + << "not supported for fixed bodies!" << std::endl; + abort(); + } + + unsigned int child_id = id; + unsigned int parent_id = lambda[id]; + if (mBodies[parent_id].mIsVirtual) { + while (mBodies[parent_id].mIsVirtual) { + child_id = parent_id; + parent_id = lambda[child_id]; + } + X_T[child_id] = transform; + } else if (id > 0) { + X_T[id] = transform; + } + } + + /** Gets the quaternion for body i (only valid if body i is connected by + * a JointTypeSpherical joint) + * + * See \ref joint_singularities for details. + */ + Math::Quaternion GetQuaternion (unsigned int i, + const Math::VectorNd &Q) const { + assert (mJoints[i].mJointType == JointTypeSpherical); + unsigned int q_index = mJoints[i].q_index; + return Math::Quaternion ( Q[q_index], + Q[q_index + 1], + Q[q_index + 2], + Q[multdof3_w_index[i]]); + } + + /** Sets the quaternion for body i (only valid if body i is connected by + * a JointTypeSpherical joint) + * + * See \ref joint_singularities for details. + */ + void SetQuaternion (unsigned int i, + const Math::Quaternion &quat, + Math::VectorNd &Q) const { + assert (mJoints[i].mJointType == JointTypeSpherical); + unsigned int q_index = mJoints[i].q_index; + + Q[q_index] = quat[0]; + Q[q_index + 1] = quat[1]; + Q[q_index + 2] = quat[2]; + Q[multdof3_w_index[i]] = quat[3]; + } +}; + +/** @} */ +} + +/* _MODEL_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Quaternion.h b/3rdparty/rbdl/include/rbdl/Quaternion.h new file mode 100644 index 0000000..fc71aa7 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Quaternion.h @@ -0,0 +1,211 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_QUATERNION_H +#define RBDL_QUATERNION_H + +#include + +namespace RigidBodyDynamics { + +namespace Math { + +/** \brief Quaternion that are used for \ref joint_singularities "singularity free" joints. + * + * order: x,y,z,w + */ +class Quaternion : public Vector4d { + public: + Quaternion () : + Vector4d (0., 0., 0., 1.) + {} + Quaternion (const Vector4d &vec4) : + Vector4d (vec4) + {} + Quaternion (double x, double y, double z, double w): + Vector4d (x, y, z, w) + {} + Quaternion operator* (const double &s) const { + return Quaternion ( + (*this)[0] * s, + (*this)[1] * s, + (*this)[2] * s, + (*this)[3] * s + ); + } + /** This function is equivalent to multiplicate their corresponding rotation matrices */ + Quaternion operator* (const Quaternion &q) const { + return Quaternion ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + } + Quaternion& operator*=(const Quaternion &q) { + set ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + return *this; + } + + static Quaternion fromGLRotate (double angle, double x, double y, double z) { + double st = std::sin (angle * M_PI / 360.); + return Quaternion ( + st * x, + st * y, + st * z, + std::cos (angle * M_PI / 360.) + ); + } + + Quaternion slerp (double alpha, const Quaternion &quat) const { + // check whether one of the two has 0 length + double s = std::sqrt (squaredNorm() * quat.squaredNorm()); + + // division by 0.f is unhealthy! + assert (s != 0.); + + double angle = acos (dot(quat) / s); + if (angle == 0. || std::isnan(angle)) { + return *this; + } + assert(!std::isnan(angle)); + + double d = 1. / std::sin (angle); + double p0 = std::sin ((1. - alpha) * angle); + double p1 = std::sin (alpha * angle); + + if (dot (quat) < 0.) { + return Quaternion( ((*this) * p0 - quat * p1) * d); + } + return Quaternion( ((*this) * p0 + quat * p1) * d); + } + + static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) { + double d = axis.norm(); + double s2 = std::sin (angle_rad * 0.5) / d; + return Quaternion ( + axis[0] * s2, + axis[1] * s2, + axis[2] * s2, + std::cos(angle_rad * 0.5) + ); + } + + static Quaternion fromMatrix (const Matrix3d &mat) { + double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; + return Quaternion ( + (mat(1,2) - mat(2,1)) / (w * 4.), + (mat(2,0) - mat(0,2)) / (w * 4.), + (mat(0,1) - mat(1,0)) / (w * 4.), + w); + } + + static Quaternion fromZYXAngles (const Vector3d &zyx_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]); + } + + static Quaternion fromYXZAngles (const Vector3d &yxz_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), yxz_angles[0]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), yxz_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), yxz_angles[2]); + } + + static Quaternion fromXYZAngles (const Vector3d &xyz_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), xyz_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), xyz_angles[0]); + } + + Matrix3d toMatrix() const { + double x = (*this)[0]; + double y = (*this)[1]; + double z = (*this)[2]; + double w = (*this)[3]; + return Matrix3d ( + 1 - 2*y*y - 2*z*z, + 2*x*y + 2*w*z, + 2*x*z - 2*w*y, + + 2*x*y - 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z + 2*w*x, + + 2*x*z + 2*w*y, + 2*y*z - 2*w*x, + 1 - 2*x*x - 2*y*y + + /* + 1 - 2*y*y - 2*z*z, + 2*x*y - 2*w*z, + 2*x*z + 2*w*y, + + 2*x*y + 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z - 2*w*x, + + 2*x*z - 2*w*y, + 2*y*z + 2*w*x, + 1 - 2*x*x - 2*y*y + */ + ); + } + + Quaternion conjugate() const { + return Quaternion ( + -(*this)[0], + -(*this)[1], + -(*this)[2], + (*this)[3]); + } + + Quaternion timeStep (const Vector3d &omega, double dt) { + double omega_norm = omega.norm(); + return Quaternion::fromAxisAngle (omega / omega_norm, dt * omega_norm) * (*this); + } + + Vector3d rotate (const Vector3d &vec) const { + Vector3d vn (vec); + Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; + + res_quat = vec_quat * (*this); + res_quat = conjugate() * res_quat; + + return Vector3d (res_quat[0], res_quat[1], res_quat[2]); + } + + /** \brief Converts a 3d angular velocity vector into a 4d derivative of the + * components of the quaternion. + * + * \param omega the angular velocity. + * + * \return a 4d vector containing the derivatives of the 4 components of the + * quaternion corresponding to omega. + * + */ + Vector4d omegaToQDot(const Vector3d& omega) const { + Math::Matrix43 m; + m(0, 0) = (*this)[3]; m(0, 1) = -(*this)[2]; m(0, 2) = (*this)[1]; + m(1, 0) = (*this)[2]; m(1, 1) = (*this)[3]; m(1, 2) = -(*this)[0]; + m(2, 0) = -(*this)[1]; m(2, 1) = (*this)[0]; m(2, 2) = (*this)[3]; + m(3, 0) = -(*this)[0]; m(3, 1) = -(*this)[1]; m(3, 2) = -(*this)[2]; + return Quaternion(0.5 * m * omega); + } +}; + +} + +} + +/* RBDL_QUATERNION_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/.hg_archival.txt b/3rdparty/rbdl/include/rbdl/SimpleMath/.hg_archival.txt new file mode 100644 index 0000000..d304728 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/.hg_archival.txt @@ -0,0 +1,5 @@ +repo: 1e43843734d109ad9faf98c54f37d419ae680546 +node: 5822281865de4bcd58e4a6582ea7940391314492 +branch: default +latesttag: null +latesttagdistance: 5 diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/.hgignore b/3rdparty/rbdl/include/rbdl/SimpleMath/.hgignore new file mode 100644 index 0000000..8dfdff4 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/.hgignore @@ -0,0 +1,15 @@ +syntax: glob + +doc/html/* +doc/notes/*.aux +doc/notes/*.pdf +Debug/* +Release/* +.*.swp +CMakeFiles/* +cmake_install.cmake +Makefile +tags +*.log +tests/runtests +tests/CMakeCache.txt diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/README b/3rdparty/rbdl/include/rbdl/SimpleMath/README new file mode 100644 index 0000000..e49e343 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/README @@ -0,0 +1,10 @@ +This is a highly inefficient math library. It was conceived by Martin +Felis while he was waiting for code to +compile which used a highly efficient math library. + +It is intended to be used as a fast compiling substitute for the +blazingly fast Eigen3 http://eigen.tuxfamily.org/index.php?title=Main_Page +library and tries to mimic its API to a certain extent. + +Feel free to use it wherever you like. However, no guarantees are given +that this code does what it says it would. diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h new file mode 100644 index 0000000..568dc66 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h @@ -0,0 +1,28 @@ +#ifndef _SIMPLEMATH_H +#define _SIMPLEMATH_H + +#include "SimpleMathFixed.h" +#include "SimpleMathDynamic.h" +#include "SimpleMathMixed.h" +#include "SimpleMathQR.h" +#include "SimpleMathCommaInitializer.h" + +namespace SimpleMath { + +typedef SimpleMath::Fixed::Matrix Vector3i; + +typedef SimpleMath::Fixed::Matrix Vector3d; +typedef SimpleMath::Fixed::Matrix Matrix33d; + +typedef SimpleMath::Fixed::Matrix Vector4d; + +typedef SimpleMath::Fixed::Matrix Vector3f; +typedef SimpleMath::Fixed::Matrix Vector4f; +typedef SimpleMath::Fixed::Matrix Matrix33f; +typedef SimpleMath::Fixed::Matrix Matrix44f; + +typedef SimpleMath::Dynamic::Matrix VectorNd; + +} + +#endif /* _SIMPLEMATH_H */ diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathBlock.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathBlock.h new file mode 100644 index 0000000..cb2e472 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathBlock.h @@ -0,0 +1,194 @@ +/** + * This is a highly inefficient math library. It was conceived by Martin + * Felis while he was compiling code + * that uses a highly efficient math library. + * + * It is intended to be used as a fast compiling substitute for the + * blazingly fast Eigen3 library and tries to mimic its API to a certain + * extend. + * + * Feel free to use it wherever you like. However, no guarantees are given + * that this code does what it says it would. + */ + +#ifndef SIMPLEMATHBLOCK_H +#define SIMPLEMATHBLOCK_H + +#include +#include +#include +#include + +#include "compileassert.h" + +// #include "SimpleMathQR.h" + +/** \brief Namespace for a highly inefficient math library + * + */ +namespace SimpleMath { + +/** \brief Namespace for fixed size elements + */ +// forward declaration +template +class Matrix; + +template +class Block { + public: + typedef val_type value_type; + + Block() : + mParentRows(0), + mParentCols(0), + mParentRowStart(0), + mParentColStart(0) + { } + Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) : + mParentRows (matrix.rows()), + mParentCols (matrix.cols()), + mParentRowStart (row_start), + mParentColStart (col_start), + mRowCount (row_count), + mColCount (col_count), + mTransposed (false) { + assert (mParentRows >= mParentRowStart + mRowCount); + assert (mParentCols >= mParentColStart + mColCount); + + // without the following line we could not create blocks from const + // matrices + mParentMatrix = const_cast(&matrix); + } + + // copy data from the other block into this + Block& operator=(const Block &other) { + if (this != &other) { + if (mRowCount != other.rows() || mColCount != other.cols()) { + std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; + abort(); + } + + value_type* temp_values = new value_type [mRowCount * mColCount]; + + for (unsigned int i = 0; i < mRowCount; i++) { + for (unsigned int j = 0; j < mColCount; j++) { + temp_values[i * mColCount + j] = static_cast(other(i,j)); + } + } + + for (unsigned int i = 0; i < mRowCount; i++) { + for (unsigned int j = 0; j < mColCount; j++) { + (*this)(i,j) = temp_values[i * mColCount + j]; + } + } + + delete[] temp_values; + } + + return *this; + } + + template + // copy data from the other block into this + Block& operator=(const other_matrix_type &other) { + if (mRowCount != other.rows() || mColCount != other.cols()) { + std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl; + abort(); + } + + value_type *temp_values = new value_type[mRowCount * mColCount]; + + for (unsigned int i = 0; i < mRowCount; i++) { + for (unsigned int j = 0; j < mColCount; j++) { + temp_values[i * mColCount + j] = static_cast(other(i,j)); + } + } + + for (unsigned int i = 0; i < mRowCount; i++) { + for (unsigned int j = 0; j < mColCount; j++) { + (*this)(i,j) = temp_values[i * mColCount + j]; + } + } + + delete[] temp_values; + + return *this; + } + + unsigned int rows() const { + if (!mTransposed) + return mRowCount; + + return mColCount; + } + unsigned int cols() const { + if (!mTransposed) + return mColCount; + + return mRowCount; + } + const val_type& operator() (const unsigned int i, const unsigned int j) const { + + if (!mTransposed) { + assert (i < mRowCount); + assert (j < mColCount); + return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); + } + + return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); + } + + val_type& operator() (const unsigned int i, const unsigned int j) { + if (!mTransposed) { + assert (i < mRowCount); + assert (j < mColCount); + return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart); + } + + assert (j < mRowCount); + assert (i < mColCount); + return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart); + } + + Block transpose() const { + Block result (*this); + result.mTransposed = mTransposed ^ true; + return result; + } + + private: + matrix_type *mParentMatrix; + const unsigned int mParentRows; + const unsigned int mParentCols; + const unsigned int mParentRowStart; + const unsigned int mParentColStart; + const unsigned int mRowCount; + const unsigned int mColCount; + bool mTransposed; +}; + +template +inline std::ostream& operator<<(std::ostream& output, const Block &block) { + unsigned int i,j; + for (i = 0; i < block.rows(); i++) { + output << "[ "; + for (j = 0; j < block.cols(); j++) { + output << block(i,j); + + if (j < block.cols() - 1) + output << ", "; + } + output << " ]"; + + if (block.rows() > 1 && i < block.rows() - 1) + output << std::endl; + } + + return output; +} + + +} + +#endif /* SIMPLEMATHBLOCK_H */ diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCholesky.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCholesky.h new file mode 100644 index 0000000..b51a361 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCholesky.h @@ -0,0 +1,94 @@ +#ifndef _SIMPLE_MATH_CHOLESKY_H +#define _SIMPLE_MATH_CHOLESKY_H + +#include +#include + +#include "SimpleMathDynamic.h" + +namespace SimpleMath { + + template + class LLT { + public: + typedef typename matrix_type::value_type value_type; + + private: + LLT () {} + + typedef Dynamic::Matrix MatrixXXd; + typedef Dynamic::Matrix VectorXd; + + bool mIsFactorized; + MatrixXXd mL; + + public: + LLT (const matrix_type &matrix) : + mIsFactorized(false), + mL(matrix) { + compute(); + } + LLT compute() { + for (int i = 0; i < mL.rows(); i++) { + for (int j = 0; j < mL.rows(); j++) { + if (j > i) { + mL(i,j) = 0.; + continue; + } + double s = mL(i,j); + for (int k = 0; k < j; k++) { + s = s - mL(i,k) * mL(j,k); + } + if (i > j) { + mL(i,j) = s / mL(j,j); + } else if (s > 0.) { + mL (i,i) = sqrt (s); + } else { + std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl; + assert (false); + } + } + } + + mIsFactorized = true; + + return *this; + } + Dynamic::Matrix solve ( + const Dynamic::Matrix &rhs + ) const { + assert (mIsFactorized); + + VectorXd y (mL.rows()); + for (unsigned int i = 0; i < mL.rows(); i++) { + double temp = rhs[i]; + + for (unsigned int j = 0; j < i; j++) { + temp = temp - mL(i,j) * y[j]; + } + + y[i] = temp / mL(i,i); + } + + VectorXd x (mL.rows()); + for (int i = mL.rows() - 1; i >= 0; i--) { + double temp = y[i]; + + for (unsigned int j = i + 1; j < mL.rows(); j++) { + temp = temp - mL(j, i) * x[j]; + } + + x[i] = temp / mL(i,i); + } + + return x; + } + Dynamic::Matrix matrixL() const { + return mL; + } + }; + +} + +/* _SIMPLE_MATH_CHOLESKY_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCommaInitializer.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCommaInitializer.h new file mode 100644 index 0000000..85657ba --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathCommaInitializer.h @@ -0,0 +1,69 @@ +#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H +#define _SIMPLE_MATH_COMMA_INITIALIZER_H + +#include +#include + +#include "SimpleMathFixed.h" +#include "SimpleMathDynamic.h" + +namespace SimpleMath { + + template + class CommaInitializer { + public: + typedef typename matrix_type::value_type value_type; + + CommaInitializer(matrix_type &matrix, const value_type &value) : + mElementWasAdded (false) { + assert (matrix.cols() > 0 && matrix.rows() > 0); + mParentMatrix = &matrix; + mRowIndex = 0; + mColIndex = 0; + (*mParentMatrix)(mRowIndex, mColIndex) = value; + } + CommaInitializer(matrix_type &matrix, unsigned int row_index, unsigned int col_index) : + mRowIndex (row_index), + mColIndex (col_index), + mElementWasAdded (false) { + assert (matrix.cols() > 0 && matrix.rows() > 0); + mParentMatrix = &matrix; + mRowIndex = row_index; + mColIndex = col_index; + } + ~CommaInitializer() { + if (!mElementWasAdded + && (mColIndex + 1 < mParentMatrix->cols() || mRowIndex + 1 < mParentMatrix->rows())) { + std::cerr << "Error: too few elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + } + CommaInitializer operator, (const value_type &value) { + mColIndex++; + if (mColIndex >= mParentMatrix->cols()) { + mRowIndex++; + mColIndex = 0; + } + if (mRowIndex == mParentMatrix->rows() && mColIndex == 0 ) { + std::cerr << "Error: too many elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + (*mParentMatrix)(mRowIndex, mColIndex) = value; + mElementWasAdded = true; + + return CommaInitializer (*mParentMatrix, mRowIndex, mColIndex); + } + + private: + CommaInitializer() {} + + matrix_type *mParentMatrix; + unsigned int mRowIndex; + unsigned int mColIndex; + bool mElementWasAdded; + }; + +} + +/* _SIMPLE_MATH_COMMA_INITIALIZER_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathDynamic.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathDynamic.h new file mode 100644 index 0000000..62975e8 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathDynamic.h @@ -0,0 +1,667 @@ +/** + * This is a highly inefficient math library. It was conceived by Martin + * Felis while he was compiling code + * that uses a highly efficient math library. + * + * It is intended to be used as a fast compiling substitute for the + * blazingly fast Eigen3 library and tries to mimic its API to a certain + * extend. + * + * Feel free to use it wherever you like. However, no guarantees are given + * that this code does what it says it would. + */ + +#ifndef SIMPLEMATHDYNAMIC_H +#define SIMPLEMATHDYNAMIC_H + +#include +#include +#include +#include + +#include "compileassert.h" +#include "SimpleMathBlock.h" + +/** \brief Namespace for a highly inefficient math library + * + */ +namespace SimpleMath { + +template +class LLT; + +template +class HouseholderQR; + +template +class ColPivHouseholderQR; + +template +class CommaInitializer; + +namespace Fixed { + template class Matrix; +} + + +/** \brief Namespace for elements of varying size. + */ +namespace Dynamic { + +// forward declaration +template +class Matrix; + +/** \brief Class for both matrices and vectors. + */ +template +class Matrix { + public: + typedef Matrix matrix_type; + typedef val_type value_type; + + Matrix() : + nrows (0), + ncols (0), + mapped_data (false), + mData (NULL) {}; + Matrix(unsigned int rows) : + nrows (rows), + ncols (1), + mapped_data (false) { + mData = new val_type[rows]; + } + Matrix(unsigned int rows, unsigned int cols) : + nrows (rows), + ncols (cols), + mapped_data (false) { + mData = new val_type[rows * cols]; + } + Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) : + nrows (rows), + ncols (cols), + mapped_data (true) { + mData = data_ptr; + } + + unsigned int rows() const { + return nrows; + } + + unsigned int cols() const { + return ncols; + } + + unsigned int size() const { + return nrows * ncols; + } + void resize (unsigned int rows, unsigned int cols=1) { + if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { + delete[] mData; + } + + nrows = rows; + ncols = cols; + + mData = new val_type[nrows * ncols]; + } + + void conservativeResize (unsigned int rows, unsigned int cols = 1) { + Matrix result = Matrix::Zero(rows, cols); + + unsigned int arows = std::min (rows, nrows); + unsigned int acols = std::min (cols, ncols); + + for (unsigned int i = 0; i < arows; i++) { + for (unsigned int j = 0; j < acols; j++) { + result(i,j) = (*this)(i,j); + } + } + + *this = result; + } + + Matrix(const Matrix &matrix) : + nrows (matrix.nrows), + ncols (matrix.ncols), + mapped_data (false) { + unsigned int i; + + mData = new val_type[nrows * ncols]; + + for (i = 0; i < nrows * ncols; i++) + mData[i] = matrix.mData[i]; + } + Matrix& operator=(const Matrix &matrix) { + if (this != &matrix) { + if (!mapped_data) { + delete[] mData; + + nrows = matrix.nrows; + ncols = matrix.ncols; + mapped_data = false; + + mData = new val_type[nrows * ncols]; + + unsigned int i; + for (i = 0; i < nrows * ncols; i++) + mData[i] = matrix.mData[i]; + } else { + // we overwrite any existing data + nrows = matrix.nrows; + ncols = matrix.ncols; + mapped_data = true; + + unsigned int i; + for (i = 0; i < nrows * ncols; i++) + mData[i] = matrix.mData[i]; + } + } + return *this; + } + + CommaInitializer operator<< (const val_type& value) { + return CommaInitializer (*this, value); + } + + // conversion different val_types + template + Matrix (const Matrix &matrix) : + nrows (matrix.rows()), + ncols (matrix.cols()), + mapped_data(false) { + + mData = new val_type[nrows * ncols]; + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(matrix(i,j)); + } + } + } + + // conversion from a fixed size matrix + template + Matrix (const Fixed::Matrix &fixed_matrix) : + nrows (fnrows), + ncols (fncols), + mapped_data (false), + mData (NULL) { + mData = new val_type[nrows * ncols]; + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(fixed_matrix(i,j)); + } + } + } + + template + Matrix (const Block &block) : + nrows(block.rows()), + ncols(block.cols()), + mapped_data (false) { + mData = new val_type[nrows * ncols]; + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(block(i,j)); + } + } + + } + + ~Matrix() { + if (nrows * ncols > 0 && mData != NULL && mapped_data == false) { + delete[] mData; + mData = NULL; + } + + nrows = 0; + ncols = 0; + }; + + // comparison + bool operator==(const Matrix &matrix) const { + if (nrows != matrix.nrows || ncols != matrix.ncols) + return false; + + for (unsigned int i = 0; i < nrows * ncols; i++) { + if (mData[i] != matrix.mData[i]) + return false; + } + return true; + } + bool operator!=(const Matrix &matrix) const { + if (nrows != matrix.nrows || ncols != matrix.ncols) + return true; + + for (unsigned int i = 0; i < nrows * ncols; i++) { + if (mData[i] != matrix.mData[i]) + return true; + } + return false; + } + + // access operators + const val_type& operator[](const unsigned int &index) const { + assert (index >= 0); + assert (index < nrows * ncols); + return mData[index]; + }; + val_type& operator[](const unsigned int &index) { + assert (index >= 0 && index < nrows * ncols); + return mData[index]; + } + + const val_type& operator()(const unsigned int &row, const unsigned int &col) const { + if (!(row >= 0 && row < nrows && col >= 0 && col < ncols)) { + std::cout << "row = " << row << " col = " << col << std::endl; + std::cout << "nrows = " << nrows << " ncols = " << ncols << std::endl; + std::cout << "invalid read = " << mData[100000] << std::endl; + } + assert (row >= 0 && row < nrows && col >= 0 && col < ncols); + return mData[row*ncols + col]; + }; + val_type& operator()(const unsigned int &row, const unsigned int &col) { + assert (row >= 0 && row < nrows && col >= 0 && col < ncols); + return mData[row*ncols + col]; + }; + + void zero() { + for (unsigned int i = 0; i < ncols * nrows; i++) + mData[i] = 0.; + } + void setZero() { + zero(); + } + + val_type norm() const { + return sqrt(this->squaredNorm()); + } + + void normalize() { + val_type length = this->norm(); + + for (unsigned int i = 0; i < ncols * nrows; i++) + mData[i] /= length; + } + + Matrix normalized() const { + return Matrix (*this) / this->norm(); + } + + Matrix cross(const Matrix &other_vector) { + assert (nrows * ncols == 3); + assert (other_vector.nrows * other_vector.ncols == 3); + + Matrix result (3, 1); + result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; + result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; + result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; + + return result; + } + + val_type trace() const { + assert (rows() == cols()); + val_type result = 0.; + + for (unsigned int i = 0; i < rows(); i++) { + result += operator()(i,i); + } + + return result; + } + + val_type mean() const { + assert (rows() == 1 || cols() == 1); + + val_type result = 0.; + for (unsigned int i = 0; i < rows() * cols(); i++) { + result += operator[](i); + } + + return result / static_cast(rows() * cols()); + } + + static matrix_type Zero() { + matrix_type result; + result.setZero(); + return result; + } + + static matrix_type Zero(int rows, int cols = 1) { + matrix_type result (rows, cols); + result.setZero(); + return result; + } + + static matrix_type Constant (int rows, val_type value) { + matrix_type result (rows, 1); + unsigned int i; + for (i = 0; i < result.size(); i++) + result[i] = value; + + return result; + } + + static matrix_type Constant (int rows, int cols, val_type value) { + matrix_type result (rows, cols); + unsigned int i; + for (i = 0; i < result.size(); i++) + result[i] = value; + + return result; + } + + static matrix_type Identity (int rows, int cols = 1) { + assert (rows == cols); + + matrix_type result (rows, cols); + result.identity(); + + return result; + } + + void identity() { + assert (nrows == ncols); + + setZero(); + for (unsigned int i = 0; i < ncols; i++) + mData[i * ncols + i] = 1.; + } + + void random() { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] = static_cast (rand()) / static_cast (RAND_MAX); + } + + val_type squaredNorm() const { + assert (ncols == 1 || nrows == 1); + val_type result = 0; + + for (unsigned int i = 0; i < nrows * ncols; i++) + result += mData[i] * mData[i]; + + return result; + } + + val_type dot(const matrix_type &matrix) const { + assert (ncols == 1 || nrows == 1); + val_type result = 0; + + for (unsigned int i = 0; i < nrows * ncols; i++) + result += mData[i] * matrix[i]; + + return result; + } + + // Blocks + Block + block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { + return Block(*this, row_start, col_start, row_count, col_count); + } + + template + Block + block (unsigned int row_start, unsigned int col_start) { + return Block(*this, row_start, col_start, row_count, col_count); + } + + Block + block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { + return Block(*this, row_start, col_start, row_count, col_count); + } + + template + Block + block (unsigned int row_start, unsigned int col_start) const { + return Block(*this, row_start, col_start, row_count, col_count); + } + + // row and col accessors + Block + col(unsigned int index) const { + return Block(*this, 0, index, rows(), 1); + } + + Block + col(unsigned int index) { + return Block(*this, 0, index, rows(), 1); + } + + Block + row(unsigned int index) const { + return Block(*this, index, 0, 1, cols()); + } + + Block + row(unsigned int index) { + return Block(*this, index, 0, 1, cols()); + } + + // Operators with scalars + void operator*=(const val_type &scalar) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] *= scalar; + }; + void operator/=(const val_type &scalar) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] /= scalar; + } + Matrix operator/(const val_type& scalar) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] /= scalar; + + return result; + } + + // Operators with other matrices + Matrix operator+(const Matrix &matrix) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] += matrix[i]; + + return result; + } + void operator+=(const matrix_type &matrix) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] += matrix.mData[i]; + } + Matrix operator-(const Matrix &matrix) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] -= matrix[i]; + + return result; + } + void operator-=(const Matrix &matrix) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] -= matrix.mData[i]; + } + + Matrix operator*(const Matrix &other_matrix) const { + assert (ncols == other_matrix.nrows); + + Matrix result(nrows, other_matrix.ncols); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_matrix.cols(); j++) { + for (k = 0; k < other_matrix.rows(); k++) { + result(i,j) += mData[i * ncols + k] * other_matrix(k,j); + } + } + } + + return result; + } + + template + Matrix operator*(const Fixed::Matrix &other_matrix) const { + assert (ncols == other_matrix.rows()); + + Matrix result(nrows, other_matrix.cols()); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_matrix.cols(); j++) { + for (k = 0; k < other_matrix.rows(); k++) { + result(i,j) += mData[i * ncols + k] * other_matrix(k,j); + } + } + } + + return result; + } + + Matrix operator*(const Block &other_matrix) const { + assert (ncols == other_matrix.rows()); + + Matrix result(nrows, other_matrix.cols()); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_matrix.cols(); j++) { + for (k = 0; k < other_matrix.rows(); k++) { + result(i,j) += mData[i * ncols + k] * other_matrix(k,j); + } + } + } + + return result; + } + + void operator*=(const Matrix &matrix) { + matrix_type temp (*this); + *this = temp * matrix; + } + + // Special operators + val_type *data(){ + return mData; + } + + // regular transpose of a 6 dimensional matrix + Matrix transpose() const { + Matrix result(ncols, nrows); + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + result(j,i) = mData[i * ncols + j]; + } + } + + return result; + } + + operator val_type() { + + assert (nrows == 1); + assert (nrows == 1); + + return mData[0]; + } + + Matrix operator-() const { + return *this * -1.0; + }; + + Matrix inverse() const { + return colPivHouseholderQr().inverse(); + } + + const LLT llt() const { + return LLT(*this); + } + + const HouseholderQR householderQr() const { + return HouseholderQR(*this); + } + const ColPivHouseholderQR colPivHouseholderQr() const { + return ColPivHouseholderQR(*this); + } + + private: + unsigned int nrows; + unsigned int ncols; + bool mapped_data; + + val_type* mData; +}; + +template +inline Matrix operator*(val_type scalar, const Matrix &matrix) { + Matrix result (matrix); + + for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) + result.data()[i] *= scalar; + + return result; +} + +template +inline Matrix operator*(const Matrix &matrix, other_type scalar) { + Matrix result (matrix); + + for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++) + result.data()[i] *= static_cast(scalar); + + return result; +} + +template +inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { + size_t max_width = 0; + size_t out_width = output.width(); + + // get the widest number + for (size_t i = 0; i < matrix.rows(); i++) { + for (size_t j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream << matrix(i,j); + max_width = std::max (out_stream.str().size(),max_width); + } + } + + // overwrite width if it was explicitly prescribed + if (out_width != 0) { + max_width = out_width; + } + + for (unsigned int i = 0; i < matrix.rows(); i++) { + output.width(0); + output << "[ "; + output.width(out_width); + for (unsigned int j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream.width (max_width); + out_stream << matrix(i,j); + output << out_stream.str(); + + if (j < matrix.cols() - 1) + output << ", "; + } + output << " ]"; + + if (matrix.rows() > 1 && i < matrix.rows() - 1) + output << std::endl; + } + return output;} + +} + +} + +#endif /* SIMPLEMATHDYNAMIC_H */ diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathFixed.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathFixed.h new file mode 100644 index 0000000..7a39b2b --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathFixed.h @@ -0,0 +1,881 @@ +/** + * This is a highly inefficient math library. It was conceived by Martin + * Felis while he was compiling code + * that uses a highly efficient math library. + * + * It is intended to be used as a fast compiling substitute for the + * blazingly fast Eigen3 library and tries to mimic its API to a certain + * extend. + * + * Feel free to use it wherever you like. However, no guarantees are given + * that this code does what it says it would. + */ + +#ifndef SIMPLEMATHFIXED_H +#define SIMPLEMATHFIXED_H + +#include +#include +#include +#include +#include + +#include "compileassert.h" +#include "SimpleMathBlock.h" + +/** \brief Namespace for a highly inefficient math library + * + */ +namespace SimpleMath { + +template +class LLT; + +template +class HouseholderQR; + +template +class ColPivHouseholderQR; + +template +class CommaInitializer; + +namespace Dynamic { +template class Matrix; +} + +/** \brief Namespace for fixed size elements + */ +namespace Fixed { + +// forward declaration +template +class Matrix; + +/** \brief Fixed size matrix class + */ +template +class Matrix { + public: + typedef Matrix matrix_type; + typedef val_type value_type; + + unsigned int rows() const { + return nrows; + } + + unsigned int cols() const { + return ncols; + } + + unsigned int size() const { + return nrows * ncols; + } + + Matrix() {}; + Matrix(const Matrix &matrix) { + unsigned int i; + for (i = 0; i < nrows * ncols; i++) + mData[i] = matrix.mData[i]; + } + Matrix& operator=(const Matrix &matrix) { + if (this != &matrix) { + unsigned int i; + for (i = 0; i < nrows * ncols; i++) + mData[i] = matrix.mData[i]; + } + return *this; + } + + // conversion different val_types + + template + Matrix (const Block &block) { + assert (nrows == block.rows()); + assert (ncols == block.cols()); + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(block(i,j)); + } + } + } + template + Matrix& operator= (const Block &block) { + assert (nrows == block.rows()); + assert (ncols == block.cols()); + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(block(i,j)); + } + } + + return *this; + } + + template + Matrix (const Matrix &matrix) { + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(matrix(i,j)); + } + } + } + + template + Matrix& operator=(const Matrix &matrix) { + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + (*this)(i,j) = static_cast(matrix(i,j)); + } + } + + return *this; + } + + CommaInitializer operator<< (const val_type& value) { + return CommaInitializer (*this, value); + } + + // conversion Dynamic->Fixed + Matrix(const Dynamic::Matrix &dynamic_matrix); + Matrix& operator=(const Dynamic::Matrix &dynamic_matrix); + + ~Matrix() {}; + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02 + ) { + assert (nrows == 3); + assert (ncols == 1); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + } + + void set( + const val_type &v00, const val_type &v01, const val_type &v02 + ) { + COMPILE_ASSERT (nrows * ncols == 3); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + } + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02, + const val_type &v10, const val_type &v11, const val_type &v12, + const val_type &v20, const val_type &v21, const val_type &v22 + ) { + COMPILE_ASSERT (nrows == 3); + COMPILE_ASSERT (ncols == 3); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + + mData[1 * 3 + 0] = v10; + mData[1 * 3 + 1] = v11; + mData[1 * 3 + 2] = v12; + + mData[2 * 3 + 0] = v20; + mData[2 * 3 + 1] = v21; + mData[2 * 3 + 2] = v22; + } + + void set( + const val_type v00, const val_type v01, const val_type v02, + const val_type v10, const val_type v11, const val_type v12, + const val_type v20, const val_type v21, const val_type v22 + ) { + COMPILE_ASSERT (nrows == 3); + COMPILE_ASSERT (ncols == 3); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + + mData[1 * 3 + 0] = v10; + mData[1 * 3 + 1] = v11; + mData[1 * 3 + 2] = v12; + + mData[2 * 3 + 0] = v20; + mData[2 * 3 + 1] = v21; + mData[2 * 3 + 2] = v22; + } + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 + ) { + assert (nrows == 4); + assert (ncols == 1); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + } + + void set( + const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03 + ) { + COMPILE_ASSERT (nrows * ncols == 4); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + } + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, + const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, + const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, + const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 + ) { + COMPILE_ASSERT (nrows == 4); + COMPILE_ASSERT (ncols == 4); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + + mData[1 * 4 + 0] = v10; + mData[1 * 4 + 1] = v11; + mData[1 * 4 + 2] = v12; + mData[1 * 4 + 3] = v13; + + mData[2 * 4 + 0] = v20; + mData[2 * 4 + 1] = v21; + mData[2 * 4 + 2] = v22; + mData[2 * 4 + 3] = v23; + + mData[3 * 4 + 0] = v30; + mData[3 * 4 + 1] = v31; + mData[3 * 4 + 2] = v32; + mData[3 * 4 + 3] = v33; + } + + void set( + const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03, + const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13, + const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23, + const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33 + ) { + COMPILE_ASSERT (nrows == 4); + COMPILE_ASSERT (ncols == 4); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + + mData[1 * 4 + 0] = v10; + mData[1 * 4 + 1] = v11; + mData[1 * 4 + 2] = v12; + mData[1 * 4 + 3] = v13; + + mData[2 * 4 + 0] = v20; + mData[2 * 4 + 1] = v21; + mData[2 * 4 + 2] = v22; + mData[2 * 4 + 3] = v23; + + mData[3 * 4 + 0] = v30; + mData[3 * 4 + 1] = v31; + mData[3 * 4 + 2] = v32; + mData[3 * 4 + 3] = v33; + } + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02, + const val_type &v03, const val_type &v04, const val_type &v05 + ) { + COMPILE_ASSERT (nrows == 6); + COMPILE_ASSERT (ncols == 1); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + mData[4] = v04; + mData[5] = v05; + } + + void set( + const val_type &v00, const val_type &v01, const val_type &v02, + const val_type &v03, const val_type &v04, const val_type &v05 + ) { + COMPILE_ASSERT (nrows * ncols == 6); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + mData[4] = v04; + mData[5] = v05; + } + + Matrix ( + const val_type &v00, const val_type &v01, const val_type &v02, + const val_type &v03, const val_type &v04, const val_type &v05, + + const val_type &v10, const val_type &v11, const val_type &v12, + const val_type &v13, const val_type &v14, const val_type &v15, + + const val_type &v20, const val_type &v21, const val_type &v22, + const val_type &v23, const val_type &v24, const val_type &v25, + + const val_type &v30, const val_type &v31, const val_type &v32, + const val_type &v33, const val_type &v34, const val_type &v35, + + const val_type &v40, const val_type &v41, const val_type &v42, + const val_type &v43, const val_type &v44, const val_type &v45, + + const val_type &v50, const val_type &v51, const val_type &v52, + const val_type &v53, const val_type &v54, const val_type &v55 + ) { + COMPILE_ASSERT (nrows == 6); + COMPILE_ASSERT (ncols == 6); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + mData[4] = v04; + mData[5] = v05; + + mData[6 + 0] = v10; + mData[6 + 1] = v11; + mData[6 + 2] = v12; + mData[6 + 3] = v13; + mData[6 + 4] = v14; + mData[6 + 5] = v15; + + mData[12 + 0] = v20; + mData[12 + 1] = v21; + mData[12 + 2] = v22; + mData[12 + 3] = v23; + mData[12 + 4] = v24; + mData[12 + 5] = v25; + + mData[18 + 0] = v30; + mData[18 + 1] = v31; + mData[18 + 2] = v32; + mData[18 + 3] = v33; + mData[18 + 4] = v34; + mData[18 + 5] = v35; + + mData[24 + 0] = v40; + mData[24 + 1] = v41; + mData[24 + 2] = v42; + mData[24 + 3] = v43; + mData[24 + 4] = v44; + mData[24 + 5] = v45; + + mData[30 + 0] = v50; + mData[30 + 1] = v51; + mData[30 + 2] = v52; + mData[30 + 3] = v53; + mData[30 + 4] = v54; + mData[30 + 5] = v55; + }; + + void set( + const val_type v00, const val_type v01, const val_type v02, + const val_type v03, const val_type v04, const val_type v05, + + const val_type v10, const val_type v11, const val_type v12, + const val_type v13, const val_type v14, const val_type v15, + + const val_type v20, const val_type v21, const val_type v22, + const val_type v23, const val_type v24, const val_type v25, + + const val_type v30, const val_type v31, const val_type v32, + const val_type v33, const val_type v34, const val_type v35, + + const val_type v40, const val_type v41, const val_type v42, + const val_type v43, const val_type v44, const val_type v45, + + const val_type v50, const val_type v51, const val_type v52, + const val_type v53, const val_type v54, const val_type v55 + ) { + COMPILE_ASSERT (nrows == 6); + COMPILE_ASSERT (ncols == 6); + + mData[0] = v00; + mData[1] = v01; + mData[2] = v02; + mData[3] = v03; + mData[4] = v04; + mData[5] = v05; + + mData[6 + 0] = v10; + mData[6 + 1] = v11; + mData[6 + 2] = v12; + mData[6 + 3] = v13; + mData[6 + 4] = v14; + mData[6 + 5] = v15; + + mData[12 + 0] = v20; + mData[12 + 1] = v21; + mData[12 + 2] = v22; + mData[12 + 3] = v23; + mData[12 + 4] = v24; + mData[12 + 5] = v25; + + mData[18 + 0] = v30; + mData[18 + 1] = v31; + mData[18 + 2] = v32; + mData[18 + 3] = v33; + mData[18 + 4] = v34; + mData[18 + 5] = v35; + + mData[24 + 0] = v40; + mData[24 + 1] = v41; + mData[24 + 2] = v42; + mData[24 + 3] = v43; + mData[24 + 4] = v44; + mData[24 + 5] = v45; + + mData[30 + 0] = v50; + mData[30 + 1] = v51; + mData[30 + 2] = v52; + mData[30 + 3] = v53; + mData[30 + 4] = v54; + mData[30 + 5] = v55; + } + + // comparison + bool operator==(const Matrix &matrix) const { + for (unsigned int i = 0; i < nrows * ncols; i++) { + if (mData[i] != matrix.mData[i]) + return false; + } + return true; + } + bool operator!=(const Matrix &matrix) const { + for (unsigned int i = 0; i < nrows * ncols; i++) { + if (mData[i] != matrix.mData[i]) + return true; + } + return false; + } + + // access operators + const val_type& operator[](const unsigned int &index) const { + assert (index >= 0 && index < nrows * ncols); + return mData[index]; + }; + val_type& operator[](const unsigned int &index) { + assert (index >= 0 && index < nrows * ncols); + return mData[index]; + } + + const val_type& operator()(const unsigned int &row, const unsigned int &col) const { + assert (row >= 0 && row < nrows && col >= 0 && col < ncols); + return mData[row*ncols + col]; + }; + val_type& operator()(const unsigned int &row, const unsigned int &col) { + assert (row >= 0 && row < nrows && col >= 0 && col < ncols); + return mData[row*ncols + col]; + }; + + void zero() { + for (unsigned int i = 0; i < ncols * nrows; i++) + mData[i] = 0.; + } + void setZero() { + zero(); + } + + val_type norm() const { + return sqrt(this->squaredNorm()); + } + + matrix_type normalize() { + val_type length = this->norm(); + + for (unsigned int i = 0; i < ncols * nrows; i++) + mData[i] /= length; + + return *this; + } + + matrix_type normalized() const { + return matrix_type (*this) / this->norm(); + } + + Matrix cross(const Matrix &other_vector) const { + COMPILE_ASSERT (nrows * ncols == 3); + + Matrix result; + result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1]; + result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2]; + result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0]; + + return result; + } + + val_type trace() const { + COMPILE_ASSERT(nrows == ncols); + val_type result = 0.; + + for (unsigned int i = 0; i < rows(); i++) { + result += operator()(i,i); + } + + return result; + } + + val_type mean() const { + COMPILE_ASSERT(nrows == 1 || ncols == 1); + + val_type result = 0.; + for (unsigned int i = 0; i < rows() * cols(); i++) { + result += operator[](i); + } + + return result / static_cast(nrows * ncols); + } + + static matrix_type Zero() { + matrix_type result; + result.setZero(); + return result; + } + + static matrix_type Zero(int ignore_me) { + matrix_type result; + result.setZero(); + return result; + } + + static matrix_type Zero(int ignore_me, int ignore_me_too) { + matrix_type result; + result.setZero(); + return result; + } + + static matrix_type Identity() { + matrix_type result; + result.identity(); + return result; + } + + static matrix_type Identity(int ignore_me, int ignore_me_too) { + matrix_type result; + result.identity(); + return result; + } + + void identity() { + COMPILE_ASSERT (nrows == ncols); + + setZero(); + for (unsigned int i = 0; i < ncols; i++) + mData[i * ncols + i] = 1.; + } + + void random() { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] = static_cast (rand()) / static_cast (RAND_MAX); + } + + val_type squaredNorm() const { + COMPILE_ASSERT (ncols == 1 || nrows == 1); + val_type result = 0; + + for (unsigned int i = 0; i < nrows * ncols; i++) + result += mData[i] * mData[i]; + + return result; + } + + val_type dot(const matrix_type &matrix) const { + COMPILE_ASSERT (ncols == 1 || nrows == 1); + val_type result = 0; + + for (unsigned int i = 0; i < nrows * ncols; i++) + result += mData[i] * matrix[i]; + + return result; + } + + // Blocks using block(i,j,r,c) syntax + Block + block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) { + return Block(*this, row_start, col_start, row_count, col_count); + } + + const Block + block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const { + return Block(*this, row_start, col_start, row_count, col_count); + } + + // Blocks using block(i,j) syntax + template + Block + block (unsigned int row_start, unsigned int col_start) { + return Block(*this, row_start, col_start, block_row_count, block_col_count); + } + + template + const Block + block (unsigned int row_start, unsigned int col_start) const { + return Block(*this, row_start, col_start, block_row_count, block_col_count); + } + + // row and col accessors + Block + col(unsigned int index) const { + return Block(*this, 0, index, rows(), 1); + } + + Block + col(unsigned int index) { + return Block(*this, 0, index, rows(), 1); + } + + Block + row(unsigned int index) const { + return Block(*this, index, 0, 1, cols()); + } + + Block + row(unsigned int index) { + return Block(*this, index, 0, 1, cols()); + } + + // Operators with scalars + void operator*=(const val_type &scalar) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] *= scalar; + }; + void operator/=(const val_type &scalar) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] /= scalar; + } + Matrix operator/(const val_type& scalar) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] /= scalar; + + return result; + } + + // Operators with other matrices + Matrix operator+(const Matrix &matrix) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] += matrix[i]; + + return result; + } + void operator+=(const matrix_type &matrix) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] += matrix.mData[i]; + } + Matrix operator-(const Matrix &matrix) const { + matrix_type result (*this); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] -= matrix[i]; + + return result; + } + void operator-=(const Matrix &matrix) { + for (unsigned int i = 0; i < nrows * ncols; i++) + mData[i] -= matrix.mData[i]; + } + + template + Matrix operator*(const Matrix &matrix) const { + COMPILE_ASSERT (ncols == other_rows); + + Matrix result; + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_cols; j++) { + for (k = 0; k < other_rows; k++) { + result(i,j) += mData[i * ncols + k] * matrix(k,j); + } + } + } + + return result; + } + + // multiplication with dynamic sized matrix + template + Dynamic::Matrix operator*(const Dynamic::Matrix &other_matrix) { + assert (ncols == other_matrix.rows()); + + Dynamic::Matrix result(nrows, other_matrix.cols()); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_matrix.cols(); j++) { + for (k = 0; k < other_matrix.rows(); k++) { + result(i,j) += mData[i * ncols + k] * static_cast(other_matrix(k,j)); + } + } + } + + return result; + } + + // Multiplication with a block + template + Dynamic::Matrix operator*(const Block &block) { + assert (ncols == block.rows()); + + Dynamic::Matrix result(nrows, block.cols()); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < block.cols(); j++) { + for (k = 0; k < block.rows(); k++) { + result(i,j) += mData[i * ncols + k] * static_cast(block(k,j)); + } + } + } + + return result; + } + + + void operator*=(const Matrix &matrix) { + matrix_type temp (*this); + *this = temp * matrix; + } + + // Special operators + val_type *data(){ + return mData; + } + + const val_type *data() const{ + return mData; + } + + // regular transpose of a 6 dimensional matrix + Matrix transpose() const { + Matrix result; + + for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int j = 0; j < ncols; j++) { + result(j,i) = mData[i * ncols + j]; + } + } + + return result; + } + + operator val_type() { + COMPILE_ASSERT (nrows == 1); + COMPILE_ASSERT (nrows == 1); + + return mData[0]; + } + + Matrix operator-() const { + return *this * -1.; + } + + Matrix inverse() const { + return colPivHouseholderQr().inverse(); + } + + const LLT llt() const { + return LLT(*this); + } + + const HouseholderQR householderQr() const { + return HouseholderQR(*this); + } + const ColPivHouseholderQR colPivHouseholderQr() const { + return ColPivHouseholderQR(*this); + } + + private: + val_type mData[nrows * ncols]; +}; + +template +inline Matrix operator*(val_type scalar, const Matrix &matrix) { + Matrix result (matrix); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result.data()[i] *= scalar; + + return result; +} + +template +inline Matrix operator*(const Matrix &matrix, other_type scalar) { + Matrix result (matrix); + + for (unsigned int i = 0; i < nrows * ncols; i++) + result.data()[i] *= static_cast (scalar); + + return result; +} + +template +inline std::ostream& operator<<(std::ostream& output, const Matrix &matrix) { + size_t max_width = 0; + size_t out_width = output.width(); + + // get the widest number + for (size_t i = 0; i < matrix.rows(); i++) { + for (size_t j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream << matrix(i,j); + max_width = std::max (out_stream.str().size(),max_width); + } + } + + // overwrite width if it was explicitly prescribed + if (out_width != 0) { + max_width = out_width; + } + + for (unsigned int i = 0; i < matrix.rows(); i++) { + output.width(0); + output << "[ "; + output.width(out_width); + for (unsigned int j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream.width (max_width); + out_stream << matrix(i,j); + output << out_stream.str(); + + if (j < matrix.cols() - 1) + output << ", "; + } + output << " ]"; + + if (matrix.rows() > 1 && i < matrix.rows() - 1) + output << std::endl; + } + return output; +} + +} + +} + +#endif /* SIMPLEMATHFIXED_H */ diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathGL.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathGL.h new file mode 100644 index 0000000..15e45ee --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathGL.h @@ -0,0 +1,253 @@ +#ifndef _SIMPLEMATHGL_H_ +#define _SIMPLEMATHGL_H_ + +#include "SimpleMath.h" +#include + +namespace SimpleMath { + +namespace GL { + +inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) { + float c = cosf (rot_deg * M_PI / 180.f); + float s = sinf (rot_deg * M_PI / 180.f); + return Matrix44f ( + x * x * (1.0f - c) + c, + y * x * (1.0f - c) + z * s, + x * z * (1.0f - c) - y * s, + 0.f, + + x * y * (1.0f - c) - z * s, + y * y * (1.0f - c) + c, + y * z * (1.0f - c) + x * s, + 0.f, + + x * z * (1.0f - c) + y * s, + y * z * (1.0f - c) - x * s, + z * z * (1.0f - c) + c, + 0.f, + + 0.f, 0.f, 0.f, 1.f + ); +} + +inline Matrix44f TranslateMat44 (float x, float y, float z) { + return Matrix44f ( + 1.f, 0.f, 0.f, 0.f, + 0.f, 1.f, 0.f, 0.f, + 0.f, 0.f, 1.f, 0.f, + x, y, z, 1.f + ); +} + +inline Matrix44f ScaleMat44 (float x, float y, float z) { + return Matrix44f ( + x, 0.f, 0.f, 0.f, + 0.f, y, 0.f, 0.f, + 0.f, 0.f, z, 0.f, + 0.f, 0.f, 0.f, 1.f + ); +} + +/** Quaternion + * + * order: x,y,z,w + */ +class Quaternion : public Vector4f { + public: + Quaternion () : + Vector4f (0.f, 0.f, 0.f, 1.f) + {} + Quaternion (const Vector4f vec4) : + Vector4f (vec4) + {} + Quaternion (float x, float y, float z, float w): + Vector4f (x, y, z, w) + {} + /** This function is equivalent to multiplicate their corresponding rotation matrices */ + Quaternion operator* (const Quaternion &q) const { + return Quaternion ( + q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1], + q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2], + q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0], + q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2] + ); + } + Quaternion& operator*=(const Quaternion &q) { + set ( + q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1], + q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2], + q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0], + q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2] + ); + return *this; + } + + static Quaternion fromGLRotate (float angle, float x, float y, float z) { + float st = sinf (angle * M_PI / 360.f); + return Quaternion ( + st * x, + st * y, + st * z, + cosf (angle * M_PI / 360.f) + ); + } + + Quaternion normalize() { + return Vector4f::normalize(); + } + + Quaternion slerp (float alpha, const Quaternion &quat) const { + // check whether one of the two has 0 length + float s = sqrt (squaredNorm() * quat.squaredNorm()); + + // division by 0.f is unhealthy! + assert (s != 0.f); + + float angle = acos (dot(quat) / s); + if (angle == 0.f || isnan(angle)) { + return *this; + } + assert(!isnan(angle)); + + float d = 1.f / sinf (angle); + float p0 = sinf ((1.f - alpha) * angle); + float p1 = sinf (alpha * angle); + + if (dot (quat) < 0.f) { + return Quaternion( ((*this) * p0 - quat * p1) * d); + } + return Quaternion( ((*this) * p0 + quat * p1) * d); + } + + Matrix44f toGLMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix44f ( + 1 - 2*y*y - 2*z*z, + 2*x*y + 2*w*z, + 2*x*z - 2*w*y, + 0.f, + + 2*x*y - 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z + 2*w*x, + 0.f, + + 2*x*z + 2*w*y, + 2*y*z - 2*w*x, + 1 - 2*x*x - 2*y*y, + 0.f, + + 0.f, + 0.f, + 0.f, + 1.f); + } + + static Quaternion fromGLMatrix(const Matrix44f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + -(mat(2,1) - mat(1,2)) / (w * 4.f), + -(mat(0,2) - mat(2,0)) / (w * 4.f), + -(mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromMatrix (const Matrix33f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + (mat(2,1) - mat(1,2)) / (w * 4.f), + (mat(0,2) - mat(2,0)) / (w * 4.f), + (mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromEulerZYX (const Vector3f &zyx_euler) { + return Quaternion::fromGLRotate (zyx_euler[0] * 180.f / M_PI, 0.f, 0.f, 1.f) + * Quaternion::fromGLRotate (zyx_euler[1] * 180.f / M_PI, 0.f, 1.f, 0.f) + * Quaternion::fromGLRotate (zyx_euler[2] * 180.f / M_PI, 1.f, 0.f, 0.f); + } + + Vector3f toEulerZYX () const { + return Vector3f ( + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + (*this)[0] * (*this)[0] + (*this)[3] * (*this)[3] + -(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]), + asin (2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1]), + atan2 (-2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0], + (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] + -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3] + ) + ); + } + + + static Quaternion fromEulerYXZ (const Vector3f &yxz_euler) { + return Quaternion::fromGLRotate (yxz_euler[0] * 180.f / M_PI, 0.f, 1.f, 0.f) + * Quaternion::fromGLRotate (yxz_euler[1] * 180.f / M_PI, 1.f, 0.f, 0.f) + * Quaternion::fromGLRotate (yxz_euler[2] * 180.f / M_PI, 0.f, 0.f, 1.f); + } + + Vector3f toEulerYXZ() const { + return Vector3f ( + atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1], + (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] + -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]), + asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]), + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2] + +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0] + ) + ); + } + + Matrix33f toMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix33f ( + 1 - 2*y*y - 2*z*z, + 2*x*y - 2*w*z, + 2*x*z + 2*w*y, + + 2*x*y + 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z - 2*w*x, + + 2*x*z - 2*w*y, + 2*y*z + 2*w*x, + 1 - 2*x*x - 2*y*y + ); + } + + Quaternion conjugate() const { + return Quaternion ( + -(*this)[0], + -(*this)[1], + -(*this)[2], + (*this)[3]); + } + + Vector3f rotate (const Vector3f &vec) const { + Vector3f vn (vec); + Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; + + res_quat = vec_quat * (*this); + res_quat = conjugate() * res_quat; + + return Vector3f (res_quat[0], res_quat[1], res_quat[2]); + } +}; + +// namespace GL +} + +// namespace SimpleMath +} + +/* _SIMPLEMATHGL_H_ */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMap.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMap.h new file mode 100644 index 0000000..dbb757a --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMap.h @@ -0,0 +1,22 @@ +#ifndef SIMPLEMATHMAP_H +#define SIMPLEMATHMAP_H + +#include "compileassert.h" + +namespace SimpleMath { + +/** \brief \brief Wraps a varying size matrix type around existing data + * + * \warning If you create a matrix using the map function and then assign + * a bigger matrix you invalidate the original memory! + * + */ +template < typename MatrixType > +MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) { + return MatrixType (rows, cols, data); +} + +} + +// SIMPLEMATHMAP_H +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMixed.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMixed.h new file mode 100644 index 0000000..777670f --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathMixed.h @@ -0,0 +1,138 @@ +/** + * This is a highly inefficient math library. It was conceived by Martin + * Felis while he was compiling code + * that uses a highly efficient math library. + * + * It is intended to be used as a fast compiling substitute for the + * blazingly fast Eigen3 library and tries to mimic its API to a certain + * extend. + * + * Feel free to use it wherever you like. However, no guarantees are given + * that this code does what it says it would. + */ + +#ifndef SIMPLEMATHMIXED_H +#define SIMPLEMATHMIXED_H + +#include +#include +#include +#include + +#include "compileassert.h" + +/** \brief Namespace for a highly inefficient math library + * + */ +namespace SimpleMath { + +// conversion Dynamic->Fixed +template +inline Fixed::Matrix::Matrix(const Dynamic::Matrix &dynamic_matrix) { + if (dynamic_matrix.cols() != ncols + || dynamic_matrix.rows() != nrows) { + std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; + abort(); + } + + for (unsigned int i = 0; i < nrows * ncols; i++) { + mData[i] = dynamic_matrix[i]; + } +} + +template +inline Fixed::Matrix& Fixed::Matrix::operator=(const Dynamic::Matrix &dynamic_matrix) { + if (dynamic_matrix.cols() != ncols + || dynamic_matrix.rows() != nrows) { + std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; + abort(); + } + + for (unsigned int i = 0; i < nrows * ncols; i++) { + mData[i] = dynamic_matrix[i]; + } + + return *this; +} + +// multiplication +template +inline Dynamic::Matrix operator*( + const Fixed::Matrix &matrix_a, + const Dynamic::Matrix &matrix_b) { + assert (matrix_a.cols() == matrix_b.rows()); + + Dynamic::Matrix result (nrows, matrix_b.cols()); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < nrows; i++) { + for (j = 0; j < matrix_b.cols(); j++) { + for (k = 0; k < matrix_b.rows(); k++) { + result(i,j) += matrix_a(i,k) * matrix_b(k,j); + } + } + } + + return result; +} + +template +inline Dynamic::Matrix operator*( + const Dynamic::Matrix &matrix_a, + const Fixed::Matrix &matrix_b) { + assert (matrix_a.cols() == matrix_b.rows()); + + Dynamic::Matrix result (matrix_a.rows(), ncols); + + result.setZero(); + + unsigned int i,j, k; + for (i = 0; i < matrix_a.rows(); i++) { + for (j = 0; j < matrix_b.cols(); j++) { + for (k = 0; k < matrix_b.rows(); k++) { + result(i,j) += matrix_a(i,k) * matrix_b(k,j); + } + } + } + + return result; +} + +// equality +template +inline bool operator==( + const Fixed::Matrix &matrix_a, + const Dynamic::Matrix &matrix_b) { + assert (nrows == matrix_a.rows()); + assert (ncols == matrix_a.cols()); + + unsigned int i; + for (i = 0; i < matrix_a.size(); i++) { + if (matrix_a[i] != matrix_b[i]) + return false; + } + + return true; +} + +template +inline bool operator==( + const Dynamic::Matrix &matrix_b, + const Fixed::Matrix &matrix_a) { + assert (nrows == matrix_a.rows()); + assert (ncols == matrix_a.cols()); + + unsigned int i; + for (i = 0; i < matrix_a.size(); i++) { + if (matrix_a[i] != matrix_b[i]) + return false; + } + + return true; +} + +} + +#endif /* SIMPLEMATHMIXED_H */ diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathQR.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathQR.h new file mode 100644 index 0000000..91832f2 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMathQR.h @@ -0,0 +1,324 @@ +#ifndef _SIMPLE_MATH_QR_H +#define _SIMPLE_MATH_QR_H + +#include +#include + +#include "SimpleMathFixed.h" +#include "SimpleMathDynamic.h" +#include "SimpleMathBlock.h" + +namespace SimpleMath { + + template + class HouseholderQR { + public: + typedef typename matrix_type::value_type value_type; + HouseholderQR() : + mIsFactorized(false) + {} + + private: + typedef Dynamic::Matrix MatrixXXd; + typedef Dynamic::Matrix VectorXd; + + bool mIsFactorized; + MatrixXXd mQ; + MatrixXXd mR; + + public: + HouseholderQR(const matrix_type &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()) + { + compute(matrix); + } + HouseholderQR compute(const matrix_type &matrix) { + mR = matrix; + mQ = Dynamic::Matrix::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5)); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + Dynamic::Matrix solve ( + const Dynamic::Matrix &rhs + ) const { + assert (mIsFactorized); + + VectorXd y = mQ.transpose() * rhs; + VectorXd x = VectorXd::Zero(mR.cols()); + + for (int i = mR.cols() - 1; i >= 0; --i) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + z = z - x[j] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[i] = z / mR(i,i); + } + + return x; + } + Dynamic::Matrix inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + Dynamic::Matrix householderQ () const { + return mQ; + } + Dynamic::Matrix matrixR () const { + return mR; + } + }; + + template + class ColPivHouseholderQR { + public: + typedef typename matrix_type::value_type value_type; + private: + typedef Dynamic::Matrix MatrixXXd; + typedef Dynamic::Matrix VectorXd; + + bool mIsFactorized; + MatrixXXd mQ; + MatrixXXd mR; + unsigned int *mPermutations; + value_type mThreshold; + unsigned int mRank; + + public: + ColPivHouseholderQR(): + mIsFactorized(false) { + mPermutations = new unsigned int[1]; + } + + ColPivHouseholderQR (const ColPivHouseholderQR& other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) { + if (this != &other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + delete[] mPermutations; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + return *this; + } + + ColPivHouseholderQR(const matrix_type &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()), + mThreshold (std::numeric_limits::epsilon() * matrix.cols()) { + mPermutations = new unsigned int [matrix.cols()]; + for (unsigned int i = 0; i < matrix.cols(); i++) { + mPermutations[i] = i; + } + compute(matrix); + } + ~ColPivHouseholderQR() { + delete[] mPermutations; + } + + ColPivHouseholderQR& setThreshold (const value_type& threshold) { + mThreshold = threshold; + + return *this; + } + ColPivHouseholderQR& compute(const matrix_type &matrix) { + mR = matrix; + mQ = Dynamic::Matrix::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + // find and swap the column with the highest norm + unsigned int col_index_norm_max = i; + value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm(); + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + VectorXd column = mR.block(i, j, block_rows, 1); + + if (column.squaredNorm() > col_norm_max) { + col_index_norm_max = j; + col_norm_max = column.squaredNorm(); + } + } + + if (col_norm_max < mThreshold) { + // if all entries of the column is close to zero, we bail out + break; + } + + + if (col_index_norm_max != i) { + VectorXd temp_col = mR.block(0, i, mR.rows(), 1); + mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1); + mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col; + + unsigned int temp_index = mPermutations[i]; + mPermutations[i] = mPermutations[col_index_norm_max]; + mPermutations[col_index_norm_max] = temp_index; + } + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5)); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + Dynamic::Matrix solve ( + const Dynamic::Matrix &rhs + ) const { + assert (mIsFactorized); + + VectorXd y = mQ.transpose() * rhs; + VectorXd x = VectorXd::Zero(mR.cols()); + + for (int i = mR.cols() - 1; i >= 0; --i) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + z = z - x[mPermutations[j]] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl; + abort(); + } + x[mPermutations[i]] = z / mR(i,i); + } + + return x; + } + Dynamic::Matrix inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + + Dynamic::Matrix householderQ () const { + return mQ; + } + Dynamic::Matrix matrixR () const { + return mR; + } + Dynamic::Matrix matrixP () const { + MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols()); + MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols()); + for (unsigned int i = 0; i < mR.cols(); i++) { + P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1); + } + return P; + } + + unsigned int rank() const { + value_type abs_threshold = fabs(mR(0,0)) * mThreshold; + + for (unsigned int i = 1; i < mR.cols(); i++) { + if (fabs(mR(i,i) < abs_threshold)) + return i; + } + + return mR.cols(); + } + }; + +} + +/* _SIMPLE_MATH_QR_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/compileassert.h b/3rdparty/rbdl/include/rbdl/SimpleMath/compileassert.h new file mode 100644 index 0000000..4d12fdb --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/compileassert.h @@ -0,0 +1,39 @@ +#ifndef _COMPILE_ASSERT_H +#define _COMPILE_ASSERT_H + +/* + * This is a simple compile time assertion tool taken from: + * http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx + * written by Abhinaba Basu! + * + * Thanks! + */ + +#ifdef __cplusplus + +#define JOIN( X, Y ) JOIN2(X,Y) +#define JOIN2( X, Y ) X##Y + +namespace static_assert_compat +{ + template struct STATIC_ASSERT_FAILURE; + template <> struct STATIC_ASSERT_FAILURE { enum { value = 1 }; }; + + template struct static_assert_test{}; +} + +#define COMPILE_ASSERT(x) \ + typedef ::static_assert_compat::static_assert_test<\ + sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\ + JOIN(_static_assert_typedef, __LINE__) + +#else // __cplusplus + +#define COMPILE_ASSERT(x) extern int __dummy[(int)x] + +#endif // __cplusplus + +#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) + +// _COMPILE_ASSERT_H_ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h b/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h new file mode 100644 index 0000000..2ec9bed --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h @@ -0,0 +1,452 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_SPATIALALGEBRAOPERATORS_H +#define RBDL_SPATIALALGEBRAOPERATORS_H + +#include +#include + +namespace RigidBodyDynamics { + +namespace Math { + +inline Matrix3d VectorCrossMatrix (const Vector3d &vector) { + return Matrix3d ( + 0., -vector[2], vector[1], + vector[2], 0., -vector[0], + -vector[1], vector[0], 0. + ); +} + +/** \brief Compact representation for Spatial Inertia. */ +struct RBDL_DLLAPI SpatialRigidBodyInertia { + SpatialRigidBodyInertia() : + m (0.), + h (Vector3d::Zero(3,1)), + Ixx (0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.) + {} + SpatialRigidBodyInertia ( + double mass, const Vector3d &com_mass, const Matrix3d &inertia) : + m (mass), h (com_mass), + Ixx (inertia(0,0)), + Iyx (inertia(1,0)), Iyy(inertia(1,1)), + Izx (inertia(2,0)), Izy(inertia(2,1)), Izz(inertia(2,2)) + { } + SpatialRigidBodyInertia (double m, const Vector3d &h, + const double &Ixx, + const double &Iyx, const double &Iyy, + const double &Izx, const double &Izy, const double &Izz + ) : + m (m), h (h), + Ixx (Ixx), + Iyx (Iyx), Iyy(Iyy), + Izx (Izx), Izy(Izy), Izz(Izz) + { } + + SpatialVector operator* (const SpatialVector &mv) { + Vector3d mv_lower (mv[3], mv[4], mv[5]); + + Vector3d res_upper = Vector3d ( + Ixx * mv[0] + Iyx * mv[1] + Izx * mv[2], + Iyx * mv[0] + Iyy * mv[1] + Izy * mv[2], + Izx * mv[0] + Izy * mv[1] + Izz * mv[2] + ) + h.cross(mv_lower); + Vector3d res_lower = m * mv_lower - h.cross (Vector3d (mv[0], mv[1], mv[2])); + + return SpatialVector ( + res_upper[0], res_upper[1], res_upper[2], + res_lower[0], res_lower[1], res_lower[2] + ); + } + + SpatialRigidBodyInertia operator+ (const SpatialRigidBodyInertia &rbi) { + return SpatialRigidBodyInertia ( + m + rbi.m, + h + rbi.h, + Ixx + rbi.Ixx, + Iyx + rbi.Iyx, Iyy + rbi.Iyy, + Izx + rbi.Izx, Izy + rbi.Izy, Izz + rbi.Izz + ); + } + + void createFromMatrix (const SpatialMatrix &Ic) { + m = Ic(3,3); + h.set (-Ic(1,5), Ic(0,5), -Ic(0,4)); + Ixx = Ic(0,0); + Iyx = Ic(1,0); Iyy = Ic(1,1); + Izx = Ic(2,0); Izy = Ic(2,1); Izz = Ic(2,2); + } + + SpatialMatrix toMatrix() const { + SpatialMatrix result; + result(0,0) = Ixx; result(0,1) = Iyx; result(0,2) = Izx; + result(1,0) = Iyx; result(1,1) = Iyy; result(1,2) = Izy; + result(2,0) = Izx; result(2,1) = Izy; result(2,2) = Izz; + + result.block<3,3>(0,3) = VectorCrossMatrix(h); + result.block<3,3>(3,0) = - VectorCrossMatrix(h); + result.block<3,3>(3,3) = Matrix3d::Identity(3,3) * m; + + return result; + } + + void setSpatialMatrix (SpatialMatrix &mat) const { + mat(0,0) = Ixx; mat(0,1) = Iyx; mat(0,2) = Izx; + mat(1,0) = Iyx; mat(1,1) = Iyy; mat(1,2) = Izy; + mat(2,0) = Izx; mat(2,1) = Izy; mat(2,2) = Izz; + + mat(3,0) = 0.; mat(3,1) = h[2]; mat(3,2) = -h[1]; + mat(4,0) = -h[2]; mat(4,1) = 0.; mat(4,2) = h[0]; + mat(5,0) = h[1]; mat(5,1) = -h[0]; mat(5,2) = 0.; + + mat(0,3) = 0.; mat(0,4) = -h[2]; mat(0,5) = h[1]; + mat(1,3) = h[2]; mat(1,4) = 0.; mat(1,5) = -h[0]; + mat(2,3) = -h[1]; mat(2,4) = h[0]; mat(2,5) = 0.; + + mat(3,3) = m; mat(3,4) = 0.; mat(3,5) = 0.; + mat(4,3) = 0.; mat(4,4) = m; mat(4,5) = 0.; + mat(5,3) = 0.; mat(5,4) = 0.; mat(5,5) = m; + } + + static SpatialRigidBodyInertia createFromMassComInertiaC (double mass, const Vector3d &com, const Matrix3d &inertia_C) { + SpatialRigidBodyInertia result; + result.m = mass; + result.h = com * mass; + Matrix3d I = inertia_C + VectorCrossMatrix (com) * VectorCrossMatrix(com).transpose() * mass; + result.Ixx = I(0,0); + result.Iyx = I(1,0); + result.Iyy = I(1,1); + result.Izx = I(2,0); + result.Izy = I(2,1); + result.Izz = I(2,2); + return result; + } + + /// Mass + double m; + /// Coordinates of the center of mass + Vector3d h; + /// Inertia expressed at the origin + double Ixx, Iyx, Iyy, Izx, Izy, Izz; +}; + +/** \brief Compact representation of spatial transformations. + * + * Instead of using a verbose 6x6 matrix, this structure only stores a 3x3 + * matrix and a 3-d vector to store spatial transformations. It also + * encapsulates efficient operations such as concatenations and + * transformation of spatial vectors. + */ +struct RBDL_DLLAPI SpatialTransform { + SpatialTransform() : + E (Matrix3d::Identity(3,3)), + r (Vector3d::Zero(3,1)) + {} + SpatialTransform (const Matrix3d &rotation, const Vector3d &translation) : + E (rotation), + r (translation) + {} + + /** Same as X * v. + * + * \returns (E * w, - E * rxw + E * v) + */ + SpatialVector apply (const SpatialVector &v_sp) { + Vector3d v_rxw ( + v_sp[3] - r[1]*v_sp[2] + r[2]*v_sp[1], + v_sp[4] - r[2]*v_sp[0] + r[0]*v_sp[2], + v_sp[5] - r[0]*v_sp[1] + r[1]*v_sp[0] + ); + return SpatialVector ( + E(0,0) * v_sp[0] + E(0,1) * v_sp[1] + E(0,2) * v_sp[2], + E(1,0) * v_sp[0] + E(1,1) * v_sp[1] + E(1,2) * v_sp[2], + E(2,0) * v_sp[0] + E(2,1) * v_sp[1] + E(2,2) * v_sp[2], + E(0,0) * v_rxw[0] + E(0,1) * v_rxw[1] + E(0,2) * v_rxw[2], + E(1,0) * v_rxw[0] + E(1,1) * v_rxw[1] + E(1,2) * v_rxw[2], + E(2,0) * v_rxw[0] + E(2,1) * v_rxw[1] + E(2,2) * v_rxw[2] + ); + } + + /** Same as X^T * f. + * + * \returns (E^T * n + rx * E^T * f, E^T * f) + */ + SpatialVector applyTranspose (const SpatialVector &f_sp) { + Vector3d E_T_f ( + E(0,0) * f_sp[3] + E(1,0) * f_sp[4] + E(2,0) * f_sp[5], + E(0,1) * f_sp[3] + E(1,1) * f_sp[4] + E(2,1) * f_sp[5], + E(0,2) * f_sp[3] + E(1,2) * f_sp[4] + E(2,2) * f_sp[5] + ); + + return SpatialVector ( + E(0,0) * f_sp[0] + E(1,0) * f_sp[1] + E(2,0) * f_sp[2] - r[2] * E_T_f[1] + r[1] * E_T_f[2], + E(0,1) * f_sp[0] + E(1,1) * f_sp[1] + E(2,1) * f_sp[2] + r[2] * E_T_f[0] - r[0] * E_T_f[2], + E(0,2) * f_sp[0] + E(1,2) * f_sp[1] + E(2,2) * f_sp[2] - r[1] * E_T_f[0] + r[0] * E_T_f[1], + E_T_f [0], + E_T_f [1], + E_T_f [2] + ); + } + + /** Same as X^* I X^{-1} + */ + SpatialRigidBodyInertia apply (const SpatialRigidBodyInertia &rbi) { + return SpatialRigidBodyInertia ( + rbi.m, + E * (rbi.h - rbi.m * r), + E * + ( + Matrix3d ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ) + + VectorCrossMatrix (r) * VectorCrossMatrix (rbi.h) + + (VectorCrossMatrix(rbi.h - rbi.m * r) * VectorCrossMatrix (r)) + ) + * E.transpose() + ); + } + + /** Same as X^T I X + */ + SpatialRigidBodyInertia applyTranspose (const SpatialRigidBodyInertia &rbi) { + Vector3d E_T_mr = E.transpose() * rbi.h + rbi.m * r; + return SpatialRigidBodyInertia ( + rbi.m, + E_T_mr, + E.transpose() * + Matrix3d ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ) * E + - VectorCrossMatrix(r) * VectorCrossMatrix (E.transpose() * rbi.h) + - VectorCrossMatrix (E_T_mr) * VectorCrossMatrix (r)); + } + + SpatialVector applyAdjoint (const SpatialVector &f_sp) { + Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Vector3d (f_sp[3], f_sp[4], f_sp[5]))); + // Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Eigen::Map (&(f_sp[3])))); + + return SpatialVector ( + En_rxf[0], + En_rxf[1], + En_rxf[2], + E(0,0) * f_sp[3] + E(0,1) * f_sp[4] + E(0,2) * f_sp[5], + E(1,0) * f_sp[3] + E(1,1) * f_sp[4] + E(1,2) * f_sp[5], + E(2,0) * f_sp[3] + E(2,1) * f_sp[4] + E(2,2) * f_sp[5] + ); + } + + SpatialMatrix toMatrix () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E; + result.block<3,3>(0,3) = Matrix3d::Zero(3,3); + result.block<3,3>(3,0) = -_Erx; + result.block<3,3>(3,3) = E; + + return result; + } + + SpatialMatrix toMatrixAdjoint () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E; + result.block<3,3>(0,3) = -_Erx; + result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,3) = E; + + return result; + } + + SpatialMatrix toMatrixTranspose () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E.transpose(); + result.block<3,3>(0,3) = -_Erx.transpose(); + result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,3) = E.transpose(); + + return result; + } + + SpatialTransform inverse() const { + return SpatialTransform ( + E.transpose(), + - E * r + ); + } + + SpatialTransform operator* (const SpatialTransform &XT) const { + return SpatialTransform (E * XT.E, XT.r + XT.E.transpose() * r); + } + + void operator*= (const SpatialTransform &XT) { + r = XT.r + XT.E.transpose() * r; + E *= XT.E; + } + + Matrix3d E; + Vector3d r; +}; + +inline std::ostream& operator<<(std::ostream& output, const SpatialRigidBodyInertia &rbi) { + output << "rbi.m = " << rbi.m << std::endl; + output << "rbi.h = " << rbi.h.transpose(); + output << "rbi.Ixx = " << rbi.Ixx << std::endl; + output << "rbi.Iyx = " << rbi.Iyx << " rbi.Iyy = " << rbi.Iyy << std::endl; + output << "rbi.Izx = " << rbi.Izx << " rbi.Izy = " << rbi.Izy << " rbi.Izz = " << rbi.Izz << std::endl; + return output; +} + +inline std::ostream& operator<<(std::ostream& output, const SpatialTransform &X) { + output << "X.E = " << std::endl << X.E << std::endl; + output << "X.r = " << X.r.transpose(); + return output; +} + +inline SpatialTransform Xrot (double angle_rad, const Vector3d &axis) { + double s, c; + s = sin(angle_rad); + c = cos(angle_rad); + + return SpatialTransform ( + Matrix3d ( + axis[0] * axis[0] * (1.0f - c) + c, + axis[1] * axis[0] * (1.0f - c) + axis[2] * s, + axis[0] * axis[2] * (1.0f - c) - axis[1] * s, + + axis[0] * axis[1] * (1.0f - c) - axis[2] * s, + axis[1] * axis[1] * (1.0f - c) + c, + axis[1] * axis[2] * (1.0f - c) + axis[0] * s, + + axis[0] * axis[2] * (1.0f - c) + axis[1] * s, + axis[1] * axis[2] * (1.0f - c) - axis[0] * s, + axis[2] * axis[2] * (1.0f - c) + c + + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xrotx (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + return SpatialTransform ( + Matrix3d ( + 1., 0., 0., + 0., c, s, + 0., -s, c + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xroty (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + return SpatialTransform ( + Matrix3d ( + c, 0., -s, + 0., 1., 0., + s, 0., c + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xrotz (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + return SpatialTransform ( + Matrix3d ( + c, s, 0., + -s, c, 0., + 0., 0., 1. + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xtrans (const Vector3d &r) { + return SpatialTransform ( + Matrix3d::Identity(3,3), + r + ); +} + +inline SpatialMatrix crossm (const SpatialVector &v) { + return SpatialMatrix ( + 0, -v[2], v[1], 0, 0, 0, + v[2], 0, -v[0], 0, 0, 0, + -v[1], v[0], 0, 0, 0, 0, + 0, -v[5], v[4], 0, -v[2], v[1], + v[5], 0, -v[3], v[2], 0, -v[0], + -v[4], v[3], 0, -v[1], v[0], 0 + ); +} + +inline SpatialVector crossm (const SpatialVector &v1, const SpatialVector &v2) { + return SpatialVector ( + -v1[2] * v2[1] + v1[1] * v2[2], + v1[2] * v2[0] - v1[0] * v2[2], + -v1[1] * v2[0] + v1[0] * v2[1], + -v1[5] * v2[1] + v1[4] * v2[2] - v1[2] * v2[4] + v1[1] * v2[5], + v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5], + -v1[4] * v2[0] + v1[3] * v2[1] - v1[1] * v2[3] + v1[0] * v2[4] + ); +} + +inline SpatialMatrix crossf (const SpatialVector &v) { + return SpatialMatrix ( + 0, -v[2], v[1], 0, -v[5], v[4], + v[2], 0, -v[0], v[5], 0, -v[3], + -v[1], v[0], 0, -v[4], v[3], 0, + 0, 0, 0, 0, -v[2], v[1], + 0, 0, 0, v[2], 0, -v[0], + 0, 0, 0, -v[1], v[0], 0 + ); +} + +inline SpatialVector crossf (const SpatialVector &v1, const SpatialVector &v2) { + return SpatialVector ( + -v1[2] * v2[1] + v1[1] * v2[2] - v1[5] * v2[4] + v1[4] * v2[5], + v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5], + -v1[1] * v2[0] + v1[0] * v2[1] - v1[4] * v2[3] + v1[3] * v2[4], + - v1[2] * v2[4] + v1[1] * v2[5], + + v1[2] * v2[3] - v1[0] * v2[5], + - v1[1] * v2[3] + v1[0] * v2[4] + ); +} + +} /* Math */ + +} /* RigidBodyDynamics */ + +/* RBDL_SPATIALALGEBRAOPERATORS_H*/ +#endif diff --git a/3rdparty/rbdl/include/rbdl/compileassert.h b/3rdparty/rbdl/include/rbdl/compileassert.h new file mode 100644 index 0000000..613006d --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/compileassert.h @@ -0,0 +1,46 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_COMPILE_ASSERT_H +#define RBDL_COMPILE_ASSERT_H + +/* + * This is a simple compile time assertion tool taken from: + * http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx + * written by Abhinaba Basu! + * + * Thanks! + */ + +#ifdef __cplusplus + +#define JOIN( X, Y ) JOIN2(X,Y) +#define JOIN2( X, Y ) X##Y + +namespace custom_static_assert +{ +template struct STATIC_ASSERT_FAILURE; +template <> struct STATIC_ASSERT_FAILURE { enum { value = 1 }; }; + +template struct custom_static_assert_test{}; +} + +#define COMPILE_ASSERT(x) \ + typedef ::custom_static_assert::custom_static_assert_test<\ +sizeof(::custom_static_assert::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\ +JOIN(_custom_static_assert_typedef, __LINE__) + +#else // __cplusplus + +#define COMPILE_ASSERT(x) extern int __dummy[(int)x] + +#endif // __cplusplus + +#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) + +// RBDL_COMPILE_ASSERT_H_ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl.h b/3rdparty/rbdl/include/rbdl/rbdl.h new file mode 100644 index 0000000..22505bc --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl.h @@ -0,0 +1,69 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_H +#define RBDL_H + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +#include "rbdl/Body.h" +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Joint.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Constraints.h" + +#include "rbdl/rbdl_utils.h" + +/** \page api_version_checking_page API Changes + * @{ + * + * This documentation was created for API version 2.2.0. + * + * Here is a list of changes introduced by the different versions and what + * adjustements have to be made to migrate. + * + * \include api_changes.txt + */ + +/** Returns the API version at compile time of the library. */ +RBDL_DLLAPI int rbdl_get_api_version(); + +/** Ensures whether the RBDL library we are linking against is compatible + * with the the version we have from rbdl.h. + * + * To perform the check run: + * \code + * rbdl_check_api_version(API_VERSION); + * \endcode + * + * This function will abort if compatibility is not met or warn if you run + * a version that might not be entirely compatible. + * + * In most cases you want to specify a specific version to ensure you are + * using a compatible version. To do so replace API_VERSION by a + * value of the form 0xAABBCC where AA is the major, BB the minor, and CC + * the patch version in hex-format, e.g: + * + * \code + * rbdl_check_api_version(0x020A0C); + * \endcode + * + * Would abort if the API major version is not 2 (= 0x02), warn if the + * linked minor version is not 10 (= 0x0A). The patch version 12 (= 0x12) + * does not have an influence on compatibility. + */ +RBDL_DLLAPI void rbdl_check_api_version(int version); + +/** Prints version information to standard output */ +RBDL_DLLAPI void rbdl_print_version(); + +/* RBDL_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake b/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake new file mode 100644 index 0000000..2256727 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake @@ -0,0 +1,74 @@ +/* +* RBDL - Rigid Body Dynamics Library +* Copyright (c) 2011-2012 Martin Felis +* +* Licensed under the zlib license. See LICENSE for more details. +*/ + +#ifndef RBDL_CONFIG_H +#define RBDL_CONFIG_H + +#define RBDL_API_VERSION (@RBDL_VERSION_MAJOR@ << 16) + (@RBDL_VERSION_MINOR@ << 8) + @RBDL_VERSION_PATCH@ + +#cmakedefine RBDL_USE_SIMPLE_MATH +#cmakedefine RBDL_ENABLE_LOGGING +#cmakedefine RBDL_BUILD_REVISION "@RBDL_BUILD_REVISION@" +#cmakedefine RBDL_BUILD_TYPE "@RBDL_BUILD_TYPE@" +#cmakedefine RBDL_BUILD_BRANCH "@RBDL_BUILD_BRANCH@" +#cmakedefine RBDL_BUILD_ADDON_LUAMODEL +#cmakedefine RBDL_BUILD_ADDON_URDFREADER +#cmakedefine RBDL_BUILD_STATIC +#cmakedefine RBDL_USE_ROS_URDF_LIBRARY + +/* compatibility defines */ +#ifdef _WIN32 +#define __func__ __FUNCTION__ +#define M_PI 3.1415926535897932384 +#endif + +// Handle portable symbol export. +// Defining manually which symbol should be exported is required +// under Windows whether MinGW or MSVC is used. +// +// The headers then have to be able to work in two different modes: +// - dllexport when one is building the library, +// - dllimport for clients using the library. +// +// On Linux, set the visibility accordingly. If C++ symbol visibility +// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility +# if defined _WIN32 || defined __CYGWIN__ +// On Microsoft Windows, use dllimport and dllexport to tag symbols. +# define RBDL_DLLIMPORT __declspec(dllimport) +# define RBDL_DLLEXPORT __declspec(dllexport) +# define RBDL_DLLLOCAL +# else +// On Linux, for GCC >= 4, tag symbols using GCC extension. +# if __GNUC__ >= 4 +# define RBDL_DLLIMPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLEXPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLLOCAL __attribute__ ((visibility("hidden"))) +# else +// Otherwise (GCC < 4 or another compiler is used), export everything. +# define RBDL_DLLIMPORT +# define RBDL_DLLEXPORT +# define RBDL_DLLLOCAL +# endif // __GNUC__ >= 4 +# endif // defined _WIN32 || defined __CYGWIN__ + +# ifdef RBDL_BUILD_STATIC +// If one is using the library statically, get rid of +// extra information. +# define RBDL_DLLAPI +# define RBDL_LOCAL +# else +// Depending on whether one is building or using the +// library define DLLAPI to import or export. +# ifdef rbdl_EXPORTS +# define RBDL_DLLAPI RBDL_DLLEXPORT +# else +# define RBDL_DLLAPI RBDL_DLLIMPORT +# endif // RBDL_EXPORTS +# define RBDL_LOCAL RBDL_DLLLOCAL +# endif // RBDL_BUILD_STATIC + +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h b/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h new file mode 100644 index 0000000..5d7c83d --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h @@ -0,0 +1,236 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_EIGENMATH_H +#define RBDL_EIGENMATH_H + +/* Exporting templated symbols is tricky when using MSVC. The following lines + * causes the classes in this file not to be explicitly exported. Instead + * they are already implicitly exported. + */ +#if defined(WIN32) && defined(rbdl_EXPORTS) +#define RBDL_TEMPLATE_DLLAPI +#else +#define RBDL_TEMPLATE_DLLAPI RBDL_DLLAPI +#endif + +class RBDL_TEMPLATE_DLLAPI Vector3_t : public Eigen::Vector3d +{ + public: + typedef Eigen::Vector3d Base; + + template + Vector3_t(const Eigen::MatrixBase& other) + : Eigen::Vector3d(other) + {} + + template + Vector3_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Vector3_t() + {} + + EIGEN_STRONG_INLINE Vector3_t( + const double& v0, const double& v1, const double& v2 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2; + } + + void set(const double& v0, const double& v1, const double& v2) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2; + } +}; + +class RBDL_TEMPLATE_DLLAPI Matrix3_t : public Eigen::Matrix3d +{ + public: + typedef Eigen::Matrix3d Base; + + template + Matrix3_t(const Eigen::MatrixBase& other) + : Eigen::Matrix3d(other) + {} + + template + Matrix3_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Matrix3_t() + {} + + EIGEN_STRONG_INLINE Matrix3_t( + const double& m00, const double& m01, const double& m02, + const double& m10, const double& m11, const double& m12, + const double& m20, const double& m21, const double& m22 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, + m10, m11, m12, + m20, m21, m22 + ; + } +}; + +class RBDL_TEMPLATE_DLLAPI Vector4_t : public Eigen::Vector4d +{ + public: + typedef Eigen::Vector4d Base; + + template + Vector4_t(const Eigen::MatrixBase& other) + : Eigen::Vector4d(other) + {} + + template + Vector4_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Vector4_t() + {} + + EIGEN_STRONG_INLINE Vector4_t( + const double& v0, const double& v1, const double& v2, const double& v3 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3; + } + + void set(const double& v0, const double& v1, const double& v2, const double& v3) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3; + } +}; + +class RBDL_TEMPLATE_DLLAPI SpatialVector_t : public Eigen::Matrix +{ + public: + typedef Eigen::Matrix Base; + + template + SpatialVector_t(const Eigen::MatrixBase& other) + : Eigen::Matrix(other) + {} + + template + SpatialVector_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE SpatialVector_t() + {} + + EIGEN_STRONG_INLINE SpatialVector_t( + const double& v0, const double& v1, const double& v2, + const double& v3, const double& v4, const double& v5 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3, v4, v5; + } + + void set( + const double& v0, const double& v1, const double& v2, + const double& v3, const double& v4, const double& v5 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3, v4, v5; + } +}; + +class RBDL_TEMPLATE_DLLAPI SpatialMatrix_t : public Eigen::Matrix +{ + public: + typedef Eigen::Matrix Base; + + template + SpatialMatrix_t(const Eigen::MatrixBase& other) + : Eigen::Matrix(other) + {} + + template + SpatialMatrix_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE SpatialMatrix_t() + {} + + EIGEN_STRONG_INLINE SpatialMatrix_t( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35, + const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, + const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03, m04, m05 + , m10, m11, m12, m13, m14, m15 + , m20, m21, m22, m23, m24, m25 + , m30, m31, m32, m33, m34, m35 + , m40, m41, m42, m43, m44, m45 + , m50, m51, m52, m53, m54, m55 + ; + } + + void set( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35, + const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, + const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03, m04, m05 + , m10, m11, m12, m13, m14, m15 + , m20, m21, m22, m23, m24, m25 + , m30, m31, m32, m33, m34, m35 + , m40, m41, m42, m43, m44, m45 + , m50, m51, m52, m53, m54, m55 + ; + } +}; + +/* _RBDL_EIGENMATH_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_math.h b/3rdparty/rbdl/include/rbdl/rbdl_math.h new file mode 100644 index 0000000..9b72736 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_math.h @@ -0,0 +1,82 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MATH_H +#define RBDL_MATH_H + +#include "rbdl/rbdl_config.h" + +#ifdef RBDL_USE_SIMPLE_MATH +#include "rbdl/SimpleMath/SimpleMathFixed.h" +#include "rbdl/SimpleMath/SimpleMathDynamic.h" +#include "rbdl/SimpleMath/SimpleMathMixed.h" +#include "rbdl/SimpleMath/SimpleMathQR.h" +#include "rbdl/SimpleMath/SimpleMathCholesky.h" +#include "rbdl/SimpleMath/SimpleMathCommaInitializer.h" +#include "rbdl/SimpleMath/SimpleMathMap.h" +#include + +typedef SimpleMath::Fixed::Matrix Vector3_t; +typedef SimpleMath::Fixed::Matrix Matrix3_t; +typedef SimpleMath::Fixed::Matrix Vector4_t; + +typedef SimpleMath::Fixed::Matrix SpatialVector_t; +typedef SimpleMath::Fixed::Matrix SpatialMatrix_t; + +typedef SimpleMath::Fixed::Matrix Matrix63_t; +typedef SimpleMath::Fixed::Matrix Matrix43_t; + +typedef SimpleMath::Dynamic::Matrix MatrixN_t; +typedef SimpleMath::Dynamic::Matrix VectorN_t; + +#else +#include +#include +#include + +#include "rbdl/rbdl_eigenmath.h" + +typedef Eigen::Matrix Matrix63_t; +typedef Eigen::Matrix Matrix43_t; + +typedef Eigen::VectorXd VectorN_t; +typedef Eigen::MatrixXd MatrixN_t; +#endif + +namespace RigidBodyDynamics { + +/** \brief Math types such as vectors and matrices and utility functions. */ +namespace Math { +typedef Vector3_t Vector3d; +typedef Vector4_t Vector4d; +typedef Matrix3_t Matrix3d; +typedef SpatialVector_t SpatialVector; +typedef SpatialMatrix_t SpatialMatrix; +typedef Matrix63_t Matrix63; +typedef Matrix43_t Matrix43; +typedef VectorN_t VectorNd; +typedef MatrixN_t MatrixNd; +} /* Math */ + +} /* RigidBodyDynamics */ + +#include "rbdl/Quaternion.h" +#include "rbdl/SpatialAlgebraOperators.h" + +// If we use Eigen3 we have to create specializations of the STL +// std::vector such that the alignment is done properly. +#ifndef RBDL_USE_SIMPLE_MATH + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix63) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix43) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia) +#endif + + /* RBDL_MATH_H_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h b/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h new file mode 100644 index 0000000..c11398d --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h @@ -0,0 +1,265 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MATHUTILS_H +#define RBDL_MATHUTILS_H + +#include +#include + +#include "rbdl/rbdl_math.h" + +namespace RigidBodyDynamics { +struct Model; + +namespace Math { + +/** \brief Available solver methods for the linear systems. + * + * Please note that these methods are only available when Eigen3 is used. + * When the math library SimpleMath is used it will always use a slow + * column pivoting gauss elimination. + */ +enum RBDL_DLLAPI LinearSolver { + LinearSolverUnknown = 0, + LinearSolverPartialPivLU, + LinearSolverColPivHouseholderQR, + LinearSolverHouseholderQR, + LinearSolverLLT, + LinearSolverLast, +}; + +extern RBDL_DLLAPI Vector3d Vector3dZero; +extern RBDL_DLLAPI Matrix3d Matrix3dIdentity; +extern RBDL_DLLAPI Matrix3d Matrix3dZero; + +RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) { + // TODO: use memory mapping operators for Eigen + VectorNd result (n); + + for (unsigned int i = 0; i < n; i++) { + result[i] = ptr[i]; + } + + return result; +} + +RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major = true) { + MatrixNd result (rows, cols); + + if (row_major) { + for (unsigned int i = 0; i < rows; i++) { + for (unsigned int j = 0; j < cols; j++) { + result(i,j) = ptr[i * cols + j]; + } + } + } else { + for (unsigned int i = 0; i < rows; i++) { + for (unsigned int j = 0; j < cols; j++) { + result(i,j) = ptr[i + j * rows]; + } + } + } + + return result; +} + +/// \brief Solves a linear system using gaussian elimination with pivoting +RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x); + +// \todo write test +RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix); + +RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a, + const SpatialMatrix &matrix_b, double epsilon); +RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a, + const SpatialVector &vector_b, double epsilon); + +/** \brief Translates the inertia matrix to a new center. */ +RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com); + +/** \brief Creates a transformation of a linear displacement + * + * This can be used to specify the translation to the joint center when + * adding a body to a model. See also section 2.8 in RBDA. + * + * \note The transformation returned is for motions. For a transformation for forces + * \note one has to conjugate the matrix. + * + * \param displacement The displacement as a 3D vector + */ +RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement); + +/** \brief Creates a rotational transformation around the Z-axis + * + * Creates a rotation around the current Z-axis by the given angle + * (specified in radians). + * + * \param zrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot); + +/** \brief Creates a rotational transformation around the Y-axis + * + * Creates a rotation around the current Y-axis by the given angle + * (specified in radians). + * + * \param yrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot); + +/** \brief Creates a rotational transformation around the X-axis + * + * Creates a rotation around the current X-axis by the given angle + * (specified in radians). + * + * \param xrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot); + +/** \brief Creates a spatial transformation for given parameters + * + * Creates a transformation to a coordinate system that is first rotated + * and then translated. + * + * \param displacement The displacement to the new origin + * \param zyx_euler The orientation of the new coordinate system, specifyed + * by ZYX-Euler angles. + */ +RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler); + +RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + return Matrix3d ( + 1., 0., 0., + 0., c, s, + 0., -s, c + ); +} + +RBDL_DLLAPI inline Matrix3d roty (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + return Matrix3d ( + c, 0., -s, + 0., 1., 0., + s, 0., c + ); +} + +RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + return Matrix3d ( + c, s, 0., + -s, c, 0., + 0., 0., 1. + ); +} + +RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) { + double s, c; + s = sin (x); + c = cos (x); + return Matrix3d ( + 0., 0., 0., + 0., -s * xdot, c * xdot, + 0., -c * xdot,-s * xdot + ); +} + +RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) { + double s, c; + s = sin (y); + c = cos (y); + return Matrix3d ( + -s * ydot, 0., - c * ydot, + 0., 0., 0., + c * ydot, 0., - s * ydot + ); +} + +RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) { + double s, c; + s = sin (z); + c = cos (z); + return Matrix3d ( + -s * zdot, c * zdot, 0., + -c * zdot, -s * zdot, 0., + 0., 0., 0. + ); +} + +RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) { + double sy = sin(zyx_angles[1]); + double cy = cos(zyx_angles[1]); + double sx = sin(zyx_angles[2]); + double cx = cos(zyx_angles[2]); + + return Vector3d ( + zyx_angle_rates[2] - sy * zyx_angle_rates[0], + cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0], + -sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0] + ); +} + +RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) { + Matrix3d RzT = rotz(zyx_angles[0]).transpose(); + Matrix3d RyT = roty(zyx_angles[1]).transpose(); + + return Vector3d ( + Vector3d (0., 0., zyx_rates[0]) + + RzT * Vector3d (0., zyx_rates[1], 0.) + + RzT * RyT * Vector3d (zyx_rates[2], 0., 0.) + ); +} + +RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) { + double sy = sin(zyx_angles[1]); + double cy = cos(zyx_angles[1]); + double sx = sin(zyx_angles[2]); + double cx = cos(zyx_angles[2]); + double xdot = zyx_angle_rates[2]; + double ydot = zyx_angle_rates[1]; + double zdot = zyx_angle_rates[0]; + double xddot = zyx_angle_rates_dot[2]; + double yddot = zyx_angle_rates_dot[1]; + double zddot = zyx_angle_rates_dot[0]; + + return Vector3d ( + xddot - (cy * ydot * zdot + sy * zddot), + -sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot), + -cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot) + ); +} + +RBDL_DLLAPI +void SparseFactorizeLTL (Model &model, Math::MatrixNd &H); + +RBDL_DLLAPI +void SparseMultiplyHx (Model &model, Math::MatrixNd &L); + +RBDL_DLLAPI +void SparseMultiplyLx (Model &model, Math::MatrixNd &L); +RBDL_DLLAPI +void SparseMultiplyLTx (Model &model, Math::MatrixNd &L); + +RBDL_DLLAPI +void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x); +RBDL_DLLAPI +void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x); + +} /* Math */ + +} /* RigidBodyDynamics */ + +/* RBDL_MATHUTILS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_utils.h b/3rdparty/rbdl/include/rbdl/rbdl_utils.h new file mode 100644 index 0000000..6743fa2 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_utils.h @@ -0,0 +1,54 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_UTILS_H +#define RBDL_UTILS_H + +#include +#include +#include + +namespace RigidBodyDynamics { + +struct Model; + +/** \brief Namespace that contains optional helper functions */ +namespace Utils { +/** \brief Creates a human readable overview of the model. */ +RBDL_DLLAPI std::string GetModelHierarchy (const Model &model); +/** \brief Creates a human readable overview of the Degrees of Freedom. */ +RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model); +/** \brief Creates a human readable overview of the locations of all bodies that have names. */ +RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model); + +/** \brief Computes the Center of Mass (COM) and optionally its linear velocity. + * + * When only interested in computing the location of the COM you can use + * NULL as value for com_velocity. + * + * \param model The model for which we want to compute the COM + * \param q The current joint positions + * \param qdot The current joint velocities + * \param mass (output) total mass of the model + * \param com (output) location of the Center of Mass of the model in base coordinates + * \param com_velocity (optional output) linear velocity of the COM in base coordinates + * \param angular_momentum (optional output) angular momentum of the model at the COM in base coordinates + * \param update_kinematics (optional input) whether the kinematics should be updated (defaults to true) + */ +RBDL_DLLAPI void CalcCenterOfMass (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, double &mass, Math::Vector3d &com, Math::Vector3d *com_velocity = NULL, Math::Vector3d *angular_momentum = NULL, bool update_kinematics = true); + +/** \brief Computes the potential energy of the full model. */ +RBDL_DLLAPI double CalcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics = true); + +/** \brief Computes the kinetic energy of the full model. */ +RBDL_DLLAPI double CalcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics = true); +} + +} + +/* RBDL_UTILS_H */ +#endif diff --git a/3rdparty/rbdl/python/CMakeLists.txt b/3rdparty/rbdl/python/CMakeLists.txt new file mode 100644 index 0000000..7d34106 --- /dev/null +++ b/3rdparty/rbdl/python/CMakeLists.txt @@ -0,0 +1,66 @@ +FIND_PROGRAM ( PYTHON "python" ) + +CMAKE_POLICY(SET CMP0048 NEW) + +SET (Python_ADDITIONAL_VERSIONS 2.7) + +INCLUDE ( UseCython ) + +FILE( COPY "rbdl.pyx" "crbdl.pxd" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}") + +CONFIGURE_FILE ( + ${CMAKE_CURRENT_SOURCE_DIR}/setup.py.cmake + ${CMAKE_CURRENT_BINARY_DIR}/setup.py + ) + +# Process the rbdl-wrapper.pyx to generate rbdl.pyx +ADD_CUSTOM_COMMAND ( OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx + COMMAND ${PYTHON} + ARGS ${CMAKE_CURRENT_SOURCE_DIR}/wrappergen.py + ${CMAKE_CURRENT_SOURCE_DIR}/rbdl-wrapper.pyx + ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx + COMMENT "Generating rbdl.pyx from rbdl-wrapper.pyx" + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/rbdl-wrapper.pyx ${CMAKE_CURRENT_SOURCE_DIR}/wrappergen.py + ) + +# Enable C++11 (or C++0x for older compilers) +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +# If the pyx file is a C++ file, we should specify that here. +set_source_files_properties( ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx + PROPERTIES CYTHON_IS_CXX TRUE ) + +# Multi-file cython modules do not appear to be working at the moment. +cython_add_module( rbdl-python ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx ) + +#SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES PREFIX "") +SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES OUTPUT_NAME "rbdl") + +INCLUDE_DIRECTORIES ( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/python + ${PROJECT_SOURCE_DIR} + ) + +TARGET_LINK_LIBRARIES (rbdl-python rbdl ) + +IF (RBDL_BUILD_ADDON_LUAMODEL) + TARGET_LINK_LIBRARIES (rbdl-python rbdl_luamodel ) +ENDIF() + +IF (RBDL_BUILD_ADDON_URDFREADER) + TARGET_LINK_LIBRARIES (rbdl-python rbdl_urdfreader ) +ENDIF() + +INSTALL ( TARGETS rbdl-python + LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/python2.7/site-packages/ +) diff --git a/3rdparty/rbdl/python/README.md b/3rdparty/rbdl/python/README.md new file mode 100644 index 0000000..06c2d39 --- /dev/null +++ b/3rdparty/rbdl/python/README.md @@ -0,0 +1,53 @@ +# Python wrapper for RBDL + +This wrapper uses Cython to wrap RBDL in a Python module. The code requires +C++11 features and must be compiled with the flags ```-std=c++11``` (or on +older compilers: ```-std=c++0x```). It closely follows the C++ API. All +functions are found in the module ```rbdl```, e.g. + + rbdl.ForwardDynamics (model, q, qdot, tau, qddot) + +computes the accelerations qddot of the forward dynamics for given model, +q, qdot, tau. + +Arguments are all passed as reference and where possible, the wrapper +avoids copying values, especially for larger matrices such as Jacobians, +and joint-space inertia matrix. + +All functions have embedded signatures to ease the use from within IPython, +e.g. ```rbdl.ForwardDynamics?``` shows required arguments for the function. + +# Highlights + +Wrappers for the following features are already implemented: + +* supports model creation from Python code +* supports model loading from LuaModel and URDF +* operates directly on raw numpy data for Jacobians and joint-space inertia + matrix - no copying required! +* direct access to almost all values of the Model structure +* all functions of Dynamics.h are wrapped: + - O(n) inverse dynamics via RNEA + - O(n) forward dynamics via ABA + - Coriolis term computation via simplified RNEA + - computation of joint-space inertia matrix via CRBA +* kinematic computations: + - body <-> world transformations + - body point positions, velocities, and accelerations, including their + 6-D counterparts + - point Jacobians (translations) + - 6-D Jacobians (angular and linear movement) + - Spatial 6-D body Jacobians +* model mass, Center of Mass (CoM), CoM velocity, centroidal angular momentum + +# Differences to the C++ API + +The wrapper function ```rbdl.CalcCenterOfMass``` has a scalar return value +which is the mass of the model. Therefore the function does not use the +mass parameter when calling it. + +# ToDo + +* wrapping of constraint sets, and contact dynamics +* inverse kinematics +* documentation diff --git a/3rdparty/rbdl/python/crbdl.pxd b/3rdparty/rbdl/python/crbdl.pxd new file mode 100644 index 0000000..15c1f93 --- /dev/null +++ b/3rdparty/rbdl/python/crbdl.pxd @@ -0,0 +1,412 @@ +#cython: boundscheck=False + +from libcpp.string cimport string +from libcpp cimport bool +from libcpp.vector cimport vector + +cdef extern from "" namespace "RigidBodyDynamics::Math": + cdef cppclass VectorNd: + VectorNd () + VectorNd (int dim) + int rows() + int cols() + void resize (int) + double& operator[](int) + double* data() + + cdef cppclass Vector3d: + Vector3d () + int rows() + int cols() + double& operator[](int) + double* data() + + cdef cppclass Quaternion: + Quaternion () + int rows() + int cols() + double& operator[](int) + double* data() + Matrix3d toMatrix() +# Quaternion fromMatrix (Matrix3d &mat) + + cdef cppclass SpatialVector: + SpatialVector () + int rows() + int cols() + double& operator[](int) + double* data() + + cdef cppclass Matrix3d: + Matrix3d () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + + cdef cppclass MatrixNd: + MatrixNd () + MatrixNd (int rows, int cols) + int rows() + int cols() + void resize (int,int) + double& coeff "operator()"(int,int) + double* data() + void setZero() + + cdef cppclass SpatialMatrix: + SpatialMatrix () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + + cdef cppclass Matrix63: + Matrix63 () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + +cdef extern from "" namespace "RigidBodyDynamics::Math::Quaternion": + Quaternion fromMatrix(const Matrix3d &mat) + +cdef extern from "" namespace "RigidBodyDynamics::Math": + cdef cppclass SpatialTransform: + SpatialTransform() + SpatialMatrix toMatrix() + SpatialTransform inverse() + SpatialTransform operator*(const SpatialTransform&) + Matrix3d E + Vector3d r + + cdef cppclass SpatialRigidBodyInertia: + SpatialRigidBodyInertia() + SpatialMatrix toMatrix() + + double m + Vector3d h + double Ixx, Iyx, Iyy, Izx, Izy, Izz + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Body: + Body() + Body(const double mass, const Vector3d &com, const Matrix3d &inertia) + double mMass + Vector3d mCenterOfMass + Matrix3d mInertia + bool mIsVirtual + + cdef cppclass FixedBody: + FixedBody() + double mMass + Vector3d mCenterOfMass + Matrix3d mInertia + unsigned int mMovableParent + SpatialTransform mParentTransform + SpatialTransform mBaseTransform + bool mIsVirtual + + +cdef extern from "" namespace "RigidBodyDynamics": + cdef enum JointType: + JointTypeUndefined = 0 + JointTypeRevolute + JointTypePrismatic + JointTypeRevoluteX + JointTypeRevoluteY + JointTypeRevoluteZ + JointTypeSpherical + JointTypeEulerZYX + JointTypeEulerXYZ + JointTypeEulerYXZ + JointTypeTranslationXYZ + JointTypeFloatingBase + JointTypeFixed + JointType1DoF + JointType2DoF + JointType3DoF + JointType4DoF + JointType5DoF + JointType6DoF + JointTypeCustom + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Joint: + Joint() + Joint(JointType joint_type) + SpatialVector* mJointAxes + JointType mJointType + unsigned int mDoFCount + unsigned int q_index + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Model: + Model() + unsigned int AddBody (const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + string body_name + ) + unsigned int AppendBody (const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + string body_name + ) + unsigned int GetParentBodyId( + unsigned int body_id) + unsigned int GetBodyId( + const char *body_name) + string GetBodyName ( + unsigned int body_id) + bool IsBodyId ( + unsigned int body_id) + bool IsFixedBodyId ( + unsigned int body_id) + Quaternion GetQuaternion ( + unsigned int body_id, + const VectorNd &q) + void SetQuaternion ( + unsigned int body_id, + const Quaternion &quat, + VectorNd &q) + + vector[unsigned int] _lambda + vector[unsigned int] lambda_q +# vector[vector[unsigned int]] mu + + unsigned int dof_count + unsigned int q_size + unsigned int qdot_size + unsigned int previously_added_body_id + + Vector3d gravity + vector[SpatialVector] v + vector[SpatialVector] a + + vector[Joint] mJoints + vector[SpatialVector] S + vector[SpatialTransform] X_J + vector[SpatialVector] v_J + vector[SpatialVector] c_J + + vector[unsigned int] mJointUpdateOrder + + vector[SpatialTransform] X_T + + vector[unsigned int] mFixedJointCount + + vector[Matrix63] multdof3_S + vector[Matrix63] multdof3_U + vector[Matrix63] multdof3_Dinv + vector[Matrix63] multdof3_u + vector[unsigned int] multdof3_w_index + + vector[SpatialVector] c + vector[SpatialMatrix] IA + vector[SpatialVector] pA + vector[SpatialVector] U + VectorNd d + VectorNd u + vector[SpatialVector] f + vector[SpatialRigidBodyInertia] I + vector[SpatialRigidBodyInertia] Ic + vector[SpatialVector] hc + + vector[SpatialTransform] X_lambda + vector[SpatialTransform] X_base + + vector[FixedBody] mFixedBodies + unsigned int fixed_body_discriminator + + vector[Body] mBodies + +cdef extern from "" namespace "RigidBodyDynamics": + cdef void UpdateKinematics (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot) + + cdef Vector3d CalcBodyToBaseCoordinates (Model& model, + const VectorNd &q, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcBaseToBodyCoordinates (Model& model, + const VectorNd &q, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcPointVelocity (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcPointAcceleration (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef SpatialVector CalcPointVelocity6D (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef SpatialVector CalcPointAcceleration6D (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + +cdef extern from "" namespace "RigidBodyDynamics::Utils": + cdef void CalcCenterOfMass (Model& model, + const VectorNd &q, + const VectorNd &qdot, + double &mass, + Vector3d &com, + Vector3d *com_velocity, + Vector3d *angular_momentum, + bool update_kinematics) + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass ConstraintSet: + ConstraintSet() + unsigned int AddContactConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &world_normal, + const char* constraint_name, + double normal_acceleration) + + unsigned int AddLoopConstraint ( + unsigned int id_predecessor, + unsigned int id_successor, + const SpatialTransform &X_predecessor, + const SpatialTransform &X_successor, + const SpatialVector &axis, + double T_stab_inv, + const char *constraint_name) + + ConstraintSet Copy() + # void SetSolver (Math::LinearSolver solver) + bool Bind (const Model &model) + + size_t size() + void clear() + # Math::LinearSolver + bool bound + + vector[string] name + vector[unsigned int] body + vector[Vector3d] point + vector[Vector3d] normal + + VectorNd acceleration + VectorNd force + VectorNd impulse + VectorNd v_plus + + MatrixNd H + VectorNd C + VectorNd gamma + VectorNd G + + MatrixNd A + VectorNd b + VectorNd x + + MatrixNd GT_qr_Q + MatrixNd Y + MatrixNd Z + VectorNd qddot_y + VectorNd qddot_z + + MatrixNd K + VectorNd a + VectorNd QDDot_t + VectorNd QDDot_0 + + vector[SpatialVector] f_t + vector[SpatialVector] f_ext_constraints + vector[Vector3d] point_accel_0 + + vector[SpatialVector] d_pA + vector[SpatialVector] d_a + VectorNd d_u + + vector[SpatialMatrix] d_IA + vector[SpatialVector] d_U + + VectorNd d_d + vector[Vector3d] d_multdof3_u + +cdef extern from "rbdl_ptr_functions.h" namespace "RigidBodyDynamics": + cdef void CalcPointJacobianPtr (Model& model, + const double *q_ptr, + unsigned int body_id, + const Vector3d &point_position, + double *G, + bool update_kinematics) + + cdef void CalcPointJacobian6DPtr (Model &model, + const double *q_ptr, + unsigned int body_id, + const Vector3d &point_position, + double *G, + bool update_kinematics) + + cdef void CalcBodySpatialJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + double *G, + bool update_kinematics) + + cdef void InverseDynamicsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + const double* qddot_ptr, + double* tau_ptr, + vector[SpatialVector] *f_ext + ) + + cdef void NonlinearEffectsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + double* tau_ptr + ) + + cdef void CompositeRigidBodyAlgorithmPtr (Model& model, + const double *q, + double *H, + bool update_kinematics) + + cdef void ForwardDynamicsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + double* tau_ptr, + const double* qddot_ptr, + vector[SpatialVector] *f_ext + ) + +cdef extern from "rbdl_loadmodel.cc": + cdef bool rbdl_loadmodel ( + const char* filename, + Model* model, + bool floating_base, + bool verbose) diff --git a/3rdparty/rbdl/python/rbdl-wrapper.pyx b/3rdparty/rbdl/python/rbdl-wrapper.pyx new file mode 100644 index 0000000..879336d --- /dev/null +++ b/3rdparty/rbdl/python/rbdl-wrapper.pyx @@ -0,0 +1,1393 @@ +#cython: boundscheck=False, embedsignature=True + +import numpy as np +cimport numpy as np +from libc.stdint cimport uintptr_t +from libcpp.string cimport string + +cimport crbdl + +############################## +# +# Linear Algebra Types +# +############################## + +cdef class Vector3d: + cdef crbdl.Vector3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Vector3d() + + if pyvalues != None: + for i in range (3): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Vector3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Vector3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Vector3d (0, python_values) + +cdef class Matrix3d: + cdef crbdl.Matrix3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Matrix3d() + + if pyvalues != None: + for i in range (3): + for j in range (3): + (&(self.thisptr.coeff(i,j)))[0] = pyvalues[i,j] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Matrix3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Matrix3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Matrix3d (0, python_values) + + +cdef class VectorNd: + cdef crbdl.VectorNd *thisptr + cdef free_on_dealloc + + def __cinit__(self, ndim, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.VectorNd(ndim) + + if pyvalues != None: + for i in range (ndim): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return self.thisptr.rows() + + def toNumpy (self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPythonArray (cls, python_values): + return VectorNd (len(python_values), 0, python_values) + + @classmethod + def fromPointer(cls, uintptr_t address): + cdef crbdl.VectorNd* vector_ptr = address + return VectorNd (vector_ptr.rows(), address) + +cdef class Quaternion: + cdef crbdl.Quaternion *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None, pymatvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Quaternion() + + if pyvalues != None: + for i in range (4): + self.thisptr.data()[i] = pyvalues[i] + elif pymatvalues != None: + mat = Matrix3d() + for i in range (3): + for j in range (3): + (&(mat.thisptr.coeff(i,j)))[0] = pymatvalues[i,j] + self.thisptr[0] = crbdl.fromMatrix (mat.thisptr[0]) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Quaternion [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], + self.thisptr.data()[2], self.thisptr.data()[3]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 4 + + def toMatrix(self): + cdef crbdl.Matrix3d mat + mat = self.thisptr.toMatrix() + result = np.array ([3,3]) + for i in range (3): + for j in range (3): + result[i,j] = mat.coeff(i,j) + + return result + + def toNumpy(self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Quaternion (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Quaternion (0, python_values) + + @classmethod + def fromPythonMatrix (cls, python_matrix_values): + return Quaternion (0, None, python_matrix_values) + +cdef class SpatialVector: + cdef crbdl.SpatialVector *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialVector() + + if pyvalues != None: + for i in range (6): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialVector [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialVector (address) + + @classmethod + def fromPythonArray (cls, python_values): + return SpatialVector (0, python_values) + +cdef class SpatialMatrix: + cdef crbdl.SpatialMatrix *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialMatrix() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialMatrix [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialMatrix (address) + +############################## +# +# Conversion Numpy <-> Eigen +# +############################## + +# Vector3d +cdef crbdl.Vector3d NumpyToVector3d (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Vector3d cx = crbdl.Vector3d() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray Vector3dToNumpy (crbdl.Vector3d cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +# VectorNd +cdef crbdl.VectorNd NumpyToVectorNd (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.VectorNd cx = crbdl.VectorNd(x.shape[0]) + for i in range (x.shape[0]): + cx[i] = x[i] + + return cx + +cdef np.ndarray VectorNdToNumpy (crbdl.VectorNd cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +# MatrixNd +cdef crbdl.MatrixNd NumpyToMatrixNd (np.ndarray[double, ndim=2, mode="c"] M): + cdef crbdl.MatrixNd cM = crbdl.MatrixNd(M.shape[0], M.shape[1]) + for i in range (M.shape[0]): + for j in range (M.shape[1]): + (&(cM.coeff(i,j)))[0] = M[i,j] + + return cM + +cdef np.ndarray MatrixNdToNumpy (crbdl.MatrixNd cM): + result = np.ndarray ([cM.rows(), cM.cols()]) + for i in range (cM.rows()): + for j in range (cM.cols()): + result[i,j] = cM.coeff(i,j) + + return result + +# SpatialVector +cdef np.ndarray SpatialVectorToNumpy (crbdl.SpatialVector cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +cdef crbdl.Quaternion NumpyToQuaternion (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Quaternion cx = crbdl.Quaternion() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray QuaternionToNumpy (crbdl.Quaternion cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +############################## +# +# Spatial Algebra Types +# +############################## + +cdef class SpatialTransform: + cdef crbdl.SpatialTransform *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialTransform() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialTransform E = [ [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}] ], r = [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.E.coeff(0,0), self.thisptr.E.coeff(0,1), self.thisptr.E.coeff(0,2), + self.thisptr.E.coeff(1,0), self.thisptr.E.coeff(1,1), self.thisptr.E.coeff(1,2), + self.thisptr.E.coeff(2,0), self.thisptr.E.coeff(2,1), self.thisptr.E.coeff(2,2), + self.thisptr.r[0], self.thisptr.r[1], self.thisptr.r[2]) + + property E: + """ Rotational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.E.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.E.coeff(i,j)))[0] = value[i,j] + + property r: + """ Translational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.r[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.r[i]))[0] = value[i] + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialTransform (address) + +cdef class SpatialRigidBodyInertia: + cdef crbdl.SpatialRigidBodyInertia *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialRigidBodyInertia() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.SpatialRigidBodyInertia (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialRigidBodyInertia (address) + + property m: + def __get__ (self): + return self.thisptr.m + + def __set__ (self, value): + self.thisptr.m = value + + property h: + """ Translational part of the SpatialRigidBodyInertia. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.h[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.h[i]))[0] = value[i] + + property Ixx: + def __get__ (self): + return self.thisptr.Ixx + + def __set__ (self, value): + self.thisptr.Ixx = value + + property Iyx: + def __get__ (self): + return self.thisptr.Iyx + + def __set__ (self, value): + self.thisptr.Iyx = value + + property Iyy: + def __get__ (self): + return self.thisptr.Iyy + + def __set__ (self, value): + self.thisptr.Iyy = value + + property Izx: + def __get__ (self): + return self.thisptr.Izx + + def __set__ (self, value): + self.thisptr.Izx = value + + property Izy: + def __get__ (self): + return self.thisptr.Izy + + def __set__ (self, value): + self.thisptr.Izy = value + + property Izz: + def __get__ (self): + return self.thisptr.Izz + + def __set__ (self, value): + self.thisptr.Izz = value + +############################## +# +# Rigid Multibody Types +# +############################## + +cdef class Body: + cdef crbdl.Body *thisptr + cdef free_on_dealloc + + def __cinit__(self, **kwargs): + cdef double c_mass + cdef crbdl.Vector3d c_com + cdef crbdl.Matrix3d c_inertia + cdef uintptr_t address=0 + + if "address" in kwargs.keys(): + address=kwargs["address"] + mass = None + if "mass" in kwargs.keys(): + mass=kwargs["mass"] + com = None + if "com" in kwargs.keys(): + com=kwargs["com"] + inertia = None + if "inertia" in kwargs.keys(): + inertia=kwargs["inertia"] + + if address == 0: + self.free_on_dealloc = True + if (mass != None) and (com != None) and (inertia != None): + c_mass = mass + + for i in range (3): + c_com[i] = com[i] + + for i in range (3): + for j in range (3): + (&(c_inertia.coeff(i,j)))[0] = inertia[i,j] + + self.thisptr = new crbdl.Body(c_mass, c_com, c_inertia) + else: + self.thisptr = new crbdl.Body() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.Body (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Body (address=address) + + @classmethod + def fromMassComInertia(cls, double mass, + np.ndarray[double, ndim=1] com, + np.ndarray[double, ndim=2] inertia): + + return Body (address=0, mass=mass, com=com, inertia=inertia) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + + property mIsVirtual: + def __get__ (self): + return self.thisptr.mIsVirtual + + def __set__ (self, value): + self.thisptr.mIsVirtual = value + +cdef class FixedBody: + cdef crbdl.FixedBody *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.FixedBody() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.FixedBody (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return FixedBody (address) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + +cdef enum JointType: + JointTypeUndefined = 0 + JointTypeRevolute + JointTypePrismatic + JointTypeRevoluteX + JointTypeRevoluteY + JointTypeRevoluteZ + JointTypeSpherical + JointTypeEulerZYX + JointTypeEulerXYZ + JointTypeEulerYXZ + JointTypeTranslationXYZ + JointTypeFloatingBase + JointTypeFixed + JointType1DoF + JointType2DoF + JointType3DoF + JointType4DoF + JointType5DoF + JointType6DoF + JointTypeCustom + +cdef class Joint: + cdef crbdl.Joint *thisptr + cdef free_on_dealloc + + joint_type_map = { + JointTypeUndefined: "JointTypeUndefined", + JointTypeRevolute: "JointTypeRevolute", + JointTypePrismatic: "JointTypePrismatic", + JointTypeRevoluteX: "JointTypeRevoluteX", + JointTypeRevoluteY: "JointTypeRevoluteY", + JointTypeRevoluteZ: "JointTypeRevoluteZ", + JointTypeSpherical: "JointTypeSpherical", + JointTypeEulerZYX: "JointTypeEulerZYX", + JointTypeEulerXYZ: "JointTypeEulerXYZ", + JointTypeEulerYXZ: "JointTypeEulerYXZ", + JointTypeTranslationXYZ: "JointTypeTranslationXYZ", + JointTypeFloatingBase: "JointTypeFloatingBase", + JointTypeFixed: "JointTypeFixed", + JointType1DoF: "JointType1DoF", + JointType2DoF: "JointType2DoF", + JointType3DoF: "JointType3DoF", + JointType4DoF: "JointType4DoF", + JointType5DoF: "JointType5DoF", + JointType6DoF: "JointType6DoF", + JointTypeCustom: "JointTypeCustom", + } + + def _joint_type_from_str (self, joint_type_str): + if joint_type_str not in self.joint_type_map.values(): + raise ValueError("Invalid JointType '" + str(joint_type_str) + "'!") + else: + for joint_type, joint_str in self.joint_type_map.iteritems(): + if joint_str == joint_type_str: + return joint_type + + def __cinit__(self, uintptr_t address=0, joint_type=-1): + if address == 0: + self.free_on_dealloc = True + if joint_type == -1: + self.thisptr = new crbdl.Joint() + else: + self.thisptr = new crbdl.Joint(self._joint_type_from_str(joint_type)) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + joint_type_str = "JointTypeUndefined" + + if self.thisptr.mJointType in self.joint_type_map.keys(): + joint_type_str = self.joint_type_map[self.thisptr.mJointType] + + return "rbdl.Joint (0x{:0x}), JointType: {:s}".format( self.thisptr, joint_type_str) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Joint (address) + + @classmethod + def fromJointType(cls, joint_type): + return Joint (0, joint_type) + + @classmethod + def fromJointAxes(cls, axes): + assert (len(axes) > 0) + assert (len(axes[0]) == 6) + axes_count = len(axes) + joint_type = JointType1DoF + axes_count - 1 + + result = Joint (0, cls.joint_type_map[joint_type]) + + for i in range (axes_count): + result.setJointAxis(i, axes[i]) + + return result + + property mDoFCount: + def __get__ (self): + return self.thisptr.mDoFCount + + def __set__ (self, value): + self.thisptr.mDoFCount = value + + property mJointType: + def __get__ (self): + return self.joint_type_map[self.thisptr.mJointType] + + property q_index: + def __get__ (self): + return self.thisptr.q_index + + def getJointAxis (self, index): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + return SpatialVectorToNumpy (self.thisptr.mJointAxes[index]) + + def setJointAxis (self, index, value): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + for i in range (6): + (&(self.thisptr.mJointAxes[index][i]))[0] = value[i] + self.thisptr.mJointAxes[index][i] + +cdef class Model + +%VectorWrapperClassDefinitions(PARENT=Model)% + +cdef class Model: + cdef crbdl.Model *thisptr + %VectorWrapperMemberDefinitions (PARENT=Model)% + + def __cinit__(self): + self.thisptr = new crbdl.Model() + %VectorWrapperCInitCode (PARENT=Model)% + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.Model (0x{:0x})".format( self.thisptr) + + def AddBody (self, + parent_id, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = ""): + return self.thisptr.AddBody ( + parent_id, + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def AppendBody (self, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = ""): + return self.thisptr.AppendBody ( + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def SetQuaternion (self, + int body_id, + np.ndarray[double, ndim=1, mode="c"] quat, + np.ndarray[double, ndim=1, mode="c"] q): + quat_wrap = Quaternion.fromPythonArray (quat) + q_wrap = VectorNd.fromPythonArray (q) + self.thisptr.SetQuaternion (body_id, + (quat_wrap).thisptr[0], + (q_wrap).thisptr[0]) + for i in range(len(q)): + q[i] = q_wrap[i] + + def GetQuaternion (self, + int body_id, + np.ndarray[double, ndim=1, mode="c"] q): + return QuaternionToNumpy (self.thisptr.GetQuaternion(body_id, NumpyToVectorNd (q))) + + def GetBody (self, index): + return Body (address= &(self.thisptr.mBodies[index])) + + def GetParentBodyId (self, index): + return self.thisptr.GetParentBodyId(index) + + def GetBodyId (self, name): + return self.thisptr.GetBodyId(name) + + def GetBodyName (self, index): + return self.thisptr.GetBodyName(index) + + def IsBodyId (self, index): + return self.thisptr.IsBodyId(index) + + def IsFixedBodyId (self, index): + return self.thisptr.IsFixedBodyId(index) + + property dof_count: + def __get__ (self): + return self.thisptr.dof_count + + property q_size: + def __get__ (self): + return self.thisptr.q_size + + property qdot_size: + def __get__ (self): + return self.thisptr.qdot_size + + property previously_added_body_id: + def __get__ (self): + return self.thisptr.previously_added_body_id + + property gravity: + def __get__ (self): + return np.array ([ + self.thisptr.gravity[0], + self.thisptr.gravity[1], + self.thisptr.gravity[2] + ] + ) + def __set__ (self, values): + for i in range (0,3): + self.thisptr.gravity[i] = values[i] + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=a, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=Joint, MEMBER=mJoints, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=S, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_J, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v_J, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c_J, PARENT=Model)% + + property mJointUpdateOrder: + def __get__ (self): + return self.thisptr.mJointUpdateOrder + + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_T, PARENT=Model)% + + property mFixedJointCount: + def __get__ (self): + return self.thisptr.mFixedJointCount + + # TODO + # multdof3_S + # multdof3_U + # multdof3_Dinv + # multdof3_u + + property multdof3_w_index: + def __get__ (self): + return self.thisptr.multdof3_w_index + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialMatrix, MEMBER=IA, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=pA, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=U, PARENT=Model)% + + # TODO + # d + # u + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=f, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=I, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=Ic, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=hc, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_lambda, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_base, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=FixedBody, MEMBER=mFixedBodies, PARENT=Model)% + + property fixed_body_discriminator: + def __get__ (self): + return self.thisptr.fixed_body_discriminator + + %VectorWrapperAddProperty (TYPE=Body, MEMBER=mBodies, PARENT=Model)% + +############################## +# +# Constraint Types +# +############################## + +cdef class ConstraintSet + +%VectorWrapperClassDefinitions(PARENT=ConstraintSet)% + +cdef class ConstraintSet: + cdef crbdl.ConstraintSet *thisptr + %VectorWrapperMemberDefinitions (PARENT=ConstraintSet)% + + def __cinit__(self): + self.thisptr = new crbdl.ConstraintSet() + %VectorWrapperCInitCode (PARENT=ConstraintSet)% + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.ConstraintSet (0x{:0x})".format( self.thisptr) + + def AddContactConstraint (self, + body_id not None, + body_point not None, + world_normal not None, + constraint_name = None, + normal_acceleration = 0.): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Vector3d c_world_normal + cdef char* constraint_name_ptr + + for i in range (3): + c_body_point[i] = body_point[i] + c_world_normal[i] = world_normal[i] + + if constraint_name == None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddContactConstraint ( + body_id, + c_body_point, + c_world_normal, + constraint_name_ptr, + normal_acceleration + ) + + def AddLoopConstraint (self, + id_predecessor not None, + id_successor not None, + SpatialTransform X_predecessor not None, + SpatialTransform X_successor not None, + SpatialVector axis not None, + double T_stab_inv, + constraint_name = None): + cdef char* constraint_name_ptr + + if constraint_name == None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddLoopConstraint ( + id_predecessor, + id_successor, + X_predecessor.thisptr[0], + X_successor.thisptr[0], + axis.thisptr[0], + T_stab_inv, + constraint_name_ptr) + + def Bind (self, model): + return self.thisptr.Bind ((model).thisptr[0]) + + def size (self): + return self.thisptr.size() + + def clear (self): + self.thisptr.clear() + + property bound: + def __get__ (self): + return self.thisptr.bound + +# %VectorWrapperAddProperty (TYPE=string, MEMBER=name, PARENT=ConstraintSet)% + + %VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=point, PARENT=ConstraintSet)% + %VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=normal, PARENT=ConstraintSet)% + +# property acceleration: +# def __get__(self): +# return VectorNd.fromPointer ( &(self.thisptr.acceleration)).toNumpy() +# def __set__(self, values): +# vec = VectorNd.fromPythonArray (values) +# self.thisptr.acceleration = (vec.thisptr[0]) + +############################## +# +# Kinematics.h +# +############################## + +def CalcBodyToBaseCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBodyToBaseCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcBaseToBodyCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBaseToBodyCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointVelocity (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointVelocity ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointAcceleration ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointVelocity6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointVelocity6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointAcceleration6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointJacobian (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobianPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcPointJacobian6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobian6DPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcBodySpatialJacobian(Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcBodySpatialJacobianPtr( + model.thisptr[0], + q.data, + body_id, + G.data, + update_kinematics + ) + +############################## +# +# rbdl_utils.h +# +############################## + +def CalcCenterOfMass (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] com, + np.ndarray[double, ndim=1, mode="c"] com_velocity=None, + np.ndarray[double, ndim=1, mode="c"] angular_momentum=None, + update_kinematics=True): + cdef double cmass + cdef crbdl.Vector3d c_com = crbdl.Vector3d() + cdef crbdl.Vector3d* c_com_vel_ptr # = crbdl.Vector3d() + cdef crbdl.Vector3d* c_ang_momentum_ptr # = crbdl.Vector3d() + + c_com_vel_ptr = NULL + c_ang_momentum_ptr = NULL + + if com_velocity != None: + c_com_vel_ptr = new crbdl.Vector3d() + + if angular_momentum != None: + c_ang_momentum_ptr = new crbdl.Vector3d() + + cmass = 0.0 + crbdl.CalcCenterOfMass ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + cmass, + c_com, + c_com_vel_ptr, + c_ang_momentum_ptr, + update_kinematics) + + com[0] = c_com[0] + com[1] = c_com[1] + com[2] = c_com[2] + + if com_velocity != None: + com_velocity[0] = c_com_vel_ptr.data()[0] + com_velocity[1] = c_com_vel_ptr.data()[1] + com_velocity[2] = c_com_vel_ptr.data()[2] + del c_com_vel_ptr + + if angular_momentum != None: + angular_momentum[0] = c_ang_momentum_ptr.data()[0] + angular_momentum[1] = c_ang_momentum_ptr.data()[1] + angular_momentum[2] = c_ang_momentum_ptr.data()[2] + del c_ang_momentum_ptr + + return cmass + +############################## +# +# Dynamics.h +# +############################## + +def InverseDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.InverseDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + qddot.data, + tau.data, + NULL + ) + +def NonlinearEffects (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.NonlinearEffectsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data + ) + +def CompositeRigidBodyAlgorithm (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=2, mode="c"] H, + update_kinematics=True): + crbdl.CompositeRigidBodyAlgorithmPtr (model.thisptr[0], + q.data, + H.data, + update_kinematics); + +def ForwardDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau, + np.ndarray[double, ndim=1, mode="c"] qddot): + crbdl.ForwardDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data, + qddot.data, + NULL + ) + +def loadModel ( + filename, + **kwargs + ): + verbose = False + if "verbose" in kwargs.keys(): + verbose=kwargs["verbose"] + + floating_base = False + if "floating_base" in kwargs.keys(): + floating_base=kwargs["floating_base"] + + result = Model() + if crbdl.rbdl_loadmodel (filename, result.thisptr, floating_base, verbose): + return result + + print ("Error loading model {}!".format (filename)) + return None diff --git a/3rdparty/rbdl/python/rbdl.pyx b/3rdparty/rbdl/python/rbdl.pyx new file mode 100644 index 0000000..16332d1 --- /dev/null +++ b/3rdparty/rbdl/python/rbdl.pyx @@ -0,0 +1,2119 @@ +# WARNING! +# +# This file was automatically created from rbdl-wrapper.pyx using wrappergen.py. +# Do not modify this file directly. Edit original source instead!! + +#cython: boundscheck=False, embedsignature=True + +import numpy as np +cimport numpy as np +from libc.stdint cimport uintptr_t +from libcpp.string cimport string + +cimport crbdl + +############################## +# +# Linear Algebra Types +# +############################## + +cdef class Vector3d: + cdef crbdl.Vector3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Vector3d() + + if pyvalues != None: + for i in range (3): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Vector3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Vector3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Vector3d (0, python_values) + +cdef class Matrix3d: + cdef crbdl.Matrix3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Matrix3d() + + if pyvalues != None: + for i in range (3): + for j in range (3): + (&(self.thisptr.coeff(i,j)))[0] = pyvalues[i,j] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Matrix3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Matrix3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Matrix3d (0, python_values) + + +cdef class VectorNd: + cdef crbdl.VectorNd *thisptr + cdef free_on_dealloc + + def __cinit__(self, ndim, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.VectorNd(ndim) + + if pyvalues != None: + for i in range (ndim): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return self.thisptr.rows() + + def toNumpy (self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPythonArray (cls, python_values): + return VectorNd (len(python_values), 0, python_values) + + @classmethod + def fromPointer(cls, uintptr_t address): + cdef crbdl.VectorNd* vector_ptr = address + return VectorNd (vector_ptr.rows(), address) + +cdef class Quaternion: + cdef crbdl.Quaternion *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None, pymatvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Quaternion() + + if pyvalues != None: + for i in range (4): + self.thisptr.data()[i] = pyvalues[i] + elif pymatvalues != None: + mat = Matrix3d() + for i in range (3): + for j in range (3): + (&(mat.thisptr.coeff(i,j)))[0] = pymatvalues[i,j] + self.thisptr[0] = crbdl.fromMatrix (mat.thisptr[0]) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Quaternion [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], + self.thisptr.data()[2], self.thisptr.data()[3]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 4 + + def toMatrix(self): + cdef crbdl.Matrix3d mat + mat = self.thisptr.toMatrix() + result = np.array ([3,3]) + for i in range (3): + for j in range (3): + result[i,j] = mat.coeff(i,j) + + return result + + def toNumpy(self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Quaternion (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Quaternion (0, python_values) + + @classmethod + def fromPythonMatrix (cls, python_matrix_values): + return Quaternion (0, None, python_matrix_values) + +cdef class SpatialVector: + cdef crbdl.SpatialVector *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialVector() + + if pyvalues != None: + for i in range (6): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialVector [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialVector (address) + + @classmethod + def fromPythonArray (cls, python_values): + return SpatialVector (0, python_values) + +cdef class SpatialMatrix: + cdef crbdl.SpatialMatrix *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialMatrix() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialMatrix [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialMatrix (address) + +############################## +# +# Conversion Numpy <-> Eigen +# +############################## + +# Vector3d +cdef crbdl.Vector3d NumpyToVector3d (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Vector3d cx = crbdl.Vector3d() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray Vector3dToNumpy (crbdl.Vector3d cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +# VectorNd +cdef crbdl.VectorNd NumpyToVectorNd (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.VectorNd cx = crbdl.VectorNd(x.shape[0]) + for i in range (x.shape[0]): + cx[i] = x[i] + + return cx + +cdef np.ndarray VectorNdToNumpy (crbdl.VectorNd cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +# MatrixNd +cdef crbdl.MatrixNd NumpyToMatrixNd (np.ndarray[double, ndim=2, mode="c"] M): + cdef crbdl.MatrixNd cM = crbdl.MatrixNd(M.shape[0], M.shape[1]) + for i in range (M.shape[0]): + for j in range (M.shape[1]): + (&(cM.coeff(i,j)))[0] = M[i,j] + + return cM + +cdef np.ndarray MatrixNdToNumpy (crbdl.MatrixNd cM): + result = np.ndarray ([cM.rows(), cM.cols()]) + for i in range (cM.rows()): + for j in range (cM.cols()): + result[i,j] = cM.coeff(i,j) + + return result + +# SpatialVector +cdef np.ndarray SpatialVectorToNumpy (crbdl.SpatialVector cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +cdef crbdl.Quaternion NumpyToQuaternion (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Quaternion cx = crbdl.Quaternion() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray QuaternionToNumpy (crbdl.Quaternion cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +############################## +# +# Spatial Algebra Types +# +############################## + +cdef class SpatialTransform: + cdef crbdl.SpatialTransform *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialTransform() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialTransform E = [ [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}] ], r = [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.E.coeff(0,0), self.thisptr.E.coeff(0,1), self.thisptr.E.coeff(0,2), + self.thisptr.E.coeff(1,0), self.thisptr.E.coeff(1,1), self.thisptr.E.coeff(1,2), + self.thisptr.E.coeff(2,0), self.thisptr.E.coeff(2,1), self.thisptr.E.coeff(2,2), + self.thisptr.r[0], self.thisptr.r[1], self.thisptr.r[2]) + + property E: + """ Rotational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.E.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.E.coeff(i,j)))[0] = value[i,j] + + property r: + """ Translational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.r[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.r[i]))[0] = value[i] + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialTransform (address) + +cdef class SpatialRigidBodyInertia: + cdef crbdl.SpatialRigidBodyInertia *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialRigidBodyInertia() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.SpatialRigidBodyInertia (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialRigidBodyInertia (address) + + property m: + def __get__ (self): + return self.thisptr.m + + def __set__ (self, value): + self.thisptr.m = value + + property h: + """ Translational part of the SpatialRigidBodyInertia. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.h[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.h[i]))[0] = value[i] + + property Ixx: + def __get__ (self): + return self.thisptr.Ixx + + def __set__ (self, value): + self.thisptr.Ixx = value + + property Iyx: + def __get__ (self): + return self.thisptr.Iyx + + def __set__ (self, value): + self.thisptr.Iyx = value + + property Iyy: + def __get__ (self): + return self.thisptr.Iyy + + def __set__ (self, value): + self.thisptr.Iyy = value + + property Izx: + def __get__ (self): + return self.thisptr.Izx + + def __set__ (self, value): + self.thisptr.Izx = value + + property Izy: + def __get__ (self): + return self.thisptr.Izy + + def __set__ (self, value): + self.thisptr.Izy = value + + property Izz: + def __get__ (self): + return self.thisptr.Izz + + def __set__ (self, value): + self.thisptr.Izz = value + +############################## +# +# Rigid Multibody Types +# +############################## + +cdef class Body: + cdef crbdl.Body *thisptr + cdef free_on_dealloc + + def __cinit__(self, **kwargs): + cdef double c_mass + cdef crbdl.Vector3d c_com + cdef crbdl.Matrix3d c_inertia + cdef uintptr_t address=0 + + if "address" in kwargs.keys(): + address=kwargs["address"] + mass = None + if "mass" in kwargs.keys(): + mass=kwargs["mass"] + com = None + if "com" in kwargs.keys(): + com=kwargs["com"] + inertia = None + if "inertia" in kwargs.keys(): + inertia=kwargs["inertia"] + + if address == 0: + self.free_on_dealloc = True + if (mass != None) and (com != None) and (inertia != None): + c_mass = mass + + for i in range (3): + c_com[i] = com[i] + + for i in range (3): + for j in range (3): + (&(c_inertia.coeff(i,j)))[0] = inertia[i,j] + + self.thisptr = new crbdl.Body(c_mass, c_com, c_inertia) + else: + self.thisptr = new crbdl.Body() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.Body (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Body (address=address) + + @classmethod + def fromMassComInertia(cls, double mass, + np.ndarray[double, ndim=1] com, + np.ndarray[double, ndim=2] inertia): + + return Body (address=0, mass=mass, com=com, inertia=inertia) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + + property mIsVirtual: + def __get__ (self): + return self.thisptr.mIsVirtual + + def __set__ (self, value): + self.thisptr.mIsVirtual = value + +cdef class FixedBody: + cdef crbdl.FixedBody *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.FixedBody() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.FixedBody (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return FixedBody (address) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + +cdef enum JointType: + JointTypeUndefined = 0 + JointTypeRevolute + JointTypePrismatic + JointTypeRevoluteX + JointTypeRevoluteY + JointTypeRevoluteZ + JointTypeSpherical + JointTypeEulerZYX + JointTypeEulerXYZ + JointTypeEulerYXZ + JointTypeTranslationXYZ + JointTypeFloatingBase + JointTypeFixed + JointType1DoF + JointType2DoF + JointType3DoF + JointType4DoF + JointType5DoF + JointType6DoF + JointTypeCustom + +cdef class Joint: + cdef crbdl.Joint *thisptr + cdef free_on_dealloc + + joint_type_map = { + JointTypeUndefined: "JointTypeUndefined", + JointTypeRevolute: "JointTypeRevolute", + JointTypePrismatic: "JointTypePrismatic", + JointTypeRevoluteX: "JointTypeRevoluteX", + JointTypeRevoluteY: "JointTypeRevoluteY", + JointTypeRevoluteZ: "JointTypeRevoluteZ", + JointTypeSpherical: "JointTypeSpherical", + JointTypeEulerZYX: "JointTypeEulerZYX", + JointTypeEulerXYZ: "JointTypeEulerXYZ", + JointTypeEulerYXZ: "JointTypeEulerYXZ", + JointTypeTranslationXYZ: "JointTypeTranslationXYZ", + JointTypeFloatingBase: "JointTypeFloatingBase", + JointTypeFixed: "JointTypeFixed", + JointType1DoF: "JointType1DoF", + JointType2DoF: "JointType2DoF", + JointType3DoF: "JointType3DoF", + JointType4DoF: "JointType4DoF", + JointType5DoF: "JointType5DoF", + JointType6DoF: "JointType6DoF", + JointTypeCustom: "JointTypeCustom", + } + + def _joint_type_from_str (self, joint_type_str): + if joint_type_str not in self.joint_type_map.values(): + raise ValueError("Invalid JointType '" + str(joint_type_str) + "'!") + else: + for joint_type, joint_str in self.joint_type_map.iteritems(): + if joint_str == joint_type_str: + return joint_type + + def __cinit__(self, uintptr_t address=0, joint_type=-1): + if address == 0: + self.free_on_dealloc = True + if joint_type == -1: + self.thisptr = new crbdl.Joint() + else: + self.thisptr = new crbdl.Joint(self._joint_type_from_str(joint_type)) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + joint_type_str = "JointTypeUndefined" + + if self.thisptr.mJointType in self.joint_type_map.keys(): + joint_type_str = self.joint_type_map[self.thisptr.mJointType] + + return "rbdl.Joint (0x{:0x}), JointType: {:s}".format( self.thisptr, joint_type_str) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Joint (address) + + @classmethod + def fromJointType(cls, joint_type): + return Joint (0, joint_type) + + @classmethod + def fromJointAxes(cls, axes): + assert (len(axes) > 0) + assert (len(axes[0]) == 6) + axes_count = len(axes) + joint_type = JointType1DoF + axes_count - 1 + + result = Joint (0, cls.joint_type_map[joint_type]) + + for i in range (axes_count): + result.setJointAxis(i, axes[i]) + + return result + + property mDoFCount: + def __get__ (self): + return self.thisptr.mDoFCount + + def __set__ (self, value): + self.thisptr.mDoFCount = value + + property mJointType: + def __get__ (self): + return self.joint_type_map[self.thisptr.mJointType] + + property q_index: + def __get__ (self): + return self.thisptr.q_index + + def getJointAxis (self, index): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + return SpatialVectorToNumpy (self.thisptr.mJointAxes[index]) + + def setJointAxis (self, index, value): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + for i in range (6): + (&(self.thisptr.mJointAxes[index][i]))[0] = value[i] + self.thisptr.mJointAxes[index][i] + +cdef class Model + +cdef class _Model_v_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.v[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.v[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.v[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.v[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.v.size() + +cdef class _Model_a_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.a[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.a[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.a[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.a[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.a.size() + +cdef class _Model_mJoints_Joint_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [Joint.fromPointer ( &(self.parent.mJoints[i])) for i in xrange (*key.indices(len(self)))] + else: + return Joint.fromPointer ( &(self.parent.mJoints[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], Joint), "Invalid type! Expected Joint, but got " + str(type(value[src_index])) + "." + self.parent.mJoints[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, Joint), "Invalid type! Expected Joint, but got " + str(type(value)) + "." + self.parent.mJoints[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.mJoints.size() + +cdef class _Model_S_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.S[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.S[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.S[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.S[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.S.size() + +cdef class _Model_X_J_SpatialTransform_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialTransform.fromPointer ( &(self.parent.X_J[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialTransform.fromPointer ( &(self.parent.X_J[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value[src_index])) + "." + self.parent.X_J[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value)) + "." + self.parent.X_J[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.X_J.size() + +cdef class _Model_v_J_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.v_J[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.v_J[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.v_J[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.v_J[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.v_J.size() + +cdef class _Model_c_J_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.c_J[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.c_J[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.c_J[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.c_J[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.c_J.size() + +cdef class _Model_X_T_SpatialTransform_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialTransform.fromPointer ( &(self.parent.X_T[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialTransform.fromPointer ( &(self.parent.X_T[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value[src_index])) + "." + self.parent.X_T[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value)) + "." + self.parent.X_T[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.X_T.size() + +cdef class _Model_c_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.c[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.c[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.c[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.c[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.c.size() + +cdef class _Model_IA_SpatialMatrix_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialMatrix.fromPointer ( &(self.parent.IA[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialMatrix.fromPointer ( &(self.parent.IA[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialMatrix), "Invalid type! Expected SpatialMatrix, but got " + str(type(value[src_index])) + "." + self.parent.IA[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialMatrix), "Invalid type! Expected SpatialMatrix, but got " + str(type(value)) + "." + self.parent.IA[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.IA.size() + +cdef class _Model_pA_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.pA[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.pA[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.pA[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.pA[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.pA.size() + +cdef class _Model_U_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.U[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.U[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.U[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.U[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.U.size() + +cdef class _Model_f_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.f[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.f[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.f[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.f[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.f.size() + +cdef class _Model_I_SpatialRigidBodyInertia_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialRigidBodyInertia.fromPointer ( &(self.parent.I[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialRigidBodyInertia.fromPointer ( &(self.parent.I[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialRigidBodyInertia), "Invalid type! Expected SpatialRigidBodyInertia, but got " + str(type(value[src_index])) + "." + self.parent.I[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialRigidBodyInertia), "Invalid type! Expected SpatialRigidBodyInertia, but got " + str(type(value)) + "." + self.parent.I[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.I.size() + +cdef class _Model_Ic_SpatialRigidBodyInertia_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialRigidBodyInertia.fromPointer ( &(self.parent.Ic[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialRigidBodyInertia.fromPointer ( &(self.parent.Ic[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialRigidBodyInertia), "Invalid type! Expected SpatialRigidBodyInertia, but got " + str(type(value[src_index])) + "." + self.parent.Ic[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialRigidBodyInertia), "Invalid type! Expected SpatialRigidBodyInertia, but got " + str(type(value)) + "." + self.parent.Ic[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.Ic.size() + +cdef class _Model_hc_SpatialVector_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialVector.fromPointer ( &(self.parent.hc[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialVector.fromPointer ( &(self.parent.hc[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value[src_index])) + "." + self.parent.hc[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialVector), "Invalid type! Expected SpatialVector, but got " + str(type(value)) + "." + self.parent.hc[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.hc.size() + +cdef class _Model_X_lambda_SpatialTransform_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialTransform.fromPointer ( &(self.parent.X_lambda[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialTransform.fromPointer ( &(self.parent.X_lambda[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value[src_index])) + "." + self.parent.X_lambda[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value)) + "." + self.parent.X_lambda[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.X_lambda.size() + +cdef class _Model_X_base_SpatialTransform_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [SpatialTransform.fromPointer ( &(self.parent.X_base[i])) for i in xrange (*key.indices(len(self)))] + else: + return SpatialTransform.fromPointer ( &(self.parent.X_base[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value[src_index])) + "." + self.parent.X_base[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, SpatialTransform), "Invalid type! Expected SpatialTransform, but got " + str(type(value)) + "." + self.parent.X_base[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.X_base.size() + +cdef class _Model_mFixedBodies_FixedBody_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [FixedBody.fromPointer ( &(self.parent.mFixedBodies[i])) for i in xrange (*key.indices(len(self)))] + else: + return FixedBody.fromPointer ( &(self.parent.mFixedBodies[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], FixedBody), "Invalid type! Expected FixedBody, but got " + str(type(value[src_index])) + "." + self.parent.mFixedBodies[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, FixedBody), "Invalid type! Expected FixedBody, but got " + str(type(value)) + "." + self.parent.mFixedBodies[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.mFixedBodies.size() + +cdef class _Model_mBodies_Body_VectorWrapper: + cdef crbdl.Model *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [Body.fromPointer ( &(self.parent.mBodies[i])) for i in xrange (*key.indices(len(self)))] + else: + return Body.fromPointer ( &(self.parent.mBodies[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], Body), "Invalid type! Expected Body, but got " + str(type(value[src_index])) + "." + self.parent.mBodies[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, Body), "Invalid type! Expected Body, but got " + str(type(value)) + "." + self.parent.mBodies[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.mBodies.size() + + +cdef class Model: + cdef crbdl.Model *thisptr + cdef _Model_v_SpatialVector_VectorWrapper v + cdef _Model_a_SpatialVector_VectorWrapper a + cdef _Model_mJoints_Joint_VectorWrapper mJoints + cdef _Model_S_SpatialVector_VectorWrapper S + cdef _Model_X_J_SpatialTransform_VectorWrapper X_J + cdef _Model_v_J_SpatialVector_VectorWrapper v_J + cdef _Model_c_J_SpatialVector_VectorWrapper c_J + cdef _Model_X_T_SpatialTransform_VectorWrapper X_T + cdef _Model_c_SpatialVector_VectorWrapper c + cdef _Model_IA_SpatialMatrix_VectorWrapper IA + cdef _Model_pA_SpatialVector_VectorWrapper pA + cdef _Model_U_SpatialVector_VectorWrapper U + cdef _Model_f_SpatialVector_VectorWrapper f + cdef _Model_I_SpatialRigidBodyInertia_VectorWrapper I + cdef _Model_Ic_SpatialRigidBodyInertia_VectorWrapper Ic + cdef _Model_hc_SpatialVector_VectorWrapper hc + cdef _Model_X_lambda_SpatialTransform_VectorWrapper X_lambda + cdef _Model_X_base_SpatialTransform_VectorWrapper X_base + cdef _Model_mFixedBodies_FixedBody_VectorWrapper mFixedBodies + cdef _Model_mBodies_Body_VectorWrapper mBodies + + def __cinit__(self): + self.thisptr = new crbdl.Model() + self.v = _Model_v_SpatialVector_VectorWrapper ( self.thisptr) + self.a = _Model_a_SpatialVector_VectorWrapper ( self.thisptr) + self.mJoints = _Model_mJoints_Joint_VectorWrapper ( self.thisptr) + self.S = _Model_S_SpatialVector_VectorWrapper ( self.thisptr) + self.X_J = _Model_X_J_SpatialTransform_VectorWrapper ( self.thisptr) + self.v_J = _Model_v_J_SpatialVector_VectorWrapper ( self.thisptr) + self.c_J = _Model_c_J_SpatialVector_VectorWrapper ( self.thisptr) + self.X_T = _Model_X_T_SpatialTransform_VectorWrapper ( self.thisptr) + self.c = _Model_c_SpatialVector_VectorWrapper ( self.thisptr) + self.IA = _Model_IA_SpatialMatrix_VectorWrapper ( self.thisptr) + self.pA = _Model_pA_SpatialVector_VectorWrapper ( self.thisptr) + self.U = _Model_U_SpatialVector_VectorWrapper ( self.thisptr) + self.f = _Model_f_SpatialVector_VectorWrapper ( self.thisptr) + self.I = _Model_I_SpatialRigidBodyInertia_VectorWrapper ( self.thisptr) + self.Ic = _Model_Ic_SpatialRigidBodyInertia_VectorWrapper ( self.thisptr) + self.hc = _Model_hc_SpatialVector_VectorWrapper ( self.thisptr) + self.X_lambda = _Model_X_lambda_SpatialTransform_VectorWrapper ( self.thisptr) + self.X_base = _Model_X_base_SpatialTransform_VectorWrapper ( self.thisptr) + self.mFixedBodies = _Model_mFixedBodies_FixedBody_VectorWrapper ( self.thisptr) + self.mBodies = _Model_mBodies_Body_VectorWrapper ( self.thisptr) + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.Model (0x{:0x})".format( self.thisptr) + + def AddBody (self, + parent_id, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = ""): + return self.thisptr.AddBody ( + parent_id, + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def AppendBody (self, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = ""): + return self.thisptr.AppendBody ( + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def SetQuaternion (self, + int body_id, + np.ndarray[double, ndim=1, mode="c"] quat, + np.ndarray[double, ndim=1, mode="c"] q): + quat_wrap = Quaternion.fromPythonArray (quat) + q_wrap = VectorNd.fromPythonArray (q) + self.thisptr.SetQuaternion (body_id, + (quat_wrap).thisptr[0], + (q_wrap).thisptr[0]) + for i in range(len(q)): + q[i] = q_wrap[i] + + def GetQuaternion (self, + int body_id, + np.ndarray[double, ndim=1, mode="c"] q): + return QuaternionToNumpy (self.thisptr.GetQuaternion(body_id, NumpyToVectorNd (q))) + + def GetBody (self, index): + return Body (address= &(self.thisptr.mBodies[index])) + + def GetParentBodyId (self, index): + return self.thisptr.GetParentBodyId(index) + + def GetBodyId (self, name): + return self.thisptr.GetBodyId(name) + + def GetBodyName (self, index): + return self.thisptr.GetBodyName(index) + + def IsBodyId (self, index): + return self.thisptr.IsBodyId(index) + + def IsFixedBodyId (self, index): + return self.thisptr.IsFixedBodyId(index) + + property dof_count: + def __get__ (self): + return self.thisptr.dof_count + + property q_size: + def __get__ (self): + return self.thisptr.q_size + + property qdot_size: + def __get__ (self): + return self.thisptr.qdot_size + + property previously_added_body_id: + def __get__ (self): + return self.thisptr.previously_added_body_id + + property gravity: + def __get__ (self): + return np.array ([ + self.thisptr.gravity[0], + self.thisptr.gravity[1], + self.thisptr.gravity[2] + ] + ) + def __set__ (self, values): + for i in range (0,3): + self.thisptr.gravity[i] = values[i] + + property v: + def __get__ (self): + return self.v + + property a: + def __get__ (self): + return self.a + + + property mJoints: + def __get__ (self): + return self.mJoints + + property S: + def __get__ (self): + return self.S + + property X_J: + def __get__ (self): + return self.X_J + + property v_J: + def __get__ (self): + return self.v_J + + property c_J: + def __get__ (self): + return self.c_J + + + property mJointUpdateOrder: + def __get__ (self): + return self.thisptr.mJointUpdateOrder + + property X_T: + def __get__ (self): + return self.X_T + + + property mFixedJointCount: + def __get__ (self): + return self.thisptr.mFixedJointCount + + # TODO + # multdof3_S + # multdof3_U + # multdof3_Dinv + # multdof3_u + + property multdof3_w_index: + def __get__ (self): + return self.thisptr.multdof3_w_index + + property c: + def __get__ (self): + return self.c + + property IA: + def __get__ (self): + return self.IA + + property pA: + def __get__ (self): + return self.pA + + property U: + def __get__ (self): + return self.U + + + # TODO + # d + # u + + property f: + def __get__ (self): + return self.f + + property I: + def __get__ (self): + return self.I + + property Ic: + def __get__ (self): + return self.Ic + + property hc: + def __get__ (self): + return self.hc + + + property X_lambda: + def __get__ (self): + return self.X_lambda + + property X_base: + def __get__ (self): + return self.X_base + + + property mFixedBodies: + def __get__ (self): + return self.mFixedBodies + + + property fixed_body_discriminator: + def __get__ (self): + return self.thisptr.fixed_body_discriminator + + property mBodies: + def __get__ (self): + return self.mBodies + + +############################## +# +# Constraint Types +# +############################## + +cdef class ConstraintSet + +cdef class _ConstraintSet_point_Vector3d_VectorWrapper: + cdef crbdl.ConstraintSet *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [Vector3d.fromPointer ( &(self.parent.point[i])) for i in xrange (*key.indices(len(self)))] + else: + return Vector3d.fromPointer ( &(self.parent.point[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], Vector3d), "Invalid type! Expected Vector3d, but got " + str(type(value[src_index])) + "." + self.parent.point[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, Vector3d), "Invalid type! Expected Vector3d, but got " + str(type(value)) + "." + self.parent.point[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.point.size() + +cdef class _ConstraintSet_normal_Vector3d_VectorWrapper: + cdef crbdl.ConstraintSet *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [Vector3d.fromPointer ( &(self.parent.normal[i])) for i in xrange (*key.indices(len(self)))] + else: + return Vector3d.fromPointer ( &(self.parent.normal[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], Vector3d), "Invalid type! Expected Vector3d, but got " + str(type(value[src_index])) + "." + self.parent.normal[i] = ( value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, Vector3d), "Invalid type! Expected Vector3d, but got " + str(type(value)) + "." + self.parent.normal[key] = ( value).thisptr[0] + + def __len__(self): + return self.parent.normal.size() + + +cdef class ConstraintSet: + cdef crbdl.ConstraintSet *thisptr + cdef _ConstraintSet_point_Vector3d_VectorWrapper point + cdef _ConstraintSet_normal_Vector3d_VectorWrapper normal + + def __cinit__(self): + self.thisptr = new crbdl.ConstraintSet() + self.point = _ConstraintSet_point_Vector3d_VectorWrapper ( self.thisptr) + self.normal = _ConstraintSet_normal_Vector3d_VectorWrapper ( self.thisptr) + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.ConstraintSet (0x{:0x})".format( self.thisptr) + + def AddContactConstraint (self, + body_id not None, + body_point not None, + world_normal not None, + constraint_name = None, + normal_acceleration = 0.): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Vector3d c_world_normal + cdef char* constraint_name_ptr + + for i in range (3): + c_body_point[i] = body_point[i] + c_world_normal[i] = world_normal[i] + + if constraint_name == None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddContactConstraint ( + body_id, + c_body_point, + c_world_normal, + constraint_name_ptr, + normal_acceleration + ) + + def AddLoopConstraint (self, + id_predecessor not None, + id_successor not None, + SpatialTransform X_predecessor not None, + SpatialTransform X_successor not None, + SpatialVector axis not None, + double T_stab_inv, + constraint_name = None): + cdef char* constraint_name_ptr + + if constraint_name == None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddLoopConstraint ( + id_predecessor, + id_successor, + X_predecessor.thisptr[0], + X_successor.thisptr[0], + axis.thisptr[0], + T_stab_inv, + constraint_name_ptr) + + def Bind (self, model): + return self.thisptr.Bind ((model).thisptr[0]) + + def size (self): + return self.thisptr.size() + + def clear (self): + self.thisptr.clear() + + property bound: + def __get__ (self): + return self.thisptr.bound + +# %VectorWrapperAddProperty (TYPE=string, MEMBER=name, PARENT=ConstraintSet)% + + property point: + def __get__ (self): + return self.point + + property normal: + def __get__ (self): + return self.normal + + +# property acceleration: +# def __get__(self): +# return VectorNd.fromPointer ( &(self.thisptr.acceleration)).toNumpy() +# def __set__(self, values): +# vec = VectorNd.fromPythonArray (values) +# self.thisptr.acceleration = (vec.thisptr[0]) + +############################## +# +# Kinematics.h +# +############################## + +def CalcBodyToBaseCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBodyToBaseCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcBaseToBodyCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBaseToBodyCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointVelocity (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointVelocity ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointAcceleration ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointVelocity6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointVelocity6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointAcceleration6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointJacobian (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobianPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcPointJacobian6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobian6DPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcBodySpatialJacobian(Model model, + np.ndarray[double, ndim=1, mode="c"] q, + int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcBodySpatialJacobianPtr( + model.thisptr[0], + q.data, + body_id, + G.data, + update_kinematics + ) + +############################## +# +# rbdl_utils.h +# +############################## + +def CalcCenterOfMass (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] com, + np.ndarray[double, ndim=1, mode="c"] com_velocity=None, + np.ndarray[double, ndim=1, mode="c"] angular_momentum=None, + update_kinematics=True): + cdef double cmass + cdef crbdl.Vector3d c_com = crbdl.Vector3d() + cdef crbdl.Vector3d* c_com_vel_ptr # = crbdl.Vector3d() + cdef crbdl.Vector3d* c_ang_momentum_ptr # = crbdl.Vector3d() + + c_com_vel_ptr = NULL + c_ang_momentum_ptr = NULL + + if com_velocity != None: + c_com_vel_ptr = new crbdl.Vector3d() + + if angular_momentum != None: + c_ang_momentum_ptr = new crbdl.Vector3d() + + cmass = 0.0 + crbdl.CalcCenterOfMass ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + cmass, + c_com, + c_com_vel_ptr, + c_ang_momentum_ptr, + update_kinematics) + + com[0] = c_com[0] + com[1] = c_com[1] + com[2] = c_com[2] + + if com_velocity != None: + com_velocity[0] = c_com_vel_ptr.data()[0] + com_velocity[1] = c_com_vel_ptr.data()[1] + com_velocity[2] = c_com_vel_ptr.data()[2] + del c_com_vel_ptr + + if angular_momentum != None: + angular_momentum[0] = c_ang_momentum_ptr.data()[0] + angular_momentum[1] = c_ang_momentum_ptr.data()[1] + angular_momentum[2] = c_ang_momentum_ptr.data()[2] + del c_ang_momentum_ptr + + return cmass + +############################## +# +# Dynamics.h +# +############################## + +def InverseDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.InverseDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + qddot.data, + tau.data, + NULL + ) + +def NonlinearEffects (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.NonlinearEffectsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data + ) + +def CompositeRigidBodyAlgorithm (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=2, mode="c"] H, + update_kinematics=True): + crbdl.CompositeRigidBodyAlgorithmPtr (model.thisptr[0], + q.data, + H.data, + update_kinematics); + +def ForwardDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau, + np.ndarray[double, ndim=1, mode="c"] qddot): + crbdl.ForwardDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data, + qddot.data, + NULL + ) + +def loadModel ( + filename, + **kwargs + ): + verbose = False + if "verbose" in kwargs.keys(): + verbose=kwargs["verbose"] + + floating_base = False + if "floating_base" in kwargs.keys(): + floating_base=kwargs["floating_base"] + + result = Model() + if crbdl.rbdl_loadmodel (filename, result.thisptr, floating_base, verbose): + return result + + print ("Error loading model {}!".format (filename)) + return None + diff --git a/3rdparty/rbdl/python/rbdl_loadmodel.cc b/3rdparty/rbdl/python/rbdl_loadmodel.cc new file mode 100644 index 0000000..04f0941 --- /dev/null +++ b/3rdparty/rbdl/python/rbdl_loadmodel.cc @@ -0,0 +1,42 @@ +#include + +#ifdef RBDL_BUILD_ADDON_LUAMODEL +#include +#endif + +#ifdef RBDL_BUILD_ADDON_URDFREADER +#include +#endif + +#include +#include + +using namespace RigidBodyDynamics; +using namespace std; + +bool rbdl_loadmodel (const char* filename, Model* model, bool floating_base=false, bool verbose=false) { + string fname (filename); + + for (size_t i = 0; i < fname.size(); i++) { + fname[i] = tolower(fname[i]); + } + + bool result = false; + if (fname.substr (fname.size() - 4, 4) == ".lua") { +#ifdef RBDL_BUILD_ADDON_LUAMODEL + result = Addons::LuaModelReadFromFile (filename, model, verbose); +#else + cerr << "Error: RBDL Addon LuaModel not enabled!" << endl; +#endif + } else if (fname.substr (fname.size() - 5, 5) == ".urdf") { +#ifdef RBDL_BUILD_ADDON_URDFREADER + result = Addons::URDFReadFromFile (filename, model, floating_base, verbose); +#else + cerr << "Error: RBDL Addon URDFReader not enabled!" << endl; +#endif + } else { + cerr << "Error: Cannot identify model type from filename '" << filename << "'!" << endl; + } + + return result; +} diff --git a/3rdparty/rbdl/python/rbdl_ptr_functions.h b/3rdparty/rbdl/python/rbdl_ptr_functions.h new file mode 100644 index 0000000..d6fbb82 --- /dev/null +++ b/3rdparty/rbdl/python/rbdl_ptr_functions.h @@ -0,0 +1,637 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2015 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + * + * This file defines functions that allows calling of the RBDL algorithms + * by providing input and output as raw double arrays. It eliminates the + * need of copying from Numpy values into temporary RBDL (C++) vectors and + * matrices. However it requires C++11 and must be compiled with -std=c++11 + * (or -std=c++0x on older compilers). + */ + +#include +#include + +namespace RigidBodyDynamics { + +namespace Math { + +// PTR_DATA_ROW_MAJOR : +// Specifies whether the data that is provided via raw double pointers is +// stored as row major. Eigen uses column major by default and therefore +// this has to be properly mapped. +#define PTR_DATA_ROW_MAJOR 1 + +#ifdef RBDL_USE_SIMPLE_MATH + typedef VectorNd VectorNdRef; + typedef MatrixNd MatrixNdRef; +#else + typedef Eigen::Ref VectorNdRef; + +#ifdef PTR_DATA_ROW_MAJOR + typedef Eigen::Matrix MatrixNdRowMaj; + typedef Eigen::Ref MatrixNdRef; +#else + typedef Eigen::Ref MatrixNdRef; +#endif + +#endif + +RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) { +#ifdef RBDL_USE_SIMPLE_MATH + return SimpleMath::Map (ptr, n, 1); +#elif defined EIGEN_CORE_H + return Eigen::Map (ptr, n, 1); +#else + std::cerr << __func__ << " not defined for used math library!" << std::endl; + abort(); + return VectorNd::Constant (1,1./0.); +#endif +} + +RBDL_DLLAPI inline MatrixNdRef MatrixFromPtr (double *ptr, unsigned int rows, unsigned int cols, bool row_major = true) { +#ifdef RBDL_USE_SIMPLE_MATH + return SimpleMath::Map (ptr, rows, cols); +#elif defined EIGEN_CORE_H +#ifdef PTR_DATA_ROW_MAJOR + return Eigen::Map (ptr, rows, cols); +#else + return Eigen::Map (ptr, rows, cols); +#endif +#else + std::cerr << __func__ << " not defined for used math library!" << std::endl; + abort(); + return MatrixNd::Constant (1,1, 1./0.); +#endif +} + +} + +RBDL_DLLAPI +void UpdateKinematicsCustomPtr (Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *qddot_ptr + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + unsigned int i; + + if (q_ptr) { + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + VectorNd QDot_zero (VectorNd::Zero (model.q_size)); + + jcalc (model, i, (Q), QDot_zero); + + model.X_lambda[i] = model.X_J[i] * model.X_T[i]; + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + } + } + + if (qdot_ptr) { + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.q_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) { + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } else { + model.v[i] = model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } + // LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl; + } + } + + if (qddot_ptr) { + VectorNdRef QDDot = VectorFromPtr(const_cast(qddot_ptr), model.q_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + } else { + model.a[i] = model.c[i]; + } + + if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp ((QDDot)[q_index], (QDDot)[q_index + 1], (QDDot)[q_index + 2]); + model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp; + } else { + model.a[i] = model.a[i] + model.S[i] * (QDDot)[q_index]; + } + } + } +} + +RBDL_DLLAPI +void CalcPointJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + const Math::Vector3d &point_position, + double * G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 3, model.qdot_size); + + SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false)); + + assert (G.rows() == 3 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + // e[j] is set to 1 if joint j contributes to the jacobian that we are + // computing. For all other joints the column will be zero. + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 3, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(3,0,3,3); + } else { + G.block(0,q_index, 3, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(3,0,3,1); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void CalcPointJacobian6DPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + const Math::Vector3d &point_position, + double *G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 6, model.qdot_size); + + SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false)); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 6, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(0,0,6,3); + } else { + G.block(0,q_index, 6, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(0,0,6,1); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void CalcBodySpatialJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + double *G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 6, model.q_size); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + SpatialTransform base_to_body; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + base_to_body = model.mFixedBodies[fbody_id].mParentTransform * model.X_base[reference_body_id]; + } else { + base_to_body = model.X_base[reference_body_id]; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0,q_index,6,3) = (base_to_body * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]; + } else { + G.block(0,q_index,6,1) = base_to_body.apply(model.X_base[j].inverse().apply(model.S[j])); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void InverseDynamicsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *qddot_ptr, + const double *tau_ptr, + std::vector *f_ext + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.q_size); + VectorNdRef QDDot = VectorFromPtr(const_cast(qddot_ptr), model.q_size); + VectorNdRef Tau = VectorFromPtr(const_cast(tau_ptr), model.q_size); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + + if (model.mJoints[i].mDoFCount == 3) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.multdof3_S[i] * Vector3d (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]); + } else { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.S[i] * QDDot[q_index]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) + model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i]; + } else { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI +void NonlinearEffectsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *tau_ptr + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.q_size); + VectorNdRef Tau = VectorFromPtr(const_cast(tau_ptr), model.q_size); + + SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0] = spatial_gravity; + + for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) { + jcalc (model, model.mJointUpdateOrder[i], Q, QDot); + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.lambda[i] == 0) { + model.v[i] = model.v_J[i]; + model.a[i] = model.X_lambda[i].apply(spatial_gravity); + } else { + model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i]; + } else { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI +inline void CompositeRigidBodyAlgorithmPtr ( + Model& model, + const double *q_ptr, + double *H_ptr, + bool update_kinematics = true + ) { + using namespace RigidBodyDynamics::Math; + + VectorNdRef&& Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef&& H = MatrixFromPtr(H_ptr, model.qdot_size, model.qdot_size); + + assert (H.rows() == model.dof_count && H.cols() == model.dof_count); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (update_kinematics) { + jcalc_X_lambda_S (model, i, Q); + } + model.Ic[i] = model.I[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.lambda[i] != 0) { + model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]); + } + + unsigned int dof_index_i = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3) { + Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i]; + H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]); + + H.block<3,3>(dof_index_i,dof_index_j) = H_temp2; + H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } else { + Vector3d H_temp2 = F_63.transpose() * (model.S[j]); + + H.block<3,1>(dof_index_i,dof_index_j) = H_temp2; + H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } + } + } else { + SpatialVector F = model.Ic[i] * model.S[i]; + H(dof_index_i, dof_index_i) = model.S[i].dot(F); + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F = model.X_lambda[j].applyTranspose(F); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose(); + + LOG << F.transpose() << std::endl << model.multdof3_S[j] << std::endl; + LOG << H_temp2.transpose() << std::endl; + + H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose(); + H.block<3,1>(dof_index_j,dof_index_i) = H_temp2; + } else { + H(dof_index_i,dof_index_j) = F.dot(model.S[j]); + H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j); + } + } + } + } +} + +RBDL_DLLAPI +void ForwardDynamicsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *tau_ptr, + const double *qddot_ptr, + std::vector *f_ext + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef&& Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef&& QDot = VectorFromPtr(const_cast(qdot_ptr), model.q_size); + VectorNdRef&& QDDot = VectorFromPtr(const_cast(qddot_ptr), model.q_size); + VectorNdRef&& Tau = VectorFromPtr(const_cast(tau_ptr), model.q_size); + + SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]); + + unsigned int i = 0; + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "QDot = " << QDot.transpose() << std::endl; + LOG << "Tau = " << Tau.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + else + model.X_base[i] = model.X_lambda[i]; + + model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i]; + + /* + LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl; + LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl; + LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl; + LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl; + LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl; + LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl; + */ + + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.I[i].setSpatialMatrix (model.IA[i]); + + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + +// ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3) { + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); +#endif + Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]); + + model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i]; + +// LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; + } + } else { + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); +// LOG << "u[" << i << "] = " << model.u[i] << std::endl; + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; + } + } + } + +// ClearLogOutput(); + + model.a[0] = spatial_gravity * -1.; + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 3) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]); + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +} diff --git a/3rdparty/rbdl/python/setup.py.cmake b/3rdparty/rbdl/python/setup.py.cmake new file mode 100755 index 0000000..1b7fe68 --- /dev/null +++ b/3rdparty/rbdl/python/setup.py.cmake @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +from distutils.core import setup +from distutils.extension import Extension +from Cython.Distutils import build_ext +from Cython.Build import cythonize + +import os +import numpy as np + +BASEDIR = os.path.dirname(os.path.abspath(__file__)) + +extra_params = {} +extra_params['include_dirs'] = [ + '/usr/include', + BASEDIR, + np.get_include(), + '@RBDL_INCLUDE_DIR@', + '@EIGEN3_INCLUDE_DIR@', + '@CMAKE_CURRENT_SOURCE_DIR@', + '@RBDL_SOURCE_DIR@/include', + '@RBDL_BINARY_DIR@/include', + '/usr/include/eigen3/' +] + +extra_params['language'] = 'c++' +extra_params['extra_compile_args'] = ["-O3", "-Wno-unused-variable", "-std=c++11"] +extra_params['libraries'] = ['rbdl'] +extra_params['library_dirs'] = [ + '${CMAKE_CURRENT_BINARY_DIR}/../', + '${CMAKE_INSTALL_PREFIX}/lib/', + '/usr/lib', + BASEDIR + ] +extra_params['extra_link_args'] = [ + "-Wl,-O1", + "-Wl,--as-needed", + ] + +if os.name == 'posix': + extra_params['runtime_library_dirs'] = extra_params['library_dirs'] + +ext_modules = [ + Extension("rbdl", ["crbdl.pxd", "rbdl.pyx"], **extra_params), +] + +setup( + name='rbdl', + author='Martin Felis', + author_email='martin@fysx.org', + description='Python wrapper for RBDL - the Rigid Body Dynamics Library', + license='zlib', + version='${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH}', + url='http://rbdl.bitbucket.org/', + cmdclass={'build_ext': build_ext}, + ext_modules=cythonize(ext_modules), +) diff --git a/3rdparty/rbdl/python/test_wrapper.py b/3rdparty/rbdl/python/test_wrapper.py new file mode 100755 index 0000000..18796c9 --- /dev/null +++ b/3rdparty/rbdl/python/test_wrapper.py @@ -0,0 +1,326 @@ +#!/usr/bin/python +# +# RBDL - Rigid Body Dynamics Library +# Copyright (c) 2011-2015 Martin Felis +# +# Licensed under the zlib license. See LICENSE for more details. + +import unittest + +import math +import numpy as np +from numpy.testing import * +import rbdl + +class JointTests (unittest.TestCase): + def test_JointConstructorAxesSimple(self): + axis = np.asarray([[1., 0., 0., 0., 0., 0.]]) + joint_rot_x = rbdl.Joint.fromJointAxes (axis) + + assert_equal (joint_rot_x.getJointAxis(0), axis[0]) + + def test_JointConstructorAxes6DoF(self): + axis = np.asarray([ + [1., 0., 0., 0., 0., 0.], + [0., 1., 0., 0., 0., 0.], + [0., 0., 1., 0., 0., 0.], + [0., 0., 0., 1., 0., 0.], + [0., 0., 0., 0., 1., 0.], + [0., 0., 0., 0., 0., 1.], + ]) + joint = rbdl.Joint.fromJointAxes (axis) + + for i in range (axis.shape[0]): + assert_equal (joint.getJointAxis(i), axis[i]) + +class SampleModel3R (unittest.TestCase): + """ Example planar triple pendulum + + - All joints along the positive Z axis in rest position + - All joints revolute y joints + - all "links" are 1 unit length + """ + def setUp(self): + self.model = rbdl.Model() + joint_rot_y = rbdl.Joint.fromJointType ("JointTypeRevoluteY") + self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.5]), np.eye(3) * + 0.05) + self.xtrans = rbdl.SpatialTransform() + self.xtrans.r = np.array([0., 0., 1.]) + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body) + self.body_2 = self.model.AppendBody (self.xtrans, joint_rot_y, self.body) + self.body_3 = self.model.AppendBody (self.xtrans, joint_rot_y, self.body) + + self.q = np.zeros (self.model.q_size) + self.qdot = np.zeros (self.model.qdot_size) + self.qddot = np.zeros (self.model.qdot_size) + self.tau = np.zeros (self.model.qdot_size) + + def test_CoordinateTransformBodyBase (self): + """ + Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates + give the right results. + """ + q = np.random.rand(self.model.q_size) + point_local = np.array ([1., 2., 3.]) + point_base = rbdl.CalcBodyToBaseCoordinates ( + self.model, + q, + self.body_3, + point_local) + point_local_2 = rbdl.CalcBaseToBodyCoordinates ( + self.model, + q, + self.body_3, + point_base) + + assert_almost_equal (point_local, point_local_2) + + def test_CalcPointVelocity (self): + """ + Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates + give the right results. + """ + q = np.zeros(self.model.q_size) + qdot = np.zeros(self.model.q_size) + qdot[0] = 1. + point_local = np.array ([0., 0., 0.]) + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_3, + point_local + ) + + assert_almost_equal (np.array([2., 0., 0.]), point_vel) + + def test_CalcCenterOfMass (self): + """ Tests calculation of center of mass + TODO: add checks for angular momentum + """ + + com = np.array ([-1., -1., -1.]) + com_vel = np.array([-2., -2., -2.]) + ang_mom = np.array([-3., -3., -3.]) + self.qdot[0] = 1. + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + com, + None, + None + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + assert_almost_equal (np.array([0., 0., 1.5]), com) + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + com, + com_vel, + None + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + assert_almost_equal (np.array([1.5, 0., 0.0]), com_vel) + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + com, + com_vel, + ang_mom + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + + def test_DynamicsConsistency (self): + """ Checks whether forward and inverse dynamics are consistent """ + q = np.random.rand (self.model.q_size) + qdot = np.random.rand (self.model.q_size) + qddot = np.random.rand (self.model.q_size) + + tau = np.random.rand (self.model.q_size) + + rbdl.ForwardDynamics ( + self.model, + q, + qdot, + tau, + qddot) + + tau_id = np.zeros ((self.model.q_size)) + rbdl.InverseDynamics ( + self.model, + q, + qdot, + qddot, + tau_id + ) + + assert_almost_equal (tau, tau_id) + + def test_NonlinearEffectsConsistency (self): + """ Checks whether NonlinearEffects is consistent with InverseDynamics """ + q = np.random.rand (self.model.q_size) + qdot = np.random.rand (self.model.q_size) + + nle_id = np.random.rand (self.model.q_size) + + rbdl.InverseDynamics( + self.model, + q, + qdot, + np.zeros (self.model.qdot_size), + nle_id) + + nle = np.zeros ((self.model.q_size)) + rbdl.NonlinearEffects ( + self.model, + q, + qdot, + nle + ) + + assert_almost_equal (nle_id, nle) + + def test_CalcPointJacobian (self): + """ Computes point Jacobian and checks whether G * qdot is consistent + with CalcPointVelocity. """ + q = np.zeros (self.model.q_size) + G = np.zeros ([3, self.model.q_size]) + point_coords = np.array ([0., 0., 1.]) + + rbdl.CalcPointJacobian ( + self.model, + q, + self.body_3, + point_coords, + G + ) + + qdot = np.ones(self.model.qdot_size) + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_3, + point_coords + ) + + jac_point_vel = np.dot (G, qdot) + assert_almost_equal (jac_point_vel, point_vel) + + def test_CalcPointJacobianNonSquare (self): + """ Computes point Jacobian and checks whether G * qdot is consistent + with CalcPointVelocity. """ + + self.model = rbdl.Model() + joint_trans_xyz = rbdl.Joint.fromJointType ("JointTypeTranslationXYZ") + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), + joint_trans_xyz, self.body) + + self.body_4 = self.model.AppendBody (rbdl.SpatialTransform(), + joint_trans_xyz, self.body) + + point_coords = np.array ([0., 0., 1.]) + q = np.zeros (self.model.q_size) + G = np.zeros ([3, self.model.q_size]) + + rbdl.CalcPointJacobian ( + self.model, + q, + self.body_4, + point_coords, + G + ) + + qdot = np.ones(self.model.qdot_size) + jac_point_vel = np.dot (G, qdot) + + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_4, + point_coords + ) + + assert_almost_equal (jac_point_vel, point_vel) + +class FloatingBaseModel (unittest.TestCase): + """ Model with a floating base + """ + def setUp(self): + self.model = rbdl.Model() + joint_rot_y = rbdl.Joint.fromJointType ("JointTypeFloatingBase") + self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.5]), np.eye(3) * + 0.05) + self.xtrans = rbdl.SpatialTransform() + self.xtrans.r = np.array([0., 0., 0.]) + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body) + + self.q = np.zeros (self.model.q_size) + self.qdot = np.zeros (self.model.qdot_size) + self.qddot = np.zeros (self.model.qdot_size) + self.tau = np.zeros (self.model.qdot_size) + + def test_Dimensions (self): + """ + Checks whether the dimensions of q and qdot are correct + """ + + q = np.random.rand(self.model.q_size) + self.assertEqual (7, self.model.q_size) + self.assertEqual (6, self.model.qdot_size) + + def test_SetQuaternion (self): + mat = np.asarray ([[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]) + rbdl_quat = rbdl.Quaternion.fromPythonMatrix (mat) + ref_q = self.q.copy() + + self.model.SetQuaternion (2, rbdl_quat.toNumpy(), self.q) + + ref_q[3:6] = rbdl_quat[0:3] + ref_q[-1] = rbdl_quat[3] + + assert_array_equal (ref_q, self.q) + + def test_GetQuaternion (self): + mat = np.asarray ([[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]) + rbdl_quat = rbdl.Quaternion.fromPythonMatrix (mat) + + self.assertEqual (4, len(rbdl_quat)) + self.q[5] = math.sqrt(2.) * 0.5 + self.q[6] = math.sqrt(2.) * 0.5 + + ref_quat = [0., 0., math.sqrt(2.) * 0.5, math.sqrt(2.) * 0.5] + quat = self.model.GetQuaternion (2, self.q) + + assert_array_equal (np.asarray(ref_quat), quat) + +class ConstraintSetTests (unittest.TestCase): + def test_Simple (self): + # only tests whether the API seems to work. No functional + # tests yet. + + cs = rbdl.ConstraintSet() + idx = cs.AddContactConstraint (1, [1., 2., 3.], [4., 5., 6.]) + assert_equal (0, idx) + + X = rbdl.SpatialTransform() + sv = rbdl.SpatialVector.fromPythonArray ([1., 2., 3., 4., 5., 6.]) + idx2 = cs.AddLoopConstraint (1, 2, X, X, sv, 1.) + assert_equal (1, idx2) + +if __name__ == '__main__': + unittest.main() diff --git a/3rdparty/rbdl/python/wrappergen.py b/3rdparty/rbdl/python/wrappergen.py new file mode 100755 index 0000000..6eb597c --- /dev/null +++ b/3rdparty/rbdl/python/wrappergen.py @@ -0,0 +1,137 @@ +#!/usr/bin/python + +import sys, re, os + +def usage(arg0): + print ("Usage: {} ".format(arg0)) + sys.exit(-1) + +wrapper_command_strings = { + "ClassDefinitions" : """cdef class _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper: + cdef crbdl.%PARENT% *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [%TYPE%.fromPointer ( &(self.parent.%MEMBER%[i])) for i in xrange (*key.indices(len(self)))] + else: + return %TYPE%.fromPointer ( &(self.parent.%MEMBER%[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], %TYPE%), "Invalid type! Expected %TYPE%, but got " + str(type(value[src_index])) + "." + self.parent.%MEMBER%[i] = (<%TYPE%> value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, %TYPE%), "Invalid type! Expected %TYPE%, but got " + str(type(value)) + "." + self.parent.%MEMBER%[key] = (<%TYPE%> value).thisptr[0] + + def __len__(self): + return self.parent.%MEMBER%.size() +""", + + "MemberDefinitions" : """ cdef _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper %MEMBER%""", + + "CInitCode" : """ self.%MEMBER% = _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper ( self.thisptr)""", + + "AddProperty" : """ property %MEMBER%: + def __get__ (self): + return self.%MEMBER% +""" +} + +def parse_line (line_str): + command = "" + args = {} + + # remove comments + line_str = line_str.split("#")[0] + + wrapper_line_str_match = re.search ("%VectorWrapper(\S*)\s*\((.*)\).*%", line_str) + if (wrapper_line_str_match): + command = wrapper_line_str_match.group(1) + arg_str = wrapper_line_str_match.group(2) + arg_match = re.findall("(\s*(\S*)\s*=\s*(\w*)\s*,?)", arg_str) + if len(arg_match) > 0: + for arg in arg_match: + if len(arg) != 3: + print ("Invalid command args at line_str {}".format + (line_number)) + sys.exit(-1) + + args[arg[1]] = arg[2] + + return command, args + + return False, None + +if __name__ == "__main__": + if len(sys.argv) != 3: + usage (sys.argv[0]) + + infilename = sys.argv[1] + outfilename = sys.argv[2] + + print ("Processing {} to generate {}".format (infilename, outfilename)) + infile = open(infilename) + outfile = open(outfilename, "w") + outfile.write ("""# WARNING! +# +# This file was automatically created from {} using {}. +# Do not modify this file directly. Edit original source instead!! + +""".format (os.path.basename(infilename), os.path.basename(sys.argv[0]))) + + template = infile.read() + template_lines = template.split ("\n") + + # find the classes that will contain generated code + generated_parent_classes = [] + generated_parent_members = {} + for line_number, line_str in enumerate (template_lines): + command, args = parse_line (line_str) + if command: + if args["PARENT"] not in generated_parent_classes: + generated_parent_classes.append (args["PARENT"]) + generated_parent_members[args["PARENT"]] = [] + + if command=="AddProperty": + generated_parent_members[args["PARENT"]].append ({ + "TYPE": args["TYPE"], + "MEMBER": args["MEMBER"] + }) + + # generate code + for line_number, line_str in enumerate (template_lines): + command, args = parse_line (line_str) + if not command: + outfile.write (line_str + "\n") + else: +# print (command, wrapper_command_strings.keys()) + if command in wrapper_command_strings.keys(): + parent = args["PARENT"] + if command == "AddProperty": + content_type = args["TYPE"] + member_name = args["MEMBER"] + command_code = wrapper_command_strings[command][:] + command_code = command_code.replace ( + "%PARENT%", parent).replace ( + "%MEMBER%", member_name).replace ( + "%TYPE%", content_type) + outfile.write (command_code + "\n") + else: + for member in generated_parent_members[parent]: + content_type = member["TYPE"] + member_name = member["MEMBER"] + command_code = wrapper_command_strings[command][:] + command_code = command_code.replace ( + "%PARENT%", parent).replace ( + "%MEMBER%", member_name).replace ( + "%TYPE%", content_type) + outfile.write (command_code + "\n") diff --git a/3rdparty/rbdl/rbdl.pc.cmake b/3rdparty/rbdl/rbdl.pc.cmake new file mode 100644 index 0000000..2c4cdee --- /dev/null +++ b/3rdparty/rbdl/rbdl.pc.cmake @@ -0,0 +1,13 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=@CMAKE_INSTALL_FULL_LIBDIR@ +includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@ + +Name: RBDL +Description: Rigid Body Dynamics Library +URL: http://rbdl.bitbucket.org/ +Version: @RBDL_VERSION@ +Requires: eigen3 +Conflicts: +Libs: -L${libdir} -lrbdl -Wl,-rpath ${libdir} +Libs.private: +Cflags: -I${includedir} diff --git a/3rdparty/rbdl/src/Constraints.cc b/3rdparty/rbdl/src/Constraints.cc new file mode 100644 index 0000000..f2d208b --- /dev/null +++ b/3rdparty/rbdl/src/Constraints.cc @@ -0,0 +1,1537 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2015 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +void SolveLinearSystem ( + const MatrixNd& A, + const VectorNd& b, + VectorNd& x, + LinearSolver ls +); + +unsigned int GetMovableBodyId (Model& model, unsigned int id); + +unsigned int ConstraintSet::AddContactConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &world_normal, + const char *constraint_name, + double normal_acceleration + ) { + assert (bound == false); + + unsigned int n_constr = size() + 1; + + std::string name_str; + if (constraint_name != NULL) { + name_str = constraint_name; + } + + constraintType.push_back (ContactConstraint); + name.push_back (name_str); + contactConstraintIndices.push_back(size()); + + // These variables will be used for this type of constraint. + body.push_back (body_id); + point.push_back (body_point); + normal.push_back (world_normal); + + // These variables will not be used. + body_p.push_back (0); + body_s.push_back (0); + X_p.push_back (SpatialTransform()); + X_s.push_back (SpatialTransform()); + constraintAxis.push_back (SpatialVector::Zero()); + T_stab_inv.push_back (0.); + err.conservativeResize(n_constr); + err[n_constr - 1] = 0.; + errd.conservativeResize(n_constr); + errd[n_constr - 1] = 0.; + + acceleration.conservativeResize (n_constr); + acceleration[n_constr - 1] = normal_acceleration; + + force.conservativeResize (n_constr); + force[n_constr - 1] = 0.; + + impulse.conservativeResize (n_constr); + impulse[n_constr - 1] = 0.; + + v_plus.conservativeResize (n_constr); + v_plus[n_constr - 1] = 0.; + + d_multdof3_u = std::vector(n_constr, Math::Vector3d::Zero()); + + return n_constr - 1; +} + +unsigned int ConstraintSet::AddLoopConstraint ( + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + const Math::SpatialVector& axis, + double T_stabilization, + const char *constraint_name + ) { + assert (bound == false); + + unsigned int n_constr = size() + 1; + + std::string name_str; + if (constraint_name != NULL) { + name_str = constraint_name; + } + + constraintType.push_back(LoopConstraint); + name.push_back (name_str); + loopConstraintIndices.push_back(size()); + + // These variables will be used for this kind of constraint. + body_p.push_back (id_predecessor); + body_s.push_back (id_successor); + X_p.push_back (X_predecessor); + X_s.push_back (X_successor); + constraintAxis.push_back (axis); + T_stab_inv.push_back (1. / T_stabilization); + err.conservativeResize(n_constr); + err[n_constr - 1] = 0.; + errd.conservativeResize(n_constr); + errd[n_constr - 1] = 0.; + + // These variables will not be used. + body.push_back (0); + point.push_back (Vector3d::Zero()); + normal.push_back (Vector3d::Zero()); + + acceleration.conservativeResize (n_constr); + acceleration[n_constr - 1] = 0.; + + force.conservativeResize (n_constr); + force[n_constr - 1] = 0.; + + impulse.conservativeResize (n_constr); + impulse[n_constr - 1] = 0.; + + v_plus.conservativeResize (n_constr); + v_plus[n_constr - 1] = 0.; + + d_multdof3_u = std::vector(n_constr, Math::Vector3d::Zero()); + + return n_constr - 1; +} + +bool ConstraintSet::Bind (const Model &model) { + assert (bound == false); + + if (bound) { + std::cerr << "Error: binding an already bound constraint set!" << std::endl; + abort(); + } + unsigned int n_constr = size(); + + H.conservativeResize (model.dof_count, model.dof_count); + H.setZero(); + C.conservativeResize (model.dof_count); + C.setZero(); + gamma.conservativeResize (n_constr); + gamma.setZero(); + G.conservativeResize (n_constr, model.dof_count); + G.setZero(); + A.conservativeResize (model.dof_count + n_constr, model.dof_count + n_constr); + A.setZero(); + b.conservativeResize (model.dof_count + n_constr); + b.setZero(); + x.conservativeResize (model.dof_count + n_constr); + x.setZero(); + + Gi.conservativeResize (3, model.qdot_size); + GSpi.conservativeResize (6, model.qdot_size); + GSsi.conservativeResize (6, model.qdot_size); + GSJ.conservativeResize (6, model.qdot_size); + + // HouseHolderQR crashes if matrix G has more rows than columns. +#ifdef RBDL_USE_SIMPLE_MATH + GT_qr = SimpleMath::HouseholderQR (G.transpose()); +#else + GT_qr = Eigen::HouseholderQR (G.transpose()); +#endif + GT_qr_Q = MatrixNd::Zero (model.dof_count, model.dof_count); + Y = MatrixNd::Zero (model.dof_count, G.rows()); + Z = MatrixNd::Zero (model.dof_count, model.dof_count - G.rows()); + qddot_y = VectorNd::Zero (model.dof_count); + qddot_z = VectorNd::Zero (model.dof_count); + + K.conservativeResize (n_constr, n_constr); + K.setZero(); + a.conservativeResize (n_constr); + a.setZero(); + QDDot_t.conservativeResize (model.dof_count); + QDDot_t.setZero(); + QDDot_0.conservativeResize (model.dof_count); + QDDot_0.setZero(); + f_t.resize (n_constr, SpatialVector::Zero()); + f_ext_constraints.resize (model.mBodies.size(), SpatialVector::Zero()); + point_accel_0.resize (n_constr, Vector3d::Zero()); + + d_pA = std::vector (model.mBodies.size(), SpatialVector::Zero()); + d_a = std::vector (model.mBodies.size(), SpatialVector::Zero()); + d_u = VectorNd::Zero (model.mBodies.size()); + + d_IA = std::vector (model.mBodies.size() + , SpatialMatrix::Identity()); + d_U = std::vector (model.mBodies.size(), SpatialVector::Zero()); + d_d = VectorNd::Zero (model.mBodies.size()); + + d_multdof3_u = std::vector (model.mBodies.size() + , Math::Vector3d::Zero()); + + bound = true; + + return bound; +} + +void ConstraintSet::clear() { + acceleration.setZero(); + force.setZero(); + impulse.setZero(); + + H.setZero(); + C.setZero(); + gamma.setZero(); + G.setZero(); + A.setZero(); + b.setZero(); + x.setZero(); + + K.setZero(); + a.setZero(); + QDDot_t.setZero(); + QDDot_0.setZero(); + + unsigned int i; + for (i = 0; i < f_t.size(); i++) + f_t[i].setZero(); + + for (i = 0; i < f_ext_constraints.size(); i++) + f_ext_constraints[i].setZero(); + + for (i = 0; i < point_accel_0.size(); i++) + point_accel_0[i].setZero(); + + for (i = 0; i < d_pA.size(); i++) + d_pA[i].setZero(); + + for (i = 0; i < d_a.size(); i++) + d_a[i].setZero(); + + d_u.setZero(); +} + +RBDL_DLLAPI +void SolveConstrainedSystemDirect ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &A, + Math::VectorNd &b, + Math::VectorNd &x, + Math::LinearSolver &linear_solver + ) { + // Build the system: Copy H + A.block(0, 0, c.rows(), c.rows()) = H; + + // Copy G and G^T + A.block(0, c.rows(), c.rows(), gamma.rows()) = G.transpose(); + A.block(c.rows(), 0, gamma.rows(), c.rows()) = G; + + // Build the system: Copy -C + \tau + b.block(0, 0, c.rows(), 1) = c; + b.block(c.rows(), 0, gamma.rows(), 1) = gamma; + + LOG << "A = " << std::endl << A << std::endl; + LOG << "b = " << std::endl << b << std::endl; + + switch (linear_solver) { + case (LinearSolverPartialPivLU) : +#ifdef RBDL_USE_SIMPLE_MATH + // SimpleMath does not have a LU solver so just use its QR solver + x = A.householderQr().solve(b); +#else + x = A.partialPivLu().solve(b); +#endif + break; + case (LinearSolverColPivHouseholderQR) : + x = A.colPivHouseholderQr().solve(b); + break; + case (LinearSolverHouseholderQR) : + x = A.householderQr().solve(b); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } + + LOG << "x = " << std::endl << x << std::endl; +} + +RBDL_DLLAPI +void SolveConstrainedSystemRangeSpaceSparse ( + Model &model, + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &K, + Math::VectorNd &a, + Math::LinearSolver linear_solver + ) { + SparseFactorizeLTL (model, H); + + MatrixNd Y (G.transpose()); + + for (unsigned int i = 0; i < Y.cols(); i++) { + VectorNd Y_col = Y.block(0,i,Y.rows(),1); + SparseSolveLTx (model, H, Y_col); + Y.block(0,i,Y.rows(),1) = Y_col; + } + + VectorNd z (c); + SparseSolveLTx (model, H, z); + + K = Y.transpose() * Y; + + a = gamma - Y.transpose() * z; + + lambda = K.llt().solve(a); + + qddot = c + G.transpose() * lambda; + SparseSolveLTx (model, H, qddot); + SparseSolveLx (model, H, qddot); +} + +RBDL_DLLAPI +void SolveConstrainedSystemNullSpace ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &Y, + Math::MatrixNd &Z, + Math::VectorNd &qddot_y, + Math::VectorNd &qddot_z, + Math::LinearSolver &linear_solver + ) { + switch (linear_solver) { + case (LinearSolverPartialPivLU) : +#ifdef RBDL_USE_SIMPLE_MATH + // SimpleMath does not have a LU solver so just use its QR solver + qddot_y = (G * Y).householderQr().solve (gamma); +#else + qddot_y = (G * Y).partialPivLu().solve (gamma); +#endif + break; + case (LinearSolverColPivHouseholderQR) : + qddot_y = (G * Y).colPivHouseholderQr().solve (gamma); + break; + case (LinearSolverHouseholderQR) : + qddot_y = (G * Y).householderQr().solve (gamma); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } + + qddot_z = (Z.transpose() * H * Z).llt().solve(Z.transpose() * (c - H * Y * qddot_y)); + + qddot = Y * qddot_y + Z * qddot_z; + + switch (linear_solver) { + case (LinearSolverPartialPivLU) : +#ifdef RBDL_USE_SIMPLE_MATH + // SimpleMath does not have a LU solver so just use its QR solver + qddot_y = (G * Y).householderQr().solve (gamma); +#else + lambda = (G * Y).partialPivLu().solve (Y.transpose() * (H * qddot - c)); +#endif + break; + case (LinearSolverColPivHouseholderQR) : + lambda = (G * Y).colPivHouseholderQr().solve (Y.transpose() * (H * qddot - c)); + break; + case (LinearSolverHouseholderQR) : + lambda = (G * Y).householderQr().solve (Y.transpose() * (H * qddot - c)); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } +} + +RBDL_DLLAPI +void CalcConstraintsPositionError ( + Model& model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics + ) { + assert(err.size() == CS.size()); + + if(update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + for (unsigned int i = 0; i < CS.contactConstraintIndices.size(); i++) { + const unsigned int c = CS.contactConstraintIndices[i]; + err[c] = 0.; + } + + for (unsigned int i = 0; i < CS.loopConstraintIndices.size(); i++) { + const unsigned int lci = CS.loopConstraintIndices[i]; + + // Variables used for computations. + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + SpatialVector d; + + // Constraints computed in the predecessor body frame. + + // Compute the orientation of the two constraint frames. + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[lci], false).transpose() + * CS.X_p[lci].E; + rot_s = CalcBodyWorldOrientation (model, Q, CS.body_s[lci], false).transpose() + * CS.X_s[lci].E; + + // Compute the orientation from the predecessor to the successor frame. + rot_ps = rot_p.transpose() * rot_s; + + // Compute the position of the two contact points. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[lci], CS.X_p[lci].r + , false); + pos_s = CalcBodyToBaseCoordinates (model, Q, CS.body_s[lci], CS.X_s[lci].r + , false); + + // The first three elemenets represent the rotation error. + // This formulation is equivalent to u * sin(theta), where u and theta are + // the angle-axis of rotation from the predecessor to the successor frame. + // These quantities are expressed in the predecessor frame. + d[0] = -0.5 * (rot_ps(1,2) - rot_ps(2,1)); + d[1] = -0.5 * (rot_ps(2,0) - rot_ps(0,2)); + d[2] = -0.5 * (rot_ps(0,1) - rot_ps(1,0)); + + // The last three elements represent the position error. + // It is equivalent to the difference in the position of the two + // constraint points. + // The distance is projected on the predecessor frame to be consistent + // with the rotation. + d.block<3,1>(3,0) = rot_p.transpose() * (pos_s - pos_p); + + // Project the error on the constraint axis to find the actual error. + err[lci] = CS.constraintAxis[lci].transpose() * d; + } +} + +RBDL_DLLAPI +void CalcConstraintsJacobian ( + Model &model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::MatrixNd &G, + bool update_kinematics + ) { + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + // variables to check whether we need to recompute G. + ConstraintSet::ConstraintType prev_constraint_type + = ConstraintSet::ConstraintTypeLast; + unsigned int prev_body_id_1 = 0; + unsigned int prev_body_id_2 = 0; + SpatialTransform prev_body_X_1; + SpatialTransform prev_body_X_2; + + for (unsigned int i = 0; i < CS.contactConstraintIndices.size(); i++) { + const unsigned int c = CS.contactConstraintIndices[i]; + + // only compute the matrix Gi if actually needed + if (prev_constraint_type != CS.constraintType[c] + || prev_body_id_1 != CS.body[c] + || prev_body_X_1.r != CS.point[c]) { + + // Compute the jacobian for the point. + CS.Gi.setZero(); + CalcPointJacobian (model, Q, CS.body[c], CS.point[c], CS.Gi, false); + prev_constraint_type = ConstraintSet::ContactConstraint; + + // Update variables for optimization check. + prev_body_id_1 = CS.body[c]; + prev_body_X_1 = Xtrans(CS.point[c]); + } + + for(unsigned int j = 0; j < model.dof_count; j++) { + Vector3d gaxis (CS.Gi(0,j), CS.Gi(1,j), CS.Gi(2,j)); + G(c,j) = gaxis.transpose() * CS.normal[c]; + } + } + + // Variables used for computations. + Vector3d normal; + SpatialVector axis; + Vector3d pos_p; + Matrix3d rot_p; + SpatialTransform X_0p; + + for (unsigned int i = 0; i < CS.loopConstraintIndices.size(); i++) { + const unsigned int c = CS.loopConstraintIndices[i]; + + // Only recompute variables if necessary. + if( prev_body_id_1 != CS.body_p[c] + || prev_body_id_2 != CS.body_s[c] + || prev_body_X_1.r != CS.X_p[c].r + || prev_body_X_2.r != CS.X_s[c].r + || prev_body_X_1.E != CS.X_p[c].E + || prev_body_X_2.E != CS.X_s[c].E) { + + // Compute the 6D jacobians of the two contact points. + CS.GSpi.setZero(); + CS.GSsi.setZero(); + CalcPointJacobian6D(model, Q, CS.body_p[c], CS.X_p[c].r, CS.GSpi, false); + CalcPointJacobian6D(model, Q, CS.body_s[c], CS.X_s[c].r, CS.GSsi, false); + CS.GSJ = CS.GSsi - CS.GSpi; + + // Compute position and rotation matrix from predecessor body to base. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c], CS.X_p[c].r + , false); + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c] + , false).transpose()* CS.X_p[c].E; + X_0p = SpatialTransform (rot_p, pos_p); + + // Update variables for optimization check. + prev_constraint_type = ConstraintSet::LoopConstraint; + prev_body_id_1 = CS.body_p[c]; + prev_body_id_2 = CS.body_s[c]; + prev_body_X_1 = CS.X_p[c]; + prev_body_X_2 = CS.X_s[c]; + } + + // Express the constraint axis in the base frame. + axis = X_0p.apply(CS.constraintAxis[c]); + + // Compute the constraint Jacobian row. + G.block(c, 0, 1, model.dof_count) = axis.transpose() * CS.GSJ; + } +} + +RBDL_DLLAPI +void CalcConstraintsVelocityError ( + Model& model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics + ) { + MatrixNd G(MatrixNd::Zero(CS.size(), model.dof_count)); + CalcConstraintsJacobian (model, Q, CS, G, update_kinematics); + err = G * QDot; +} + +RBDL_DLLAPI +void CalcConstrainedSystemVariables ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS + ) { + // Compute C + NonlinearEffects(model, Q, QDot, CS.C); + assert(CS.H.cols() == model.dof_count && CS.H.rows() == model.dof_count); + + // Compute H + CS.H.setZero(); + CompositeRigidBodyAlgorithm(model, Q, CS.H, false); + + // Compute G + // We have to update model.X_base as they are not automatically computed + // by NonlinearEffects() + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + model.X_base[i] = model.X_lambda[i] * model.X_base[model.lambda[i]]; + } + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + // Compute position error for Baumgarte Stabilization. + CalcConstraintsPositionError (model, Q, CS, CS.err, false); + + // Compute velocity error for Baugarte stabilization. + CS.errd = CS.G * QDot; + + // Compute gamma + unsigned int prev_body_id = 0; + Vector3d prev_body_point = Vector3d::Zero(); + Vector3d gamma_i = Vector3d::Zero(); + + CS.QDDot_0.setZero(); + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0); + + for (unsigned int i = 0; i < CS.contactConstraintIndices.size(); i++) { + const unsigned int c = CS.contactConstraintIndices[i]; + + // only compute point accelerations when necessary + if (prev_body_id != CS.body[c] || prev_body_point != CS.point[c]) { + gamma_i = CalcPointAcceleration (model, Q, QDot, CS.QDDot_0, CS.body[c] + , CS.point[c], false); + prev_body_id = CS.body[c]; + prev_body_point = CS.point[c]; + } + + // we also substract ContactData[c].acceleration such that the contact + // point will have the desired acceleration + CS.gamma[c] = CS.acceleration[c] - CS.normal[c].dot(gamma_i); + } + + for (unsigned int i = 0; i < CS.loopConstraintIndices.size(); i++) { + const unsigned int c = CS.loopConstraintIndices[i]; + + // Variables used for computations. + Vector3d pos_p; + Matrix3d rot_p; + SpatialVector vel_p; + SpatialVector vel_s; + SpatialVector axis; + unsigned int id_p; + unsigned int id_s; + + // Force recomputation. + prev_body_id = 0; + + // Express the constraint axis in the base frame. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c], CS.X_p[c].r + , false); + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c], false).transpose() + * CS.X_p[c].E; + axis = SpatialTransform (rot_p, pos_p).apply(CS.constraintAxis[c]); + + // Compute the spatial velocities of the two constrained bodies. + vel_p = CalcPointVelocity6D (model, Q, QDot, CS.body_p[c], CS.X_p[c].r + , false); + vel_s = CalcPointVelocity6D (model, Q, QDot, CS.body_s[c], CS.X_s[c].r + , false); + + // Check if the bodies involved in the constraint are fixed. If yes, find + // their movable parent to access the right value in the a vector. + // This is needed because we access the model.a vector directly later. + id_p = GetMovableBodyId (model, CS.body_p[c]); + id_s = GetMovableBodyId (model, CS.body_s[c]); + + // Problem here if one of the bodies is fixed... + // Compute the value of gamma. + CS.gamma[c] + // Right hand side term. + = - axis.transpose() * (model.a[id_s] - model.a[id_p] + + crossm(vel_s, vel_p)) + // Baumgarte stabilization term. + - 2. * CS.T_stab_inv[c] * CS.errd[c] + - CS.T_stab_inv[c] * CS.T_stab_inv[c] * CS.err[c]; + } +} + +RBDL_DLLAPI +bool CalcAssemblyQ ( + Model &model, + Math::VectorNd QInit, // Note: passed by value intentionally + ConstraintSet &cs, + Math::VectorNd &Q, + const Math::VectorNd &weights, + double tolerance, + unsigned int max_iter + ) { + + if(Q.size() != model.q_size) { + std::cerr << "Incorrect Q vector size." << std::endl; + assert(false); + abort(); + } + if(QInit.size() != model.q_size) { + std::cerr << "Incorrect QInit vector size." << std::endl; + assert(false); + abort(); + } + if(weights.size() != model.dof_count) { + std::cerr << "Incorrect weights vector size." << std::endl; + assert(false); + abort(); + } + + // Initialize variables. + MatrixNd constraintJac (cs.size(), model.dof_count); + MatrixNd A = MatrixNd::Zero (cs.size() + model.dof_count, cs.size() + + model.dof_count); + VectorNd b = VectorNd::Zero (cs.size() + model.dof_count); + VectorNd x = VectorNd::Zero (cs.size() + model.dof_count); + VectorNd d = VectorNd::Zero (model.dof_count); + VectorNd e = VectorNd::Zero (cs.size()); + + // The top-left block is the weight matrix and is constant. + for(unsigned int i = 0; i < weights.size(); ++i) { + A(i,i) = weights[i]; + } + + // Check if the error is small enough already. If so, just return the initial + // guess as the solution. + CalcConstraintsPositionError (model, QInit, cs, e); + if (e.norm() < tolerance) { + Q = QInit; + return true; + } + + // We solve the linearized problem iteratively. + // Iterations are stopped if the maximum is reached. + for(unsigned int it = 0; it < max_iter; ++it) { + // Compute the constraint jacobian and build A and b. + constraintJac.setZero(); + CalcConstraintsJacobian (model, QInit, cs, constraintJac); + A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac; + A.block (0, model.dof_count, model.dof_count, cs.size()) + = constraintJac.transpose(); + b.block (model.dof_count, 0, cs.size(), 1) = -e; + + // Solve the sistem A*x = b. + SolveLinearSystem (A, b, x, cs.linear_solver); + + // Extract the d = (Q - QInit) vector from x. + d = x.block (0, 0, model.dof_count, 1); + + // Update solution. + for (size_t i = 0; i < model.mJoints.size(); ++i) { + // If the joint is spherical, translate the corresponding components + // of d into a modification in the joint quaternion. + if (model.mJoints[i].mJointType == JointTypeSpherical) { + Quaternion quat = model.GetQuaternion(i, QInit); + Vector3d omega = d.block<3,1>(model.mJoints[i].q_index,0); + // Convert the 3d representation of the displacement to 4d and sum it + // to the components of the quaternion. + quat += quat.omegaToQDot(omega); + // The quaternion needs to be normalized after the previous sum. + quat /= quat.norm(); + model.SetQuaternion(i, quat, QInit); + } + // If the current joint is not spherical, simply add the corresponding + // components of d to QInit. + else { + unsigned int qIdx = model.mJoints[i].q_index; + for(size_t j = 0; j < model.mJoints[i].mDoFCount; ++j) { + QInit[qIdx + j] += d[qIdx + j]; + } + // QInit.block(qIdx, 0, model.mJoints[i].mDoFCount, 1) + // += d.block(model.mJoints[i].q_index, 0, model.mJoints[i].mDoFCount, 1); + } + } + + // Update the errors. + CalcConstraintsPositionError (model, QInit, cs, e); + + // Check if the error and the step are small enough to end. + if (e.norm() < tolerance && d.norm() < tolerance){ + Q = QInit; + return true; + } + } + + // Return false if maximum number of iterations is exceeded. + Q = QInit; + return false; +} + +RBDL_DLLAPI +void CalcAssemblyQDot ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotInit, + ConstraintSet &cs, + Math::VectorNd &QDot, + const Math::VectorNd &weights + ) { + if(QDot.size() != model.dof_count) { + std::cerr << "Incorrect QDot vector size." << std::endl; + assert(false); + abort(); + } + if(Q.size() != model.q_size) { + std::cerr << "Incorrect Q vector size." << std::endl; + assert(false); + abort(); + } + if(QDotInit.size() != QDot.size()) { + std::cerr << "Incorrect QDotInit vector size." << std::endl; + assert(false); + abort(); + } + if(weights.size() != QDot.size()) { + std::cerr << "Incorrect weight vector size." << std::endl; + assert(false); + abort(); + } + + // Initialize variables. + MatrixNd constraintJac = MatrixNd::Zero(cs.size(), model.dof_count); + MatrixNd A = MatrixNd::Zero(cs.size() + model.dof_count, cs.size() + + model.dof_count); + VectorNd b = VectorNd::Zero(cs.size() + model.dof_count); + VectorNd x = VectorNd::Zero(cs.size() + model.dof_count); + + // The top-left block is the weight matrix and is constant. + for(unsigned int i = 0; i < weights.size(); ++i) { + A(i,i) = weights[i]; + b[i] = weights[i] * QDotInit[i]; + } + CalcConstraintsJacobian (model, Q, cs, constraintJac); + A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac; + A.block (0, model.dof_count, model.dof_count, cs.size()) + = constraintJac.transpose(); + + // Solve the sistem A*x = b. + SolveLinearSystem (A, b, x, cs.linear_solver); + + // Copy the result to the output variable. + QDot = x.block (0, 0, model.dof_count, 1); +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsDirect ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS); + + SolveConstrainedSystemDirect (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot + , CS.force, CS.A, CS.b, CS.x, CS.linear_solver); + + // Copy back QDDot + for (unsigned int i = 0; i < model.dof_count; i++) + QDDot[i] = CS.x[i]; + + // Copy back contact forces + for (unsigned int i = 0; i < CS.size(); i++) { + CS.force[i] = -CS.x[model.dof_count + i]; + } +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot + ) { + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS); + + SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, Tau - CS.C + , CS.gamma, QDDot, CS.force, CS.K, CS.a, CS.linear_solver); +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsNullSpace ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + + LOG << "-------- " << __func__ << " --------" << std::endl; + + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS); + + CS.GT_qr.compute (CS.G.transpose()); +#ifdef RBDL_USE_SIMPLE_MATH + CS.GT_qr_Q = CS.GT_qr.householderQ(); +#else + CS.GT_qr.householderQ().evalTo (CS.GT_qr_Q); +#endif + + CS.Y = CS.GT_qr_Q.block (0,0,QDot.rows(), CS.G.rows()); + CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDot.rows(), QDot.rows() - CS.G.rows()); + + SolveConstrainedSystemNullSpace (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot + , CS.force, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z, CS.linear_solver); + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + SolveConstrainedSystemDirect (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus + , QDotPlus, CS.impulse, CS.A, CS.b, CS.x, CS.linear_solver); + + // Copy back QDotPlus + for (unsigned int i = 0; i < model.dof_count; i++) + QDotPlus[i] = CS.x[i]; + + // Copy back constraint impulses + for (unsigned int i = 0; i < CS.size(); i++) { + CS.impulse[i] = CS.x[model.dof_count + i]; + } + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, CS.H * QDotMinus + , CS.v_plus, QDotPlus, CS.impulse, CS.K, CS.a, CS.linear_solver); + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + CS.GT_qr.compute(CS.G.transpose()); + CS.GT_qr_Q = CS.GT_qr.householderQ(); + + CS.Y = CS.GT_qr_Q.block (0,0,QDotMinus.rows(), CS.G.rows()); + CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDotMinus.rows(), QDotMinus.rows() + - CS.G.rows()); + + SolveConstrainedSystemNullSpace (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus + , QDotPlus, CS.impulse, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z + , CS.linear_solver); +} + +/** \brief Compute only the effects of external forces on the generalized accelerations + * + * This function is a reduced version of ForwardDynamics() which only + * computes the effects of the external forces on the generalized + * accelerations. + * + */ +RBDL_DLLAPI +void ForwardDynamicsApplyConstraintForces ( + Model &model, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (QDDot.size() == model.dof_count); + + unsigned int i = 0; + + for (i = 1; i < model.mBodies.size(); i++) { + model.IA[i] = model.I[i].toMatrix();; + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (CS.f_ext_constraints[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i]; + } + } + + // ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + unsigned int lambda = model.lambda[i]; + model.multdof3_u[i] = Vector3d (Tau[q_index], + Tau[q_index + 1], + Tau[q_index + 2]) + - model.multdof3_S[i].transpose() * model.pA[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose()); + + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() + << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.U[i] * (model.U[i] / model.d[i]).transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + model.U[i] * model.u[i] / model.d[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if(model.mJoints[i].mJointType == JointTypeCustom) { + + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + unsigned int lambda = model.lambda[i]; + VectorNd tau_temp = VectorNd::Zero(dofI); + + for(int z=0; zu = tau_temp + - (model.mCustomJoints[kI]->S.transpose() + * model.pA[i]); + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); + + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() + << std::endl; + } + } + } + + model.a[0] = SpatialVector (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * + (model.multdof3_u[i] + - model.multdof3_U[i].transpose() * model.a[i]); + + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd qdd_temp = VectorNd::Zero(dofI); + + qdd_temp = model.mCustomJoints[kI]->Dinv + * (model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() + * model.a[i]); + + for(int z=0; zS * qdd_temp); + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +/** \brief Computes the effect of external forces on the generalized accelerations. + * + * This function is essentially similar to ForwardDynamics() except that it + * tries to only perform computations of variables that change due to + * external forces defined in f_t. + */ +RBDL_DLLAPI +void ForwardDynamicsAccelerationDeltas ( + Model &model, + ConstraintSet &CS, + VectorNd &QDDot_t, + const unsigned int body_id, + const std::vector &f_t + ) { + LOG << "-------- " << __func__ << " ------" << std::endl; + + assert (CS.d_pA.size() == model.mBodies.size()); + assert (CS.d_a.size() == model.mBodies.size()); + assert (CS.d_u.size() == model.mBodies.size()); + + // TODO reset all values (debug) + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + CS.d_pA[i].setZero(); + CS.d_a[i].setZero(); + CS.d_u[i] = 0.; + CS.d_multdof3_u[i].setZero(); + } + for(unsigned int i=0; id_u.setZero(); + } + + for (unsigned int i = body_id; i > 0; i--) { + if (i == body_id) { + CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]); + } + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + CS.d_multdof3_u[i] = - model.multdof3_S[i].transpose() * (CS.d_pA[i]); + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + CS.d_pA[lambda] = CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + (model.multdof3_U[i] + * model.multdof3_Dinv[i] + * CS.d_multdof3_u[i])); + } + } else if(model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]); + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + CS.d_pA[lambda] = CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]); + } + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + //CS. + model.mCustomJoints[kI]->d_u = + - model.mCustomJoints[kI]->S.transpose() * (CS.d_pA[i]); + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + CS.d_pA[lambda] = + CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->d_u) + ); + } + } + } + + for (unsigned int i = 0; i < f_t.size(); i++) { + LOG << "f_t[" << i << "] = " << f_t[i].transpose() << std::endl; + } + + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + LOG << "i = " << i << ": d_pA[i] " << CS.d_pA[i].transpose() << std::endl; + } + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + LOG << "i = " << i << ": d_u[i] = " << CS.d_u[i] << std::endl; + } + + QDDot_t[0] = 0.; + CS.d_a[0] = model.a[0]; + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + SpatialVector Xa = model.X_lambda[i].apply(CS.d_a[lambda]); + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] + * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa); + + QDDot_t[q_index] = qdd_temp[0]; + QDDot_t[q_index + 1] = qdd_temp[1]; + QDDot_t[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + CS.d_a[i] = Xa + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom){ + + QDDot_t[q_index] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i]; + CS.d_a[i] = Xa + model.S[i] * QDDot_t[q_index]; + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd qdd_temp = VectorNd::Zero(dofI); + + qdd_temp = model.mCustomJoints[kI]->Dinv + * (model.mCustomJoints[kI]->d_u + - model.mCustomJoints[kI]->U.transpose() * Xa); + + for(int z=0; zS * qdd_temp; + CS.d_a[i] = Xa + model.mCustomJoints[kI]->S * qdd_temp; + } + + LOG << "QDDot_t[" << i - 1 << "] = " << QDDot_t[i - 1] << std::endl; + LOG << "d_a[i] = " << CS.d_a[i].transpose() << std::endl; + } +} + +inline void set_zero (std::vector &spatial_values) { + for (unsigned int i = 0; i < spatial_values.size(); i++) + spatial_values[i].setZero(); +} + +RBDL_DLLAPI +void ForwardDynamicsContactsKokkevis ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + LOG << "-------- " << __func__ << " ------" << std::endl; + + assert (CS.f_ext_constraints.size() == model.mBodies.size()); + assert (CS.QDDot_0.size() == model.dof_count); + assert (CS.QDDot_t.size() == model.dof_count); + assert (CS.f_t.size() == CS.size()); + assert (CS.point_accel_0.size() == CS.size()); + assert (CS.K.rows() == CS.size()); + assert (CS.K.cols() == CS.size()); + assert (CS.force.size() == CS.size()); + assert (CS.a.size() == CS.size()); + + Vector3d point_accel_t; + + unsigned int ci = 0; + + // The default acceleration only needs to be computed once + { + SUPPRESS_LOGGING; + ForwardDynamics(model, Q, QDot, Tau, CS.QDDot_0); + } + + LOG << "=== Initial Loop Start ===" << std::endl; + // we have to compute the standard accelerations first as we use them to + // compute the effects of each test force + for(ci = 0; ci < CS.size(); ci++) { + + { + SUPPRESS_LOGGING; + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0); + } + + if(CS.constraintType[ci] == ConstraintSet::ContactConstraint) + { + LOG << "body_id = " << CS.body[ci] << std::endl; + LOG << "point = " << CS.point[ci] << std::endl; + LOG << "normal = " << CS.normal[ci] << std::endl; + LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl; + { + SUPPRESS_LOGGING; + CS.point_accel_0[ci] = CalcPointAcceleration (model, Q, QDot + , CS.QDDot_0, CS.body[ci], CS.point[ci], false); + CS.a[ci] = - CS.acceleration[ci] + + CS.normal[ci].dot(CS.point_accel_0[ci]); + } + LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose(); + } + else + { + std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \ + type." << std::endl; + assert(false); + abort(); + } + } + + // Now we can compute and apply the test forces and use their net effect + // to compute the inverse articlated inertia to fill K. + for (ci = 0; ci < CS.size(); ci++) { + + LOG << "=== Testforce Loop Start ===" << std::endl; + + unsigned int movable_body_id = 0; + Vector3d point_global; + + switch (CS.constraintType[ci]) { + + case ConstraintSet::ContactConstraint: + + movable_body_id = GetMovableBodyId(model, CS.body[ci]); + + // assemble the test force + LOG << "normal = " << CS.normal[ci].transpose() << std::endl; + + point_global = CalcBodyToBaseCoordinates(model, Q, CS.body[ci] + , CS.point[ci], false); + + LOG << "point_global = " << point_global.transpose() << std::endl; + + CS.f_t[ci] = SpatialTransform(Matrix3d::Identity(), -point_global) + .applyAdjoint(SpatialVector (0., 0., 0. + , -CS.normal[ci][0], -CS.normal[ci][1], -CS.normal[ci][2])); + CS.f_ext_constraints[movable_body_id] = CS.f_t[ci]; + + LOG << "f_t[" << movable_body_id << "] = " << CS.f_t[ci].transpose() + << std::endl; + + { + ForwardDynamicsAccelerationDeltas(model, CS, CS.QDDot_t + , movable_body_id, CS.f_ext_constraints); + + LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl; + LOG << "QDDot_t = " << (CS.QDDot_t + CS.QDDot_0).transpose() + << std::endl; + LOG << "QDDot_t - QDDot_0 = " << (CS.QDDot_t).transpose() << std::endl; + } + + CS.f_ext_constraints[movable_body_id].setZero(); + + CS.QDDot_t += CS.QDDot_0; + + // compute the resulting acceleration + { + SUPPRESS_LOGGING; + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_t); + } + + for(unsigned int cj = 0; cj < CS.size(); cj++) { + { + SUPPRESS_LOGGING; + + point_accel_t = CalcPointAcceleration(model, Q, QDot, CS.QDDot_t + , CS.body[cj], CS.point[cj], false); + } + + LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose() + << std::endl; + LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl; + + CS.K(ci,cj) = CS.normal[cj].dot(point_accel_t - CS.point_accel_0[cj]); + + } + + break; + + default: + + std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \ + type." << std::endl; + assert(false); + abort(); + + break; + + } + + } + + LOG << "K = " << std::endl << CS.K << std::endl; + LOG << "a = " << std::endl << CS.a << std::endl; + +#ifndef RBDL_USE_SIMPLE_MATH + switch (CS.linear_solver) { + case (LinearSolverPartialPivLU) : + CS.force = CS.K.partialPivLu().solve(CS.a); + break; + case (LinearSolverColPivHouseholderQR) : + CS.force = CS.K.colPivHouseholderQr().solve(CS.a); + break; + case (LinearSolverHouseholderQR) : + CS.force = CS.K.householderQr().solve(CS.a); + break; + default: + LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl; + assert (0); + break; + } +#else + bool solve_successful = LinSolveGaussElimPivot (CS.K, CS.a, CS.force); + assert (solve_successful); +#endif + + LOG << "f = " << CS.force.transpose() << std::endl; + + for (ci = 0; ci < CS.size(); ci++) { + unsigned int body_id = CS.body[ci]; + unsigned int movable_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + movable_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + CS.f_ext_constraints[movable_body_id] -= CS.f_t[ci] * CS.force[ci]; + LOG << "f_ext[" << movable_body_id << "] = " << CS.f_ext_constraints[movable_body_id].transpose() << std::endl; + } + + { + SUPPRESS_LOGGING; + ForwardDynamicsApplyConstraintForces (model, Tau, CS, QDDot); + } + + LOG << "QDDot after applying f_ext: " << QDDot.transpose() << std::endl; +} + +void SolveLinearSystem ( + const MatrixNd& A, + const VectorNd& b, + VectorNd& x, + LinearSolver ls + ) { + if(A.rows() != b.size() || A.cols() != x.size()) { + std::cerr << "Mismatching sizes." << std::endl; + assert(false); + abort(); + } + + // Solve the sistem A*x = b. + switch (ls) { + case (LinearSolverPartialPivLU) : + #ifdef RBDL_USE_SIMPLE_MATH + // SimpleMath does not have a LU solver so just use its QR solver + x = A.householderQr().solve(b); + #else + x = A.partialPivLu().solve(b); + #endif + break; + case (LinearSolverColPivHouseholderQR) : + x = A.colPivHouseholderQr().solve(b); + break; + case (LinearSolverHouseholderQR) : + x = A.householderQr().solve(b); + break; + default: + std::cerr << "Error: Invalid linear solver: " << ls << std::endl; + assert(false); + abort(); + break; + } +} + +unsigned int GetMovableBodyId (Model& model, unsigned int id) { + if(model.IsFixedBodyId(id)) { + unsigned int fbody_id = id - model.fixed_body_discriminator; + return model.mFixedBodies[fbody_id].mMovableParent; + } + else { + return id; + } +} + +} /* namespace RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/Dynamics.cc b/3rdparty/rbdl/src/Dynamics.cc new file mode 100644 index 0000000..228fd00 --- /dev/null +++ b/3rdparty/rbdl/src/Dynamics.cc @@ -0,0 +1,863 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void InverseDynamics ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + VectorNd &Tau, + std::vector *f_ext) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.multdof3_S[i] * Vector3d (QDDot[q_index], + QDDot[q_index + 1], + QDDot[q_index + 2]); + } + }else if(model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[i].custom_joint_index; + VectorNd customJointQDDot(model.mCustomJoints[k]->mDoFCount); + for(int z=0; zmDoFCount; ++z){ + customJointQDDot[z] = QDDot[q_index+z]; + } + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.mCustomJoints[k]->S * customJointQDDot; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + } + + if (f_ext != NULL) { + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } else if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) + = model.multdof3_S[i].transpose() * model.f[i]; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[i].custom_joint_index; + Tau.block(model.mJoints[i].q_index,0, + model.mCustomJoints[k]->mDoFCount, 1) + = model.mCustomJoints[k]->S.transpose() * model.f[i]; + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI void NonlinearEffects ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + VectorNd &Tau) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0] = spatial_gravity; + + for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) { + jcalc (model, model.mJointUpdateOrder[i], Q, QDot); + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.lambda[i] == 0) { + model.v[i] = model.v_J[i]; + model.a[i] = model.X_lambda[i].apply(spatial_gravity); + } else { + model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + Tau[model.mJoints[i].q_index] + = model.S[i].dot(model.f[i]); + } else if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) + = model.multdof3_S[i].transpose() * model.f[i]; + } + } else if(model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[i].custom_joint_index; + Tau.block(model.mJoints[i].q_index,0, + model.mCustomJoints[k]->mDoFCount, 1) + = model.mCustomJoints[k]->S.transpose() * model.f[i]; + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI void CompositeRigidBodyAlgorithm ( + Model& model, + const VectorNd &Q, + MatrixNd &H, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + assert (H.rows() == model.dof_count && H.cols() == model.dof_count); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (update_kinematics) { + jcalc_X_lambda_S (model, i, Q); + } + model.Ic[i] = model.I[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.lambda[i] != 0) { + model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]); + } + + unsigned int dof_index_i = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + SpatialVector F = model.Ic[i] * model.S[i]; + H(dof_index_i, dof_index_i) = model.S[i].dot(F); + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F = model.X_lambda[j].applyTranspose(F); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom) { + if (model.mJoints[j].mDoFCount == 1) { + H(dof_index_i,dof_index_j) = F.dot(model.S[j]); + H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j); + } else if (model.mJoints[j].mDoFCount == 3) { + Vector3d H_temp2 = + (F.transpose() * model.multdof3_S[j]).transpose(); + LOG << F.transpose() << std::endl + << model.multdof3_S[j] << std::endl; + LOG << H_temp2.transpose() << std::endl; + + H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose(); + H.block<3,1>(dof_index_j,dof_index_i) = H_temp2; + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + VectorNd H_temp2 = + (F.transpose() * model.mCustomJoints[k]->S).transpose(); + + LOG << F.transpose() + << std::endl + << model.mCustomJoints[j]->S << std::endl; + + LOG << H_temp2.transpose() << std::endl; + + H.block(dof_index_i,dof_index_j,1,dof) = H_temp2.transpose(); + H.block(dof_index_j,dof_index_i,dof,1) = H_temp2; + } + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i]; + H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + Vector3d H_temp2 = F_63.transpose() * (model.S[j]); + + H.block<3,1>(dof_index_i,dof_index_j) = H_temp2; + H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } else if (model.mJoints[j].mDoFCount == 3) { + Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]); + + H.block<3,3>(dof_index_i,dof_index_j) = H_temp2; + H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + + MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S); + + H.block(dof_index_i,dof_index_j,3,dof) = H_temp2; + H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose(); + } + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + + MatrixNd F_Nd = model.Ic[i].toMatrix() + * model.mCustomJoints[kI]->S; + + H.block(dof_index_i, dof_index_i,dofI,dofI) + = model.mCustomJoints[kI]->S.transpose() * F_Nd; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_Nd = model.X_lambda[j].toMatrixTranspose() * (F_Nd); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]); + H.block( dof_index_i, dof_index_j, + H_temp2.rows(),H_temp2.cols()) = H_temp2; + H.block(dof_index_j,dof_index_i, + H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose(); + } else if (model.mJoints[j].mDoFCount == 3) { + MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]); + H.block(dof_index_i, dof_index_j, + H_temp2.rows(),H_temp2.cols()) = H_temp2; + H.block(dof_index_j, dof_index_i, + H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose(); + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + + MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S); + + H.block(dof_index_i,dof_index_j,3,dof) = H_temp2; + H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose(); + } + } + } + } +} + +RBDL_DLLAPI void ForwardDynamics ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + VectorNd &QDDot, + std::vector *f_ext) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]); + + unsigned int i = 0; + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "QDot = " << QDot.transpose() << std::endl; + LOG << "Tau = " << Tau.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + else + model.X_base[i] = model.X_lambda[i]; + + model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i]; + + /* + LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl; + LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl; + LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl; + LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl; + LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl; + LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl; + */ + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.I[i].setSpatialMatrix (model.IA[i]); + + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + + // ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.U[i] + * (model.U[i] / model.d[i]).transpose(); + + SpatialVector pa = model.pA[i] + + Ia * model.c[i] + + model.U[i] * model.u[i] / model.d[i]; + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() + += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() + += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() + * model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() + * model.multdof3_U[i]).inverse(); +#endif + VectorNd tau_temp(Tau.block(q_index,0,3,1)); + model.multdof3_u[i] = tau_temp + - model.multdof3_S[i].transpose() * model.pA[i]; + + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose(); + SpatialVector pa = model.pA[i] + + Ia + * model.c[i] + + model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_u[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() + += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + + model.pA[lambda].noalias() + += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() + << std::endl; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + model.mCustomJoints[kI]->U = + model.IA[i] * model.mCustomJoints[kI]->S; + +#ifdef EIGEN_CORE_H + model.mCustomJoints[kI]->Dinv + = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U).inverse().eval(); +#else + model.mCustomJoints[kI]->Dinv + = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U).inverse(); +#endif + VectorNd tau_temp(Tau.block(q_index,0,dofI,1)); + model.mCustomJoints[kI]->u = tau_temp + - model.mCustomJoints[kI]->S.transpose() * model.pA[i]; + + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - (model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); + SpatialVector pa = model.pA[i] + + Ia * model.c[i] + + (model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() + << std::endl; + } + } + } + + // ClearLogOutput(); + + model.a[0] = spatial_gravity * -1.; + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]); + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI=model.mCustomJoints[kI]->mDoFCount; + + VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv + * ( model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() + * model.a[i]); + + for(int z=0; zS * qdd_temp; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +RBDL_DLLAPI void ForwardDynamicsLagrangian ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + VectorNd &QDDot, + Math::LinearSolver linear_solver, + std::vector *f_ext, + Math::MatrixNd *H, + Math::VectorNd *C) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + bool free_H = false; + bool free_C = false; + + if (H == NULL) { + H = new MatrixNd (MatrixNd::Zero(model.dof_count, model.dof_count)); + free_H = true; + } + + if (C == NULL) { + C = new VectorNd (VectorNd::Zero(model.dof_count)); + free_C = true; + } + + // we set QDDot to zero to compute C properly with the InverseDynamics + // method. + QDDot.setZero(); + + InverseDynamics (model, Q, QDot, QDDot, (*C), f_ext); + CompositeRigidBodyAlgorithm (model, Q, *H, false); + + LOG << "A = " << std::endl << *H << std::endl; + LOG << "b = " << std::endl << *C * -1. + Tau << std::endl; + +#ifndef RBDL_USE_SIMPLE_MATH + switch (linear_solver) { + case (LinearSolverPartialPivLU) : + QDDot = H->partialPivLu().solve (*C * -1. + Tau); + break; + case (LinearSolverColPivHouseholderQR) : + QDDot = H->colPivHouseholderQr().solve (*C * -1. + Tau); + break; + case (LinearSolverHouseholderQR) : + QDDot = H->householderQr().solve (*C * -1. + Tau); + break; + case (LinearSolverLLT) : + QDDot = H->llt().solve (*C * -1. + Tau); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } +#else + bool solve_successful = LinSolveGaussElimPivot (*H, *C * -1. + Tau, QDDot); + assert (solve_successful); +#endif + + if (free_C) { + delete C; + } + + if (free_H) { + delete H; + } + + LOG << "x = " << QDDot << std::endl; +} + +RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, + const VectorNd &Q, + const VectorNd &Tau, + VectorNd &QDDot, + bool update_kinematics) { + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) { + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + jcalc_X_lambda_S (model, model.mJointUpdateOrder[i], Q); + + model.v_J[i].setZero(); + model.v[i].setZero(); + model.c_J[i].setZero(); + model.pA[i].setZero(); + model.I[i].setSpatialMatrix (model.IA[i]); + } + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + model.pA[i].setZero(); + } + + // ClearLogOutput(); + + if (update_kinematics) { + // Compute Articulate Body Inertias + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - + model.U[i] * (model.U[i] / model.d[i]).transpose(); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#endif + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; + +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = + (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = + (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); +#endif + // LOG << "mCustomJoints[kI]->u[" << i << "] = " + //<< model.mCustomJoints[kI]->u[i].transpose() << std::endl; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose()); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += + model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += + model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#endif + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; + +#ifdef EIGEN_CORE_H + model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U + ).inverse().eval(); +#else + model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U + ).inverse(); +#endif + // LOG << "mCustomJoints[kI]->u[" << i << "] = " + //<< model.mCustomJoints[kI]->u.transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#endif + } + } + } + } + + // compute articulated bias forces + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i]; + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + + Vector3d tau_temp ( Tau[q_index], + Tau[q_index + 1], + Tau[q_index + 2]); + model.multdof3_u[i] = tau_temp + - model.multdof3_S[i].transpose()*model.pA[i]; + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialVector pa = model.pA[i] + + model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_u[i]; + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += + model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd tau_temp(Tau.block(q_index,0,dofI,1)); + + model.mCustomJoints[kI]->u = + tau_temp - ( model.mCustomJoints[kI]->S.transpose()* model.pA[i]); + // LOG << "mCustomJoints[kI]->u" + // << model.mCustomJoints[kI]->u.transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialVector pa = model.pA[i] + + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += + model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } + } + + // ClearLogOutput(); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i])*(model.u[i]-model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = + model.multdof3_Dinv[i] * (model.multdof3_u[i] + - model.multdof3_U[i].transpose()*model.a[i]); + + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + + VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv + * ( model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() * model.a[i]); + + for(int z=0; zS * qdd_temp; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +} /* namespace RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/Joint.cc b/3rdparty/rbdl/src/Joint.cc new file mode 100644 index 0000000..0c393d4 --- /dev/null +++ b/3rdparty/rbdl/src/Joint.cc @@ -0,0 +1,436 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void jcalc ( + Model &model, + unsigned int joint_id, + const VectorNd &q, + const VectorNd &qdot + ) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) { + model.X_J[joint_id] = Xrotx (q[model.mJoints[joint_id].q_index]); + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) { + model.X_J[joint_id] = Xroty (q[model.mJoints[joint_id].q_index]); + model.v_J[joint_id][1] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) { + model.X_J[joint_id] = Xrotz (q[model.mJoints[joint_id].q_index]); + model.v_J[joint_id][2] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) { + model.X_J[joint_id] = jcalc_XJ (model, joint_id, q); + jcalc_X_lambda_S(model, joint_id, q); + double Jqd = qdot[model.mJoints[joint_id].q_index]; + model.v_J[joint_id] = model.S[joint_id] * Jqd; + + Vector3d St = model.S[joint_id].block(0,0,3,1); + Vector3d c = model.X_J[joint_id].E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + c = St.cross(c); + c *= -Jqd * Jqd; + model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]); + } else if (model.mJoints[joint_id].mDoFCount == 1 && + model.mJoints[joint_id].mJointType != JointTypeCustom) { + model.X_J[joint_id] = jcalc_XJ (model, joint_id, q); + model.v_J[joint_id] = + model.S[joint_id] * qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { + model.X_J[joint_id] = + SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(), + Vector3d (0., 0., 0.)); + + model.multdof3_S[joint_id](0,0) = 1.; + model.multdof3_S[joint_id](1,1) = 1.; + model.multdof3_S[joint_id](2,2) = 1.; + + Vector3d omega (qdot[model.mJoints[joint_id].q_index], + qdot[model.mJoints[joint_id].q_index+1], + qdot[model.mJoints[joint_id].q_index+2]); + + model.v_J[joint_id] = SpatialVector ( + omega[0], omega[1], omega[2], + 0., 0., 0.); + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ); + + model.multdof3_S[joint_id](0,0) = -s1; + model.multdof3_S[joint_id](0,2) = 1.; + + model.multdof3_S[joint_id](1,0) = c1 * s2; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0.,0., 0.); + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0, + -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0, + s1, -c1 * s0, c1 * c0 + ); + + model.multdof3_S[joint_id](0,0) = c2 * c1; + model.multdof3_S[joint_id](0,1) = s2; + + model.multdof3_S[joint_id](1,0) = -s2 * c1; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = s1; + model.multdof3_S[joint_id](2,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 + c2*qdot2*qdot1, + -c2*c1*qdot2*qdot0 + s2*s1*qdot1*qdot0 - s2*qdot2*qdot1, + c1*qdot1*qdot0, + 0., 0., 0. + ); + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0, + -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0, + c1 * s0, - s1, c1 * c0); + + model.multdof3_S[joint_id](0,0) = s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = c2 * c1; + model.multdof3_S[joint_id](1,1) = -s2; + + model.multdof3_S[joint_id](2,0) = -s1; + model.multdof3_S[joint_id](2,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + c2*c1*qdot2*qdot0 - s2*s1*qdot1*qdot0 - s2*qdot2*qdot1, + -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 - c2*qdot2*qdot1, + -c1*qdot1*qdot0, + 0., 0., 0. + ); + } else if(model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ){ + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + model.X_J[joint_id].E = Matrix3d::Identity(); + model.X_J[joint_id].r = Vector3d (q0, q1, q2); + + model.multdof3_S[joint_id](3,0) = 1.; + model.multdof3_S[joint_id](4,1) = 1.; + model.multdof3_S[joint_id](5,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.); + } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { + const Joint &joint = model.mJoints[joint_id]; + CustomJoint *custom_joint = + model.mCustomJoints[joint.custom_joint_index]; + custom_joint->jcalc (model, joint_id, q, qdot); + } else { + std::cerr << "Error: invalid joint type " << model.mJoints[joint_id].mJointType << " at id " << joint_id << std::endl; + abort(); + } + + model.X_lambda[joint_id] = model.X_J[joint_id] * model.X_T[joint_id]; +} + +RBDL_DLLAPI Math::SpatialTransform jcalc_XJ ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mDoFCount == 1 + && model.mJoints[joint_id].mJointType != JointTypeCustom) { + if (model.mJoints[joint_id].mJointType == JointTypeRevolute) { + return Xrot (q[model.mJoints[joint_id].q_index], Vector3d ( + model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2] + )); + } else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) { + return Xtrans ( Vector3d ( + model.mJoints[joint_id].mJointAxes[0][3] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][4] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][5] + * q[model.mJoints[joint_id].q_index] + ) + ); + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) { + SpatialTransform rot = Xrot( + q[model.mJoints[joint_id].q_index], Vector3d ( + model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2] + )); + SpatialTransform trans = Xtrans ( Vector3d ( + model.mJoints[joint_id].mJointAxes[0][3] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][4] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][5] + * q[model.mJoints[joint_id].q_index] + ) + ); + return rot * trans; + } + } + std::cerr << "Error: invalid joint type: " << model.mJoints[joint_id].mJointType << std::endl; + abort(); + return SpatialTransform(); +} + +RBDL_DLLAPI void jcalc_X_lambda_S ( + Model &model, + unsigned int joint_id, + const VectorNd &q + ) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) { + model.X_lambda[joint_id] = + Xrotx (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id]; + model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) { + model.X_lambda[joint_id] = + Xroty (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id]; + model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) { + model.X_lambda[joint_id] = + Xrotz (q[model.mJoints[joint_id].q_index]) * model.X_T[joint_id]; + model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical){ + SpatialTransform XJ = jcalc_XJ (model, joint_id, q); + model.X_lambda[joint_id] = XJ * model.X_T[joint_id]; + // Set the joint axis + Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + + model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2], + trans[0], trans[1], trans[2]); + } else if (model.mJoints[joint_id].mDoFCount == 1 + && model.mJoints[joint_id].mJointType != JointTypeCustom){ + model.X_lambda[joint_id] = + jcalc_XJ (model, joint_id, q) * model.X_T[joint_id]; + model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; + } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { + model.X_lambda[joint_id] = SpatialTransform ( + model.GetQuaternion (joint_id, q).toMatrix(), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id].setZero(); + + model.multdof3_S[joint_id](0,0) = 1.; + model.multdof3_S[joint_id](1,1) = 1.; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id].setZero(); + + model.multdof3_S[joint_id](0,0) = -s1; + model.multdof3_S[joint_id](0,2) = 1.; + + model.multdof3_S[joint_id](1,0) = c1 * s2; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = - s2; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0, + -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0, + s1, -c1 * s0, c1 * c0 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id].setZero(); + + model.multdof3_S[joint_id](0,0) = c2 * c1; + model.multdof3_S[joint_id](0,1) = s2; + + model.multdof3_S[joint_id](1,0) = -s2 * c1; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = s1; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0, + -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0, + c1 * s0, - s1, c1 * c0 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id].setZero(); + + model.multdof3_S[joint_id](0,0) = s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = c2 * c1; + model.multdof3_S[joint_id](1,1) = -s2; + + model.multdof3_S[joint_id](2,0) = -s1; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d::Identity (3,3), + Vector3d (q0, q1, q2)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id].setZero(); + + model.multdof3_S[joint_id](3,0) = 1.; + model.multdof3_S[joint_id](4,1) = 1.; + model.multdof3_S[joint_id](5,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { + const Joint &joint = model.mJoints[joint_id]; + CustomJoint *custom_joint + = model.mCustomJoints[joint.custom_joint_index]; + + custom_joint->jcalc_X_lambda_S (model, joint_id, q); + } else { + std::cerr << "Error: invalid joint type!" << std::endl; + abort(); + } +} +} diff --git a/3rdparty/rbdl/src/Kinematics.cc b/3rdparty/rbdl/src/Kinematics.cc new file mode 100644 index 0000000..d21f5e7 --- /dev/null +++ b/3rdparty/rbdl/src/Kinematics.cc @@ -0,0 +1,918 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void UpdateKinematics( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + unsigned int i; + + SpatialVector spatial_gravity (0., + 0., + 0., + model.gravity[0], + model.gravity[1], + model.gravity[2]); + + model.a[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + model.X_lambda[i] = model.X_J[i] * model.X_T[i]; + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + } else { + model.X_base[i] = model.X_lambda[i]; + model.v[i] = model.v_J[i]; + } + + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp (QDDot[q_index], + QDDot[q_index + 1], + QDDot[q_index + 2]); + model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp; + } + } else { + unsigned int custom_index = model.mJoints[i].custom_joint_index; + const CustomJoint* custom_joint = model.mCustomJoints[custom_index]; + unsigned int joint_dof_count = custom_joint->mDoFCount; + + model.a[i] = model.a[i] + + ( model.mCustomJoints[custom_index]->S + * QDDot.block(q_index, 0, joint_dof_count, 1)); + } + } + + for (i = 1; i < model.mBodies.size(); i++) { + LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl; + } +} + +RBDL_DLLAPI void UpdateKinematicsCustom( + Model &model, + const VectorNd *Q, + const VectorNd *QDot, + const VectorNd *QDDot) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + unsigned int i; + + if (Q) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + VectorNd QDot_zero (VectorNd::Zero (model.q_size)); + + jcalc (model, i, (*Q), QDot_zero); + + model.X_lambda[i] = model.X_J[i] * model.X_T[i]; + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + } + } + + if (QDot) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, *Q, *QDot); + + if (lambda != 0) { + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } else { + model.v[i] = model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } + // LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl; + } + } + + // FIXME?: Changing QDot can alter body accelerations via c[] - update to QDot but not QDDot can result in incorrect a[] + if (QDDot) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + } else { + model.a[i] = model.c[i]; + } + + if( model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.a[i] + model.S[i] * (*QDDot)[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp ((*QDDot)[q_index], + (*QDDot)[q_index + 1], + (*QDDot)[q_index + 2]); + model.a[i] = model.a[i] + + model.multdof3_S[i] * omegadot_temp; + } + } else { + unsigned int k = model.mJoints[i].custom_joint_index; + + const CustomJoint* custom_joint = model.mCustomJoints[k]; + unsigned int joint_dof_count = custom_joint->mDoFCount; + + model.a[i] = model.a[i] + + ( (model.mCustomJoints[k]->S) + *(QDDot->block(q_index, 0, joint_dof_count, 1))); + } + } + } +} + +RBDL_DLLAPI Vector3d CalcBodyToBaseCoordinates ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_body_coordinates, + bool update_kinematics) { + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent; + + Matrix3d fixed_rotation = + model.mFixedBodies[fbody_id].mParentTransform.E.transpose(); + Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r; + + Matrix3d parent_body_rotation = model.X_base[parent_id].E.transpose(); + Vector3d parent_body_position = model.X_base[parent_id].r; + + return (parent_body_position + + (parent_body_rotation + * (fixed_position + fixed_rotation * (point_body_coordinates))) ); + } + + Matrix3d body_rotation = model.X_base[body_id].E.transpose(); + Vector3d body_position = model.X_base[body_id].r; + + return body_position + body_rotation * point_body_coordinates; +} + +RBDL_DLLAPI Vector3d CalcBaseToBodyCoordinates ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_base_coordinates, + bool update_kinematics) { + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent; + + Matrix3d fixed_rotation = model.mFixedBodies[fbody_id].mParentTransform.E; + Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r; + + Matrix3d parent_body_rotation = model.X_base[parent_id].E; + Vector3d parent_body_position = model.X_base[parent_id].r; + + return (fixed_rotation + * ( - fixed_position + - parent_body_rotation + * (parent_body_position - point_base_coordinates))); + } + + Matrix3d body_rotation = model.X_base[body_id].E; + Vector3d body_position = model.X_base[body_id].r; + + return body_rotation * (point_base_coordinates - body_position); +} + +RBDL_DLLAPI Matrix3d CalcBodyWorldOrientation( + Model &model, + const VectorNd &Q, + const unsigned int body_id, + bool update_kinematics) { + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + model.mFixedBodies[fbody_id].mBaseTransform = + model.mFixedBodies[fbody_id].mParentTransform + * model.X_base[model.mFixedBodies[fbody_id].mMovableParent]; + + return model.mFixedBodies[fbody_id].mBaseTransform.E; + } + + return model.X_base[body_id].E; +} + +RBDL_DLLAPI void CalcPointJacobian ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_position, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + SpatialTransform point_trans = + SpatialTransform (Matrix3d::Identity(), + CalcBodyToBaseCoordinates ( model, + Q, + body_id, + point_position, + false)); + + assert (G.rows() == 3 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + // e[j] is set to 1 if joint j contributes to the jacobian that we are + // computing. For all other joints the column will be zero. + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index, 3, 1) = + point_trans.apply( + model.X_base[j].inverse().apply( + model.S[j])).block(3,0,3,1); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 3, 3) = + ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.multdof3_S[j]).block(3,0,3,3); + } + } else { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0, q_index, 3, model.mCustomJoints[k]->mDoFCount) = + ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.mCustomJoints[k]->S).block( + 3,0,3,model.mCustomJoints[k]->mDoFCount); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI void CalcPointJacobian6D ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_position, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + SpatialTransform point_trans = + SpatialTransform (Matrix3d::Identity(), + CalcBodyToBaseCoordinates (model, + Q, + body_id, + point_position, + false)); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index, 6, 1) + = point_trans.apply( + model.X_base[j].inverse().apply( + model.S[j])).block(0,0,6,1); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 6, 3) + = ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.multdof3_S[j]).block(0,0,6,3); + } + } else { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0, q_index, 6, model.mCustomJoints[k]->mDoFCount) + = ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.mCustomJoints[k]->S).block( + 0,0,6,model.mCustomJoints[k]->mDoFCount); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI void CalcBodySpatialJacobian ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + SpatialTransform base_to_body; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id + - model.fixed_body_discriminator; + + reference_body_id = model + .mFixedBodies[fbody_id] + .mMovableParent; + + base_to_body = model.mFixedBodies[fbody_id] + .mParentTransform + * model.X_base[reference_body_id]; + } else { + base_to_body = model.X_base[reference_body_id]; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index,6,1) = + base_to_body.apply( + model.X_base[j] + .inverse() + .apply(model.S[j]) + ); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0,q_index,6,3) = + (base_to_body * model.X_base[j].inverse() + ).toMatrix() * model.multdof3_S[j]; + } + }else if(model.mJoints[j].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0,q_index,6,model.mCustomJoints[k]->mDoFCount ) = + (base_to_body * model.X_base[j].inverse() + ).toMatrix() * model.mCustomJoints[k]->S; + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI Vector3d CalcPointVelocity ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (model.IsBodyId(body_id)); + assert (model.q_size == Q.size()); + assert (model.qdot_size == QDot.size()); + + // Reset the velocity of the root body + model.v[0].setZero(); + + // update the Kinematics with zero acceleration + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, &QDot, NULL); + } + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false); + } + + SpatialVector point_spatial_velocity = + SpatialTransform ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point).apply(model.v[reference_body_id]); + + return Vector3d ( + point_spatial_velocity[3], + point_spatial_velocity[4], + point_spatial_velocity[5] + ); +} + +RBDL_DLLAPI Math::SpatialVector CalcPointVelocity6D( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (model.IsBodyId(body_id)); + assert (model.q_size == Q.size()); + assert (model.qdot_size == QDot.size()); + + // Reset the velocity of the root body + model.v[0].setZero(); + + // update the Kinematics with zero acceleration + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, &QDot, NULL); + } + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false); + } + + return SpatialTransform ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point).apply(model.v[reference_body_id]); +} + +RBDL_DLLAPI Vector3d CalcPointAcceleration ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) + UpdateKinematics (model, Q, QDot, QDDot); + + LOG << std::endl; + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false); + } + + SpatialTransform p_X_i ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point); + + SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]); + Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2] + ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5])); + SpatialVector p_a_i = p_X_i.apply(model.a[reference_body_id]); + + return Vector3d ( + p_a_i[3] + a_dash[0], + p_a_i[4] + a_dash[1], + p_a_i[5] + a_dash[2] + ); +} + +RBDL_DLLAPI SpatialVector CalcPointAcceleration6D( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) + UpdateKinematics (model, Q, QDot, QDDot); + + LOG << std::endl; + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false); + } + + SpatialTransform p_X_i ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point); + + SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]); + Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2] + ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5])); + return (p_X_i.apply(model.a[reference_body_id]) + + SpatialVector (0, 0, 0, a_dash[0], a_dash[1], a_dash[2])); +} + +RBDL_DLLAPI bool InverseKinematics ( + Model &model, + const VectorNd &Qinit, + const std::vector& body_id, + const std::vector& body_point, + const std::vector& target_pos, + VectorNd &Qres, + double step_tol, + double lambda, + unsigned int max_iter) { + assert (Qinit.size() == model.q_size); + assert (body_id.size() == body_point.size()); + assert (body_id.size() == target_pos.size()); + + MatrixNd J = MatrixNd::Zero(3 * body_id.size(), model.qdot_size); + VectorNd e = VectorNd::Zero(3 * body_id.size()); + + Qres = Qinit; + + for (unsigned int ik_iter = 0; ik_iter < max_iter; ik_iter++) { + UpdateKinematicsCustom (model, &Qres, NULL, NULL); + + for (unsigned int k = 0; k < body_id.size(); k++) { + MatrixNd G (MatrixNd::Zero(3, model.qdot_size)); + CalcPointJacobian (model, Qres, body_id[k], body_point[k], G, false); + Vector3d point_base = + CalcBodyToBaseCoordinates (model, Qres, body_id[k], body_point[k], false); + LOG << "current_pos = " << point_base.transpose() << std::endl; + + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < model.qdot_size; j++) { + unsigned int row = k * 3 + i; + LOG << "i = " << i << " j = " << j << " k = " << k << " row = " + << row << " col = " << j << std::endl; + J(row, j) = G (i,j); + } + + e[k * 3 + i] = target_pos[k][i] - point_base[i]; + } + } + + LOG << "J = " << J << std::endl; + LOG << "e = " << e.transpose() << std::endl; + + // abort if we are getting "close" + if (e.norm() < step_tol) { + LOG << "Reached target close enough after " << ik_iter << " steps" << std::endl; + return true; + } + + MatrixNd JJTe_lambda2_I = + J * J.transpose() + + lambda*lambda * MatrixNd::Identity(e.size(), e.size()); + + VectorNd z (body_id.size() * 3); +#ifndef RBDL_USE_SIMPLE_MATH + z = JJTe_lambda2_I.colPivHouseholderQr().solve (e); +#else + bool solve_successful = LinSolveGaussElimPivot (JJTe_lambda2_I, e, z); + assert (solve_successful); +#endif + + LOG << "z = " << z << std::endl; + + VectorNd delta_theta = J.transpose() * z; + LOG << "change = " << delta_theta << std::endl; + + Qres = Qres + delta_theta; + LOG << "Qres = " << Qres.transpose() << std::endl; + + if (delta_theta.norm() < step_tol) { + LOG << "reached convergence after " << ik_iter << " steps" << std::endl; + return true; + } + + VectorNd test_1 (z.size()); + VectorNd test_res (z.size()); + + test_1.setZero(); + + for (unsigned int i = 0; i < z.size(); i++) { + test_1[i] = 1.; + + VectorNd test_delta = J.transpose() * test_1; + + test_res[i] = test_delta.squaredNorm(); + + test_1[i] = 0.; + } + + LOG << "test_res = " << test_res.transpose() << std::endl; + } + + return false; +} + +RBDL_DLLAPI +Vector3d CalcAngularVelocityfromMatrix ( + const Matrix3d &RotMat + ) { + double tol = 1e-12; + + Vector3d l = Vector3d (RotMat(2,1) - RotMat(1,2), RotMat(0,2) - RotMat(2,0), RotMat(1,0) - RotMat(0,1)); + if(l.norm() > tol){ + double preFactor = atan2(l.norm(),(RotMat.trace() - 1.0))/l.norm(); + return preFactor*l; + } + else if((RotMat(0,0)>0 && RotMat(1,1)>0 && RotMat(2,2) > 0) || l.norm() < tol){ + return Vector3dZero; + } + else{ + double PI = atan(1)*4.0; + return Vector3d (PI/2*(RotMat(0,0) + 1.0),PI/2*(RotMat(1,1) + 1.0),PI/2*(RotMat(2,2) + 1.0)); + } +} + +RBDL_DLLAPI +InverseKinematicsConstraintSet::InverseKinematicsConstraintSet() { + lambda = 1e-9; + num_steps = 0; + max_steps = 300; + step_tol = 1e-12; + constraint_tol = 1e-12; + num_constraints = 0; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddPointConstraint( + unsigned int body_id, + const Vector3d& body_point, + const Vector3d& target_pos + ) { + constraint_type.push_back (ConstraintTypePosition); + body_ids.push_back(body_id); + body_points.push_back(body_point); + target_positions.push_back(target_pos); + target_orientations.push_back(Matrix3d::Zero(3,3)); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 3; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddOrientationConstraint( + unsigned int body_id, + const Matrix3d& target_orientation + ) { + constraint_type.push_back (ConstraintTypeOrientation); + body_ids.push_back(body_id); + body_points.push_back(Vector3d::Zero()); + target_positions.push_back(Vector3d::Zero()); + target_orientations.push_back(target_orientation); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 3; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddFullConstraint( + unsigned int body_id, + const Vector3d& body_point, + const Vector3d& target_pos, + const Matrix3d& target_orientation + ) { + constraint_type.push_back (ConstraintTypeFull); + body_ids.push_back(body_id); + body_points.push_back(body_point); + target_positions.push_back(target_pos); + target_orientations.push_back(target_orientation); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 6; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::ClearConstraints() +{ + for (unsigned int i =0; i < constraint_type.size(); i++){ + constraint_type.pop_back(); + body_ids.pop_back(); + body_points.pop_back(); + target_positions.pop_back(); + target_orientations.pop_back(); + num_constraints = 0; + } + return constraint_type.size(); +} + + +RBDL_DLLAPI +bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + InverseKinematicsConstraintSet &CS, + Math::VectorNd &Qres + ) { + assert (Qinit.size() == model.q_size); + assert (Qres.size() == Qinit.size()); + + CS.J = MatrixNd::Zero(CS.num_constraints, model.qdot_size); + CS.e = VectorNd::Zero(CS.num_constraints); + + Qres = Qinit; + + for (CS.num_steps = 0; CS.num_steps < CS.max_steps; CS.num_steps++) { + UpdateKinematicsCustom (model, &Qres, NULL, NULL); + + for (unsigned int k = 0; k < CS.body_ids.size(); k++) { + CS.G = MatrixNd::Zero(6, model.qdot_size); + CalcPointJacobian6D (model, Qres, CS.body_ids[k], CS.body_points[k], CS.G, false); + Vector3d point_base = CalcBodyToBaseCoordinates (model, Qres, CS.body_ids[k], CS.body_points[k], false); + Matrix3d R = CalcBodyWorldOrientation(model, Qres, CS.body_ids[k], false); + Vector3d angular_velocity = R.transpose()*CalcAngularVelocityfromMatrix(R*CS.target_orientations[k].transpose()); + + //assign offsets and Jacobians + if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypeFull){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row + 3] = CS.target_positions[k][i] - point_base[i]; + CS.e[row] = angular_velocity[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row + 3, j) = CS.G (i + 3,j); + CS.J(row, j) = CS.G (i,j); + } + } + } + else if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypeOrientation){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row] = angular_velocity[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row, j) = CS.G (i,j); + } + } + } + else if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypePosition){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row] = CS.target_positions[k][i] - point_base[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row, j) = CS.G (i + 3,j); + } + } + } + else { + assert (false && !"Invalid inverse kinematics constraint"); + } + } + + LOG << "J = " << CS.J << std::endl; + LOG << "e = " << CS.e.transpose() << std::endl; + CS.error_norm = CS.e.norm(); + + // abort if we are getting "close" + if (CS.error_norm < CS.step_tol) { + LOG << "Reached target close enough after " << CS.num_steps << " steps" << std::endl; + return true; + } + + // // "task space" from puppeteer + // MatrixNd Ek = MatrixNd::Zero (CS.e.size(), CS.e.size()); + // + // for (size_t ei = 0; ei < CS.e.size(); ei ++) { + // // Ek(ei,ei) = CS.error_norm * CS.error_norm * 0.5 + CS.lambda; + // Ek(ei,ei) = CS.e[ei]*CS.e[ei] * 0.5 + CS.lambda; + // } + // + // MatrixNd JJT_Ek_wnI = CS.J * CS.J.transpose() + Ek; + // + // VectorNd delta_theta = CS.J.transpose() * JJT_Ek_wnI.colPivHouseholderQr().solve (CS.e); + // + // LOG << "change = " << delta_theta << std::endl; + + + // "joint space" from puppeteer + + double Ek = 0.; + + for (size_t ei = 0; ei < CS.e.size(); ei ++) { + Ek += CS.e[ei] * CS.e[ei] * 0.5; + } + + VectorNd ek = CS.J.transpose() * CS.e; + MatrixNd Wn = MatrixNd::Zero (Qres.size(), Qres.size()); + + assert (ek.size() == Qres.size()); + + for (size_t wi = 0; wi < Qres.size(); wi++) { + Wn(wi, wi) = ek[wi] * ek[wi] * 0.5 + CS.lambda; + // Wn(wi, wi) = Ek + 1.0e-3; + } + + MatrixNd A = CS.J.transpose() * CS.J + Wn; + VectorNd delta_theta = A.colPivHouseholderQr().solve(CS.J.transpose() * CS.e); + + Qres = Qres + delta_theta; + if (delta_theta.norm() < CS.step_tol) { + LOG << "reached convergence after " << CS.num_steps << " steps" << std::endl; + return true; + } + } + + return false; +} + +} diff --git a/3rdparty/rbdl/src/Logging.cc b/3rdparty/rbdl/src/Logging.cc new file mode 100644 index 0000000..9825a64 --- /dev/null +++ b/3rdparty/rbdl/src/Logging.cc @@ -0,0 +1,14 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "rbdl/Logging.h" + +RBDL_DLLAPI std::ostringstream LogOutput; + +RBDL_DLLAPI void ClearLogOutput() { + LogOutput.str(""); +} diff --git a/3rdparty/rbdl/src/Model.cc b/3rdparty/rbdl/src/Model.cc new file mode 100644 index 0000000..7344ede --- /dev/null +++ b/3rdparty/rbdl/src/Model.cc @@ -0,0 +1,494 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Body.h" +#include "rbdl/Joint.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +Model::Model() { + Body root_body; + Joint root_joint; + + Vector3d zero_position (0., 0., 0.); + SpatialVector zero_spatial (0., 0., 0., 0., 0., 0.); + + // structural information + lambda.push_back(0); + lambda_q.push_back(0); + mu.push_back(std::vector()); + dof_count = 0; + q_size = 0; + qdot_size = 0; + previously_added_body_id = 0; + + gravity = Vector3d (0., -9.81, 0.); + + // state information + v.push_back(zero_spatial); + a.push_back(zero_spatial); + + // Joints + mJoints.push_back(root_joint); + S.push_back (zero_spatial); + X_T.push_back(SpatialTransform()); + + X_J.push_back (SpatialTransform()); + v_J.push_back (zero_spatial); + c_J.push_back (zero_spatial); + + // Spherical joints + multdof3_S.push_back (Matrix63::Zero()); + multdof3_U.push_back (Matrix63::Zero()); + multdof3_Dinv.push_back (Matrix3d::Zero()); + multdof3_u.push_back (Vector3d::Zero()); + multdof3_w_index.push_back (0); + + // Dynamic variables + c.push_back(zero_spatial); + IA.push_back(SpatialMatrix::Identity()); + pA.push_back(zero_spatial); + U.push_back(zero_spatial); + + u = VectorNd::Zero(1); + d = VectorNd::Zero(1); + + f.push_back (zero_spatial); + SpatialRigidBodyInertia rbi(0., + Vector3d (0., 0., 0.), + Matrix3d::Zero(3,3)); + Ic.push_back (rbi); + I.push_back(rbi); + hc.push_back (zero_spatial); + + // Bodies + X_lambda.push_back(SpatialTransform()); + X_base.push_back(SpatialTransform()); + + mBodies.push_back(root_body); + mBodyNameMap["ROOT"] = 0; + + fixed_body_discriminator = std::numeric_limits::max() / 2; +} + +unsigned int AddBodyFixedJoint ( + Model &model, + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + FixedBody fbody = FixedBody::CreateFromBody (body); + fbody.mMovableParent = parent_id; + fbody.mParentTransform = joint_frame; + + if (model.IsFixedBodyId(parent_id)) { + FixedBody fixed_parent = + model.mFixedBodies[parent_id - model.fixed_body_discriminator]; + + fbody.mMovableParent = fixed_parent.mMovableParent; + fbody.mParentTransform = joint_frame * fixed_parent.mParentTransform; + } + + // merge the two bodies + Body parent_body = model.mBodies[fbody.mMovableParent]; + parent_body.Join (fbody.mParentTransform, body); + model.mBodies[fbody.mMovableParent] = parent_body; + model.I[fbody.mMovableParent] = + SpatialRigidBodyInertia::createFromMassComInertiaC ( + parent_body.mMass, + parent_body.mCenterOfMass, + parent_body.mInertia); + + model.mFixedBodies.push_back (fbody); + + if (model.mFixedBodies.size() > + std::numeric_limits::max() - model.fixed_body_discriminator) { + std::cerr << "Error: cannot add more than " + << std::numeric_limits::max() + - model.mFixedBodies.size() + << " fixed bodies. You need to modify " + << "Model::fixed_body_discriminator for this." + << std::endl; + assert (0); + abort(); + } + + if (body_name.size() != 0) { + if (model.mBodyNameMap.find(body_name) != model.mBodyNameMap.end()) { + std::cerr << "Error: Body with name '" + << body_name + << "' already exists!" + << std::endl; + assert (0); + abort(); + } + model.mBodyNameMap[body_name] = model.mFixedBodies.size() + + model.fixed_body_discriminator - 1; + } + + return model.mFixedBodies.size() + model.fixed_body_discriminator - 1; +} + +unsigned int AddBodyMultiDofJoint ( + Model &model, + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + // Here we emulate multi DoF joints by simply adding nullbodies. This + // allows us to use fixed size elements for S,v,a, etc. which is very + // fast in Eigen. + unsigned int joint_count = 0; + if (joint.mJointType == JointType1DoF) + joint_count = 1; + else if (joint.mJointType == JointType2DoF) + joint_count = 2; + else if (joint.mJointType == JointType3DoF) + joint_count = 3; + else if (joint.mJointType == JointType4DoF) + joint_count = 4; + else if (joint.mJointType == JointType5DoF) + joint_count = 5; + else if (joint.mJointType == JointType6DoF) + joint_count = 6; + else if (joint.mJointType == JointTypeFloatingBase) + // no action required + {} + else { + std::cerr << "Error: Invalid joint type: " + << joint.mJointType + << std::endl; + + assert (0 && !"Invalid joint type!"); + } + + Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + null_body.mIsVirtual = true; + + unsigned int null_parent = parent_id; + SpatialTransform joint_frame_transform; + + if (joint.mJointType == JointTypeFloatingBase) { + null_parent = model.AddBody (parent_id, + joint_frame, + JointTypeTranslationXYZ, + null_body); + + return model.AddBody (null_parent, + SpatialTransform(), + JointTypeSpherical, + body, + body_name); + } + + Joint single_dof_joint; + unsigned int j; + + // Here we add multiple virtual bodies that have no mass or inertia for + // which each is attached to the model with a single degree of freedom + // joint. + for (j = 0; j < joint_count; j++) { + single_dof_joint = Joint (joint.mJointAxes[j]); + + if (single_dof_joint.mJointType == JointType1DoF) { + Vector3d rotation ( + joint.mJointAxes[j][0], + joint.mJointAxes[j][1], + joint.mJointAxes[j][2]); + Vector3d translation ( + joint.mJointAxes[j][3], + joint.mJointAxes[j][4], + joint.mJointAxes[j][5]); + + if (rotation == Vector3d (0., 0., 0.)) { + single_dof_joint = Joint (JointTypePrismatic, translation); + } else if (translation == Vector3d (0., 0., 0.)) { + single_dof_joint = Joint (JointTypeRevolute, rotation); + } + } + + // the first joint has to be transformed by joint_frame, all the + // others must have a null transformation + if (j == 0) + joint_frame_transform = joint_frame; + else + joint_frame_transform = SpatialTransform(); + + if (j == joint_count - 1) + // if we are at the last we must add the real body + break; + else { + // otherwise we just add an intermediate body + null_parent = model.AddBody (null_parent, + joint_frame_transform, + single_dof_joint, + null_body); + } + } + + return model.AddBody (null_parent, + joint_frame_transform, + single_dof_joint, + body, + body_name); +} + +unsigned int Model::AddBody( + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + assert (lambda.size() > 0); + assert (joint.mJointType != JointTypeUndefined); + + if (joint.mJointType == JointTypeFixed) { + previously_added_body_id = AddBodyFixedJoint (*this, + parent_id, + joint_frame, + joint, + body, + body_name); + + return previously_added_body_id; + } else if ( (joint.mJointType == JointTypeSpherical) + || (joint.mJointType == JointTypeEulerZYX) + || (joint.mJointType == JointTypeEulerXYZ) + || (joint.mJointType == JointTypeEulerYXZ) + || (joint.mJointType == JointTypeTranslationXYZ) + || (joint.mJointType == JointTypeCustom) + ) { + // no action required + } else if (joint.mJointType != JointTypePrismatic + && joint.mJointType != JointTypeRevolute + && joint.mJointType != JointTypeRevoluteX + && joint.mJointType != JointTypeRevoluteY + && joint.mJointType != JointTypeRevoluteZ + && joint.mJointType != JointTypeHelical + ) { + previously_added_body_id = AddBodyMultiDofJoint (*this, + parent_id, + joint_frame, + joint, + body, + body_name); + return previously_added_body_id; + } + + // If we add the body to a fixed body we have to make sure that we + // actually add it to its movable parent. + unsigned int movable_parent_id = parent_id; + SpatialTransform movable_parent_transform; + + if (IsFixedBodyId(parent_id)) { + unsigned int fbody_id = parent_id - fixed_body_discriminator; + movable_parent_id = mFixedBodies[fbody_id].mMovableParent; + movable_parent_transform = mFixedBodies[fbody_id].mParentTransform; + } + + // structural information + lambda.push_back(movable_parent_id); + unsigned int lambda_q_last = mJoints[mJoints.size() - 1].q_index; + + if (mJoints[mJoints.size() - 1].mDoFCount > 0 + && mJoints[mJoints.size() - 1].mJointType != JointTypeCustom) { + lambda_q_last = lambda_q_last + mJoints[mJoints.size() - 1].mDoFCount; + } else if (mJoints[mJoints.size() - 1].mJointType == JointTypeCustom) { + unsigned int custom_index = mJoints[mJoints.size() - 1].custom_joint_index; + lambda_q_last = lambda_q_last + + mCustomJoints[mCustomJoints.size() - 1]->mDoFCount; + } + + for (unsigned int i = 0; i < joint.mDoFCount; i++) { + lambda_q.push_back(lambda_q_last + i); + } + mu.push_back(std::vector()); + mu.at(movable_parent_id).push_back(mBodies.size()); + + // Bodies + X_lambda.push_back(SpatialTransform()); + X_base.push_back(SpatialTransform()); + mBodies.push_back(body); + + if (body_name.size() != 0) { + if (mBodyNameMap.find(body_name) != mBodyNameMap.end()) { + std::cerr << "Error: Body with name '" + << body_name + << "' already exists!" + << std::endl; + assert (0); + abort(); + } + mBodyNameMap[body_name] = mBodies.size() - 1; + } + + // state information + v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + + // Joints + unsigned int prev_joint_index = mJoints.size() - 1; + mJoints.push_back(joint); + + if (mJoints[prev_joint_index].mJointType != JointTypeCustom) { + mJoints[mJoints.size() - 1].q_index = + mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; + } else { + mJoints[mJoints.size() - 1].q_index = + mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; + } + + S.push_back (joint.mJointAxes[0]); + + // Joint state variables + X_J.push_back (SpatialTransform()); + v_J.push_back (joint.mJointAxes[0]); + c_J.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); + + // workspace for joints with 3 dof + multdof3_S.push_back (Matrix63::Zero(6,3)); + multdof3_U.push_back (Matrix63::Zero()); + multdof3_Dinv.push_back (Matrix3d::Zero()); + multdof3_u.push_back (Vector3d::Zero()); + multdof3_w_index.push_back (0); + + dof_count = dof_count + joint.mDoFCount; + + // update the w components of the Quaternions. They are stored at the end + // of the q vector + int multdof3_joint_counter = 0; + int mCustomJoint_joint_counter = 0; + for (unsigned int i = 1; i < mJoints.size(); i++) { + if (mJoints[i].mJointType == JointTypeSpherical) { + multdof3_w_index[i] = dof_count + multdof3_joint_counter; + multdof3_joint_counter++; + } + } + + q_size = dof_count + + multdof3_joint_counter; + + qdot_size = qdot_size + joint.mDoFCount; + + // we have to invert the transformation as it is later always used from the + // child bodies perspective. + X_T.push_back(joint_frame * movable_parent_transform); + + // Dynamic variables + c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + IA.push_back(SpatialMatrix::Zero(6,6)); + pA.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + + d = VectorNd::Zero (mBodies.size()); + u = VectorNd::Zero (mBodies.size()); + + f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.)); + + SpatialRigidBodyInertia rbi = + SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, + body.mCenterOfMass, + body.mInertia); + + Ic.push_back (rbi); + I.push_back (rbi); + hc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); + + if (mBodies.size() == fixed_body_discriminator) { + std::cerr << "Error: cannot add more than " + << fixed_body_discriminator + << " movable bodies. You need to modify " + "Model::fixed_body_discriminator for this." + << std::endl; + assert (0); + abort(); + } + + previously_added_body_id = mBodies.size() - 1; + + mJointUpdateOrder.clear(); + + // update the joint order computation + std::vector > joint_types; + for (unsigned int i = 0; i < mJoints.size(); i++) { + joint_types.push_back( + std::pair (mJoints[i].mJointType,i)); + mJointUpdateOrder.push_back (mJoints[i].mJointType); + } + + mJointUpdateOrder.clear(); + JointType current_joint_type = JointTypeUndefined; + while (joint_types.size() != 0) { + current_joint_type = joint_types[0].first; + + std::vector >::iterator type_iter = + joint_types.begin(); + + while (type_iter != joint_types.end()) { + if (type_iter->first == current_joint_type) { + mJointUpdateOrder.push_back (type_iter->second); + type_iter = joint_types.erase (type_iter); + } else { + ++type_iter; + } + } + } + + // for (unsigned int i = 0; i < mJointUpdateOrder.size(); i++) { + // std::cout << "i = " << i << ": joint_id = " << mJointUpdateOrder[i] + // << " joint_type = " << mJoints[mJointUpdateOrder[i]].mJointType << std::endl; + // } + + return previously_added_body_id; +} + +unsigned int Model::AppendBody ( + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + return Model::AddBody (previously_added_body_id, + joint_frame, + joint, + body, + body_name); +} + +unsigned int Model::AddBodyCustomJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + CustomJoint *custom_joint, + const Body &body, + std::string body_name) { + Joint proxy_joint (JointTypeCustom, custom_joint->mDoFCount); + proxy_joint.custom_joint_index = mCustomJoints.size(); + //proxy_joint.mDoFCount = custom_joint->mDoFCount; //MM added. Otherwise + //model.q_size = 0, which is not good. + + mCustomJoints.push_back (custom_joint); + + unsigned int body_id = AddBody (parent_id, + joint_frame, + proxy_joint, + body, + body_name); + + return body_id; +} + diff --git a/3rdparty/rbdl/src/rbdl_mathutils.cc b/3rdparty/rbdl/src/rbdl_mathutils.cc new file mode 100644 index 0000000..a865b60 --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_mathutils.cc @@ -0,0 +1,319 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include + +#include +#include + +#include +#include + +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { +namespace Math { + +RBDL_DLLAPI Vector3d Vector3dZero (0., 0., 0.); +RBDL_DLLAPI Matrix3d Matrix3dIdentity ( + 1., 0., 0., + 0., 1., 0., + 0., 0., 1 + ); +RBDL_DLLAPI Matrix3d Matrix3dZero ( + 0., 0., 0., + 0., 0., 0., + 0., 0., 0. + ); + +RBDL_DLLAPI SpatialVector SpatialVectorZero ( 0., 0., 0., 0., 0., 0.); + +RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { + x = VectorNd::Zero(x.size()); + + // We can only solve quadratic systems + assert (A.rows() == A.cols()); + + unsigned int n = A.rows(); + unsigned int pi; + + // the pivots + size_t *pivot = new size_t[n]; + + // temporary result vector which contains the pivoted result + VectorNd px(x); + + unsigned int i,j,k; + + for (i = 0; i < n; i++) + pivot[i] = i; + + for (j = 0; j < n; j++) { + pi = j; + double pv = fabs (A(j,pivot[j])); + + // LOG << "j = " << j << " pv = " << pv << std::endl; + // find the pivot + for (k = j; k < n; k++) { + double pt = fabs (A(j,pivot[k])); + if (pt > pv) { + pv = pt; + pi = k; + unsigned int p_swap = pivot[j]; + pivot[j] = pivot[pi]; + pivot[pi] = p_swap; + // LOG << "swap " << j << " with " << pi << std::endl; + // LOG << "j = " << j << " pv = " << pv << std::endl; + } + } + + for (i = j + 1; i < n; i++) { + if (fabs(A(j,pivot[j])) <= std::numeric_limits::epsilon()) { + std::cerr << "Error: pivoting failed for matrix A = " << std::endl; + std::cerr << "A = " << std::endl << A << std::endl; + std::cerr << "b = " << b << std::endl; + } + // assert (fabs(A(j,pivot[j])) > std::numeric_limits::epsilon()); + double d = A(i,pivot[j])/A(j,pivot[j]); + + b[i] -= b[j] * d; + + for (k = j; k < n; k++) { + A(i,pivot[k]) -= A(j,pivot[k]) * d; + } + } + } + + // warning: i is an unsigned int, therefore a for loop of the + // form "for (i = n - 1; i >= 0; i--)" might end up in getting an invalid + // value for i! + i = n; + do { + i--; + + for (j = i + 1; j < n; j++) { + px[i] += A(i,pivot[j]) * px[j]; + } + px[i] = (b[i] - px[i]) / A(i,pivot[i]); + + } while (i > 0); + + // Unswapping + for (i = 0; i < n; i++) { + x[pivot[i]] = px[i]; + } + + /* + LOG << "A = " << std::endl << A << std::endl; + LOG << "b = " << b << std::endl; + LOG << "x = " << x << std::endl; + LOG << "pivot = " << pivot[0] << " " << pivot[1] << " " << pivot[2] << std::endl; + std::cout << LogOutput.str() << std::endl; + */ + + delete[] pivot; + + return true; +} + +RBDL_DLLAPI void SpatialMatrixSetSubmatrix( + SpatialMatrix &dest, + unsigned int row, + unsigned int col, + const Matrix3d &matrix) { + assert (row < 2 && col < 2); + + dest(row*3,col*3) = matrix(0,0); + dest(row*3,col*3 + 1) = matrix(0,1); + dest(row*3,col*3 + 2) = matrix(0,2); + + dest(row*3 + 1,col*3) = matrix(1,0); + dest(row*3 + 1,col*3 + 1) = matrix(1,1); + dest(row*3 + 1,col*3 + 2) = matrix(1,2); + + dest(row*3 + 2,col*3) = matrix(2,0); + dest(row*3 + 2,col*3 + 1) = matrix(2,1); + dest(row*3 + 2,col*3 + 2) = matrix(2,2); +} + +RBDL_DLLAPI bool SpatialMatrixCompareEpsilon ( + const SpatialMatrix &matrix_a, + const SpatialMatrix &matrix_b, + double epsilon) { + assert (epsilon >= 0.); + unsigned int i, j; + + for (i = 0; i < 6; i++) { + for (j = 0; j < 6; j++) { + if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) { + std::cerr << "Expected:" + << std::endl << matrix_a << std::endl + << "but was" << std::endl + << matrix_b << std::endl; + return false; + } + } + } + + return true; +} + +RBDL_DLLAPI bool SpatialVectorCompareEpsilon ( + const SpatialVector &vector_a, + const SpatialVector &vector_b, + double epsilon) { + assert (epsilon >= 0.); + unsigned int i; + + for (i = 0; i < 6; i++) { + if (fabs(vector_a[i] - vector_b[i]) >= epsilon) { + std::cerr << "Expected:" + << std::endl << vector_a << std::endl + << "but was" << std::endl + << vector_b << std::endl; + return false; + } + } + + return true; +} + +RBDL_DLLAPI Matrix3d parallel_axis ( + const Matrix3d &inertia, + double mass, + const Vector3d &com) { + Matrix3d com_cross = VectorCrossMatrix (com); + + return inertia + mass * com_cross * com_cross.transpose(); +} + +RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &r) { + return SpatialMatrix ( + 1., 0., 0., 0., 0., 0., + 0., 1., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., r[2], -r[1], 1., 0., 0., + -r[2], 0., r[0], 0., 1., 0., + r[1], -r[0], 0., 0., 0., 1. + ); +} + +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + + return SpatialMatrix( + 1., 0., 0., 0., 0., 0., + 0., c, s, 0., 0., 0., + 0., -s, c, 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., c, s, + 0., 0., 0., 0., -s, c + ); +} + +RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + + return SpatialMatrix( + c, 0., -s, 0., 0., 0., + 0., 1., 0., 0., 0., 0., + s, 0., c, 0., 0., 0., + 0., 0., 0., c, 0., -s, + 0., 0., 0., 0., 1., 0., + 0., 0., 0., s, 0., c + ); +} + +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + + return SpatialMatrix( + c, s, 0., 0., 0., 0., + -s, c, 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., c, s, 0., + 0., 0., 0., -s, c, 0., + 0., 0., 0., 0., 0., 1. + ); +} + +RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler ( + const Vector3d &displacement, + const Vector3d &zyx_euler) { + return Xrotz_mat(zyx_euler[0]) * Xroty_mat(zyx_euler[1]) * Xrotx_mat(zyx_euler[2]) * Xtrans_mat(displacement); +} + +RBDL_DLLAPI void SparseFactorizeLTL (Model &model, Math::MatrixNd &H) { + for (unsigned int i = 0; i < model.qdot_size; i++) { + for (unsigned int j = i + 1; j < model.qdot_size; j++) { + H(i,j) = 0.; + } + } + + for (unsigned int k = model.qdot_size; k > 0; k--) { + H(k - 1,k - 1) = sqrt (H(k - 1,k - 1)); + unsigned int i = model.lambda_q[k]; + while (i != 0) { + H(k - 1,i - 1) = H(k - 1,i - 1) / H(k - 1,k - 1); + i = model.lambda_q[i]; + } + + i = model.lambda_q[k]; + while (i != 0) { + unsigned int j = i; + while (j != 0) { + H(i - 1,j - 1) = H(i - 1,j - 1) - H(k - 1,i - 1) * H(k - 1, j - 1); + j = model.lambda_q[j]; + } + i = model.lambda_q[i]; + } + } +} + +RBDL_DLLAPI void SparseMultiplyHx (Model &model, Math::MatrixNd &L) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseMultiplyLx (Model &model, Math::MatrixNd &L) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseMultiplyLTx (Model &model, Math::MatrixNd &L) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) { + for (unsigned int i = 1; i <= model.qdot_size; i++) { + unsigned int j = model.lambda_q[i]; + while (j != 0) { + x[i - 1] = x[i - 1] - L(i - 1,j - 1) * x[j - 1]; + j = model.lambda_q[j]; + } + x[i - 1] = x[i - 1] / L(i - 1,i - 1); + } +} + +RBDL_DLLAPI void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) { + for (int i = model.qdot_size; i > 0; i--) { + x[i - 1] = x[i - 1] / L(i - 1,i - 1); + unsigned int j = model.lambda_q[i]; + while (j != 0) { + x[j - 1] = x[j - 1] - L(i - 1,j - 1) * x[i - 1]; + j = model.lambda_q[j]; + } + } +} + +} /* Math */ +} /* RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/rbdl_utils.cc b/3rdparty/rbdl/src/rbdl_utils.cc new file mode 100644 index 0000000..db219cc --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_utils.cc @@ -0,0 +1,229 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "rbdl/rbdl_utils.h" + +#include "rbdl/rbdl_math.h" +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include +#include + +namespace RigidBodyDynamics { + +namespace Utils { + +using namespace std; +using namespace Math; + +string get_dof_name (const SpatialVector &joint_dof) { + if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) + return "RX"; + else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.)) + return "RY"; + else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.)) + return "RZ"; + else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.)) + return "TX"; + else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.)) + return "TY"; + else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.)) + return "TZ"; + + ostringstream dof_stream(ostringstream::out); + dof_stream << "custom (" << joint_dof.transpose() << ")"; + return dof_stream.str(); +} + +string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) { + if (model.mBodies[body_id].mIsVirtual) { + // if there is not a unique child we do not know what to do... + if (model.mu[body_id].size() != 1) + return ""; + + return get_body_name (model, model.mu[body_id][0]); + } + + return model.GetBodyName(body_id); +} + +RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) { + stringstream result (""); + + unsigned int q_index = 0; + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.mJoints[i].mDoFCount == 1) { + result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.S[i]) << endl; + q_index++; + } else { + for (unsigned int j = 0; j < model.mJoints[i].mDoFCount; j++) { + result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.mJoints[i].mJointAxes[j]) << endl; + q_index++; + } + } + } + + return result.str(); +} + +std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) { + stringstream result (""); + + for (int j = 0; j < indent; j++) + result << " "; + + result << get_body_name (model, body_index); + + if (body_index > 0) + result << " [ "; + + while (model.mBodies[body_index].mIsVirtual) { + if (model.mu[body_index].size() == 0) { + result << " end"; + break; + } else if (model.mu[body_index].size() > 1) { + cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " (name: " << model.GetBodyName(body_index) << ") has more than one child:" << endl; + for (unsigned int ci = 0; ci < model.mu[body_index].size(); ci++) { + cerr << " id: " << model.mu[body_index][ci] << " name: " << model.GetBodyName(model.mu[body_index][ci]) << endl; + } + abort(); + } + + result << get_dof_name(model.S[body_index]) << ", "; + + body_index = model.mu[body_index][0]; + } + + if (body_index > 0) + result << get_dof_name(model.S[body_index]) << " ]"; + result << endl; + + unsigned int child_index = 0; + for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) { + result << print_hierarchy (model, model.mu[body_index][child_index], indent + 1); + } + + // print fixed children + for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) { + if (model.mFixedBodies[fbody_index].mMovableParent == body_index) { + for (int j = 0; j < indent + 1; j++) + result << " "; + + result << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl; + } + } + + + return result.str(); +} + +RBDL_DLLAPI std::string GetModelHierarchy (const Model &model) { + stringstream result (""); + + result << print_hierarchy (model); + + return result.str(); +} + +RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model) { + stringstream result (""); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + UpdateKinematicsCustom (model, &Q, NULL, NULL); + + for (unsigned int body_id = 0; body_id < model.mBodies.size(); body_id++) { + std::string body_name = model.GetBodyName (body_id); + + if (body_name.size() == 0) + continue; + + Vector3d position = CalcBodyToBaseCoordinates (model, Q, body_id, Vector3d (0., 0., 0.), false); + + result << body_name << ": " << position.transpose() << endl; + } + + return result.str(); +} + +RBDL_DLLAPI void CalcCenterOfMass ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + double &mass, + Math::Vector3d &com, + Math::Vector3d *com_velocity, + Vector3d *angular_momentum, + bool update_kinematics) { + if (update_kinematics) + UpdateKinematicsCustom (model, &q, &qdot, NULL); + + for (size_t i = 1; i < model.mBodies.size(); i++) { + model.Ic[i] = model.I[i]; + model.hc[i] = model.Ic[i].toMatrix() * model.v[i]; + } + + SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3)); + SpatialVector htot (SpatialVector::Zero(6)); + + for (size_t i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]); + model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]); + } else { + Itot = Itot + model.X_lambda[i].applyTranspose (model.Ic[i]); + htot = htot + model.X_lambda[i].applyTranspose (model.hc[i]); + } + } + + mass = Itot.m; + com = Itot.h / mass; + LOG << "mass = " << mass << " com = " << com.transpose() << " htot = " << htot.transpose() << std::endl; + + if (com_velocity) + *com_velocity = Vector3d (htot[3] / mass, htot[4] / mass, htot[5] / mass); + + if (angular_momentum) { + htot = Xtrans (com).applyAdjoint (htot); + angular_momentum->set (htot[0], htot[1], htot[2]); + } +} + +RBDL_DLLAPI double CalcPotentialEnergy ( + Model &model, + const Math::VectorNd &q, + bool update_kinematics) { + double mass; + Vector3d com; + CalcCenterOfMass (model, q, VectorNd::Zero (model.qdot_size), mass, com, NULL, NULL, update_kinematics); + + Vector3d g = - Vector3d (model.gravity[0], model.gravity[1], model.gravity[2]); + LOG << "pot_energy: " << " mass = " << mass << " com = " << com.transpose() << std::endl; + + return mass * com.dot(g); +} + +RBDL_DLLAPI double CalcKineticEnergy ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + bool update_kinematics) { + if (update_kinematics) + UpdateKinematicsCustom (model, &q, &qdot, NULL); + + double result = 0.; + + for (size_t i = 1; i < model.mBodies.size(); i++) { + result += 0.5 * model.v[i].transpose() * (model.I[i] * model.v[i]); + } + return result; +} + +} +} diff --git a/3rdparty/rbdl/src/rbdl_version.cc b/3rdparty/rbdl/src/rbdl_version.cc new file mode 100644 index 0000000..c7ccd12 --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_version.cc @@ -0,0 +1,91 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include +#include +#include +#include + +RBDL_DLLAPI int rbdl_get_api_version() { + static int compile_version = RBDL_API_VERSION; + return compile_version; +} + +RBDL_DLLAPI void rbdl_check_api_version(int version) { + int compile_version = rbdl_get_api_version(); + + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." << compile_patch; + + int version_major = (version & 0xff0000) >> 16; + int version_minor = (version & 0x00ff00) >> 8; + int version_patch = (version & 0x0000ff); + + std::ostringstream link_version_string (""); + link_version_string << version_major << "." << version_minor << "." << version_patch; + + if (version_major != compile_major) { + std::cerr << "Error: trying to link against an incompatible RBDL library." << std::endl; + std::cerr << "The library version is: " << compile_version_string.str() << " but rbdl_config.h is version " << link_version_string.str() << std::endl; + abort(); + } else if (version_minor != compile_minor) { + std::cout << "Warning: RBDL library is of version " << compile_version_string.str() << " but rbdl_config.h is from version " << link_version_string.str() << std::endl; + } +} + +RBDL_DLLAPI void rbdl_print_version() { + int compile_version = rbdl_get_api_version(); + + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." << compile_patch; + + std::cout << "RBDL version:" << std::endl + << " API version : " << compile_version_string.str() << std::endl; + + if (std::string("unknown") != RBDL_BUILD_REVISION) { + std::cout << " revision : " << RBDL_BUILD_REVISION + << " (branch: " << RBDL_BUILD_BRANCH << ")" << std::endl + << " build type : " << RBDL_BUILD_TYPE << std::endl; + } + +#ifdef RBDL_ENABLE_LOGGING + std::cout << " logging : on (warning: reduces performance!)" << std::endl; +#else + std::cout << " logging : off" << std::endl; +#endif +#ifdef RBDL_USE_SIMPLE_MATH + std::cout << " simplemath : on (warning: reduces performance!)" << std::endl; +#else + std::cout << " simplemath : off" << std::endl; +#endif + +#ifdef RBDL_BUILD_ADDON_LUAMODEL + std::cout << " LuaModel : on" << std::endl; +#else + std::cout << " LuaModel : off" << std::endl; +#endif +#ifdef RBDL_BUILD_ADDON_URDFREADER + std::cout << " URDFReader : on" << std::endl; +#else + std::cout << " URDFReader : off" << std::endl; +#endif + + std::string build_revision (RBDL_BUILD_REVISION); + if (build_revision == "unknown") { + std::cout << std::endl << "Version information incomplete: to enable version information re-build" << std::endl << "library from valid repository and enable RBDL_STORE_VERSION." << std::endl; + } +} diff --git a/3rdparty/rbdl/tests/BodyTests.cc b/3rdparty/rbdl/tests/BodyTests.cc new file mode 100644 index 0000000..476fe2c --- /dev/null +++ b/3rdparty/rbdl/tests/BodyTests.cc @@ -0,0 +1,244 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Body.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +/* Tests whether the spatial inertia matches the one specified by its + * parameters + */ +TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + Matrix3d inertia_C ( + 1.4, 0., 0., + 0., 2., 0., + 0., 0., 3.); + + SpatialMatrix reference_inertia ( + 4.843, -1.98, -2.145, 0, -1.43, 1.32, + -1.98, 6.334, -1.716, 1.43, 0, -1.65, + -2.145, -1.716, 7.059, -1.32, 1.65, 0, + 0, 1.43, -1.32, 1.1, 0, 0, + -1.43, 0, 1.65, 0, 1.1, 0, + 1.32, -1.65, 0, 0, 0, 1.1 + ); + + // cout << LogOutput.str() << endl; + + SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia); + + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC); + CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); +} + +TEST ( TestBodyConstructorMassComInertia ) { + double mass = 1.1; + Vector3d com (1.5, 1.2, 1.3); + Matrix3d inertia_C ( + 8.286, -3.96, -4.29, + -3.96, 10.668, -3.432, + -4.29, -3.432, 11.118 + ); + + Body body (mass, com, inertia_C); + + SpatialMatrix reference_inertia ( + 11.729, -5.94, -6.435, 0, -1.43, 1.32, + -5.94, 15.002, -5.148, 1.43, 0, -1.65, + -6.435, -5.148, 15.177, -1.32, 1.65, 0, + 0, 1.43, -1.32, 1.1, 0, 0, + -1.43, 0, 1.65, 0, 1.1, 0, + 1.32, -1.65, 0, 0, 0, 1.1 + ); + + SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC); + CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); +} + +TEST ( TestBodyJoinNullbody ) { + ClearLogOutput(); + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + Body nullbody (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + + Body joined_body = body; + joined_body.Join (Xtrans(Vector3d (0., 0., 0.)), nullbody); + + SpatialRigidBodyInertia body_rbi (body.mMass, body.mCenterOfMass, body.mInertia); + SpatialRigidBodyInertia joined_body_rbi (joined_body.mMass, joined_body.mCenterOfMass, joined_body.mInertia); + + CHECK_EQUAL (1.1, body.mMass); + CHECK_ARRAY_CLOSE (body.mCenterOfMass.data(), joined_body.mCenterOfMass.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (body_rbi.toMatrix().data(), joined_body_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodies ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xtrans(Vector3d (0., 0., 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 9.918, 0, 0, 0, -0, 2.86, + 0, 9.062, 0, 0, 0, -0, + 0, 0, 12.98, -2.86, 0, 0, + 0, 0, -2.86, 2.2, 0, 0, + -0, 0, 0, 0, 2.2, 0, + 2.86, -0, 0, 0, 0, 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodiesDisplaced ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xtrans(Vector3d (1.1, 1.3, 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 9.918, 0, 0, 0, -0, 2.86, + 0, 9.062, 0, 0, 0, -0, + 0, 0, 12.98, -2.86, 0, 0, + 0, 0, -2.86, 2.2, 0, 0, + -0, 0, 0, 0, 2.2, 0, + 2.86, -0, 0, 0, 0, 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); + + +} + +TEST ( TestBodyJoinTwoBodiesRotated ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.3, 3.2)); + + Body body_joined (body_a); + body_joined.Join (Xrotx(-M_PI*0.5), body_b); + + SpatialRigidBodyInertia body_joined_rbi (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 6.2, 0., 0., 0., 0., 0., + 0., 6.4, 0., 0., 0., 0., + 0., 0., 6.6, 0., 0., 0., + 0., 0., 0., 2.2, 0., 0., + 0., 0., 0., 0., 2.2, 0., + 0., 0., 0., 0., 0., 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodiesRotatedAndTranslated ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (-1., 1., 0.), Vector3d (3.2, 3.1, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xrotz(M_PI*0.5) * Xtrans(Vector3d (1., 1., 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 6.2, 0., 0., 0., 0., 0., + 0., 6.4, 0., 0., 0., 0., + 0., 0., 6.6, 0., 0., 0., + 0., 0., 0., 2.2, 0., 0., + 0., 0., 0., 0., 2.2, 0., + 0., 0., 0., 0., 0., 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + + SpatialVector mv (1.1, 1.2, 1.3, 1.4, 1.5, 1.6); + SpatialVector fv_matrix = rbi.toMatrix() * mv; + SpatialVector fv_rbi = rbi * mv; + + CHECK_ARRAY_CLOSE ( + fv_matrix.data(), + fv_rbi.data(), + 6, + TEST_PREC + ); +} + +TEST ( TestBodyConstructorSpatialRigidBodyInertia ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + SpatialMatrix spatial_inertia = rbi.toMatrix(); + + CHECK_ARRAY_CLOSE ( + spatial_inertia.data(), + rbi.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST ( TestBodyConstructorCopySpatialRigidBodyInertia ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + + SpatialRigidBodyInertia rbi_from_matrix; + rbi_from_matrix.createFromMatrix (rbi.toMatrix()); + + // cout << "Spatial Inertia = " << endl << spatial_inertia << endl; + // cout << "rbi = " << endl << rbi.toMatrix() << endl; + // cout << "rbi.m = " << rbi.m << endl; + // cout << "rbi.h = " << rbi.h.transpose() << endl; + // cout << "rbi.I = " << endl << rbi.I << endl; + + CHECK_ARRAY_CLOSE ( + rbi.toMatrix().data(), + rbi_from_matrix.toMatrix().data(), + 36, + TEST_PREC + ); +} diff --git a/3rdparty/rbdl/tests/CMakeLists.txt b/3rdparty/rbdl/tests/CMakeLists.txt new file mode 100644 index 0000000..b7d7498 --- /dev/null +++ b/3rdparty/rbdl/tests/CMakeLists.txt @@ -0,0 +1,70 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 2.6) + +CMAKE_POLICY(SET CMP0040 NEW) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) +INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) + +SET ( TESTS_SRCS + main.cc + MathTests.cc + SpatialAlgebraTests.cc + MultiDofTests.cc + KinematicsTests.cc + BodyTests.cc + ModelTests.cc + FloatingBaseTests.cc + CalcVelocitiesTests.cc + CalcAccelerationsTests.cc + DynamicsTests.cc + InverseDynamicsTests.cc + CompositeRigidBodyTests.cc + ImpulsesTests.cc + TwolegModelTests.cc + ContactsTests.cc + UtilsTests.cc + SparseFactorizationTests.cc + CustomJointSingleBodyTests.cc + CustomJointMultiBodyTests.cc + InverseKinematicsTests.cc + LoopConstraintsTests.cc + ScrewJointTests.cc + ) + +INCLUDE_DIRECTORIES ( ../src/ ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runtests + ) + +ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runtests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +TARGET_LINK_LIBRARIES ( rbdl_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) + ADD_CUSTOM_COMMAND (TARGET rbdl_tests + POST_BUILD + COMMAND ./runtests + COMMENT "Running automated tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/3rdparty/rbdl/tests/CalcAccelerationsTests.cc b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc new file mode 100644 index 0000000..644cec0 --- /dev/null +++ b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc @@ -0,0 +1,235 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) { + QDDot[0] = 1.; + ref_body_id = body_a_id; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE(1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE(0., point_acceleration[2], TEST_PREC); + + // LOG << "Point accel = " << point_acceleration << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimpleRotated) { + Q[0] = 0.5 * M_PI; + + ref_body_id = body_a_id; + QDDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + // LOG << "Point accel = " << point_acceleration << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) { + ref_body_id = 1; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + ClearLogOutput(); + + // if we are on the other side we should have the opposite value + point_position = Vector3d (-1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatedBaseSimple) { + // rotated first joint + + ref_body_id = 1; + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE(-1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + point_position = Vector3d (-1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatingBodyB) { + // rotating second joint, point at third body + + ref_body_id = 3; + QDot[1] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + // move it a bit further up (acceleration should stay the same) + point_position = Vector3d (1., 1., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointBodyOrigin) { + // rotating second joint, point at third body + + QDot[0] = 1.; + + ref_body_id = body_b_id; + point_position = Vector3d (0., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestAccelerationLinearFuncOfQddot) { + // rotating second joint, point at third body + + QDot[0] = 1.1; + QDot[1] = 1.3; + QDot[2] = 1.5; + + ref_body_id = body_c_id; + point_position = Vector3d (1., 1., 1.); + + VectorNd qddot_1 = VectorNd::Zero (model->dof_count); + VectorNd qddot_2 = VectorNd::Zero (model->dof_count); + VectorNd qddot_0 = VectorNd::Zero (model->dof_count); + + qddot_1[0] = 0.1; + qddot_1[1] = 0.2; + qddot_1[2] = 0.3; + + qddot_2[0] = 0.32; + qddot_2[1] = -0.1; + qddot_2[2] = 0.53; + + Vector3d acc_1 = CalcPointAcceleration(*model, Q, QDot, qddot_1, ref_body_id, point_position); + Vector3d acc_2 = CalcPointAcceleration(*model, Q, QDot, qddot_2, ref_body_id, point_position); + + MatrixNd G = MatrixNd::Zero (3, model->dof_count); + CalcPointJacobian (*model, Q, ref_body_id, point_position, G, true); + + VectorNd net_acc = G * (qddot_1 - qddot_2); + + Vector3d acc_new = acc_1 - acc_2; + + CHECK_ARRAY_CLOSE (net_acc.data(), acc_new.data(), 3, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithUpdateKinematics ) { + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + ClearLogOutput(); + Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), true); + + CHECK_ARRAY_CLOSE (Vector3d (0., -9.81, 0.), accel.data(), 3, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithoutUpdateKinematics ) { + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + //ClearLogOutput(); + Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), false); + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), accel.data(), 3, TEST_PREC); + // cout << LogOutput.str() << endl; + // cout << accel.transpose() << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJoint) { + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + unsigned int fixed_body_id = model->AddBody (body_c_id, Xtrans (Vector3d (1., -1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + + QDot[0] = 1.; + point_position = Vector3d (0., 0., 0.); + Vector3d point_acceleration_reference = CalcPointAcceleration (*model, Q, QDot, QDDot, body_c_id, Vector3d (1., -1., 0.)); + + ClearLogOutput(); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (point_acceleration_reference.data(), + point_acceleration.data(), + 3, + TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJointRotatedTransform) { + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + SpatialTransform fixed_transform = Xtrans (Vector3d (1., -1., 0.)) * Xrotz(M_PI * 0.5); + unsigned int fixed_body_id = model->AddBody (body_c_id, fixed_transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + QDot[0] = 1.; + point_position = Vector3d (0., 0., 0.); + ClearLogOutput(); + Vector3d point_acceleration_reference = CalcPointAcceleration (*model, Q, QDot, QDDot, body_c_id, Vector3d (1., 1., 0.)); + // cout << LogOutput.str() << endl; + + // cout << "Point position = " << CalcBodyToBaseCoordinates (*model, Q, fixed_body_id, Vector3d (0., 0., 0.)).transpose() << endl; + // cout << "Point position_ref = " << CalcBodyToBaseCoordinates (*model, Q, body_c_id, Vector3d (1., 1., 0.)).transpose() << endl; + + ClearLogOutput(); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (point_acceleration_reference.data(), + point_acceleration.data(), + 3, + TEST_PREC); +} + diff --git a/3rdparty/rbdl/tests/CalcVelocitiesTests.cc b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc new file mode 100644 index 0000000..059a16c --- /dev/null +++ b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc @@ -0,0 +1,215 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ModelVelocitiesFixture { + ModelVelocitiesFixture () { + ClearLogOutput(); + model = new Model; + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_c ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + body_c_id = model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero(3); + point_velocity = Vector3d::Zero(3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~ModelVelocitiesFixture () { + delete model; + } + Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + Body body_a, body_b, body_c; + Joint joint_a, joint_b, joint_c; + + VectorNd Q; + VectorNd QDot; + + Vector3d point_position, point_velocity; +}; + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointSimple) { + ref_body_id = 1; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(0., point_velocity[0], TEST_PREC); + CHECK_CLOSE(1., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseSimple) { + // rotated first joint + + ref_body_id = 1; + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(-1., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); + + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBodyB) { + // rotating second joint, point at third body + + ref_body_id = 3; + QDot[1] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBaseXAxis) { + // also rotate the first joint and take a point that is + // on the X direction + + ref_body_id = 3; + QDot[0] = 1.; + QDot[1] = 1.; + point_position = Vector3d (1., -1., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 2., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseXAxis) { + // perform the previous test with the first joint rotated by pi/2 + // upwards + ClearLogOutput(); + + ref_body_id = 3; + point_position = Vector3d (1., -1., 0.); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + QDot[1] = 1.; + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-2., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointBodyOrigin) { + // Checks whether the computation is also correct for points at the origin + // of a body + + ref_body_id = body_b_id; + point_position = Vector3d (0., 0., 0.); + + Q[0] = 0.; + QDot[0] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 1., point_velocity[1], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); +} + +TEST ( FixedJointCalcPointVelocity ) { + // the standard modeling using a null body + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + QDot[0] = 1.; + + ClearLogOutput(); + Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.)); + // cout << LogOutput.str() << endl; + Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point0_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), point1_velocity.data(), 3, TEST_PREC); +} + +TEST ( FixedJointCalcPointVelocityRotated ) { + // the standard modeling using a null body + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + + ClearLogOutput(); + Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.)); + // cout << LogOutput.str() << endl; + Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (-1., 0., 0.).data(), point0_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (-2., 0., 0.).data(), point1_velocity.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc new file mode 100644 index 0000000..6ce896c --- /dev/null +++ b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc @@ -0,0 +1,261 @@ +#include + +#include + +#include "rbdl/Logging.h" +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct CompositeRigidBodyFixture { + CompositeRigidBodyFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~CompositeRigidBodyFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsFloatingBase) { + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd TauInv = VectorNd::Constant ((size_t) model->dof_count, 0.); + + MatrixNd H = MatrixNd::Constant ((size_t) model->dof_count, (size_t) model->dof_count, 0.); + VectorNd C = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_zero = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_crba = VectorNd::Constant ((size_t) model->dof_count, 0.); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + ClearLogOutput(); + CompositeRigidBodyAlgorithm (*model, Q, H); + // cout << LogOutput.str() << endl; + + InverseDynamics (*model, Q, QDot, QDDot_zero, C); + + CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba)); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoF) { + MatrixNd H = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + VectorNd C = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_zero = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_crba = VectorNd::Constant ((size_t) model->dof_count, 0.); + + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + Q[ 6] = -1.3; + Q[ 7] = -1.4; + Q[ 8] = -1.5; + Q[ 9] = -0.3; + Q[10] = -0.4; + Q[11] = -0.5; + + QDot[ 0] = 1.1; + QDot[ 1] = -1.2; + QDot[ 2] = 1.3; + QDot[ 3] = -0.1; + QDot[ 4] = 0.2; + QDot[ 5] = -0.3; + QDot[ 6] = -1.1; + QDot[ 7] = 1.2; + QDot[ 8] = -1.3; + QDot[ 9] = 0.1; + QDot[10] = -0.2; + QDot[11] = 0.3; + + Tau[ 0] = -1.1; + Tau[ 1] = 1.2; + Tau[ 2] = -1.3; + Tau[ 3] = 1.1; + Tau[ 4] = -1.2; + Tau[ 5] = 1.3; + Tau[ 6] = 0.1; + Tau[ 7] = -0.2; + Tau[ 8] = 0.3; + Tau[ 9] = -0.1; + Tau[10] = 0.2; + Tau[11] = -0.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + ClearLogOutput(); + CompositeRigidBodyAlgorithm (*model, Q, H); + // cout << LogOutput.str() << endl; + InverseDynamics (*model, Q, QDot, QDDot_zero, C); + + CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba)); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoFInverseDynamics) { + MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + Q[ 6] = -1.3; + Q[ 7] = -1.4; + Q[ 8] = -1.5; + Q[ 9] = -0.3; + Q[10] = -0.4; + Q[11] = -0.5; + + QDot.setZero(); + + assert (model->dof_count == 12); + + UpdateKinematicsCustom (*model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (*model, Q, H_crba, false); + + VectorNd H_col = VectorNd::Zero (model->dof_count); + VectorNd QDDot_zero = VectorNd::Zero (model->dof_count); + + unsigned int i; + for (i = 0; i < model->dof_count; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (model->dof_count); + delta_a[i] = 1.; + // cout << delta_a << endl; + + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, QDDot_zero, id_zero); + + H_col = id_delta - id_zero; + // cout << "H_col = " << H_col << endl; + H_id.block<12, 1>(0, i) = H_col; + } + + // cout << "H (crba) = " << endl << H_crba << endl; + // cout << "H (id) = " << endl << H_id << endl; + + CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC); +} + +TEST_FIXTURE(FixedBase6DoF, TestCRBAFloatingBase12DoFInverseDynamics) { + MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + + QDot.setZero(); + + assert (model->dof_count == 6); + + UpdateKinematicsCustom (*model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (*model, Q, H_crba, false); + + VectorNd H_col = VectorNd::Zero (model->dof_count); + VectorNd QDDot_zero = VectorNd::Zero (model->dof_count); + + unsigned int i; + for (i = 0; i < 6; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (model->dof_count); + delta_a[i] = 1.; + + ClearLogOutput(); + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, QDDot_zero, id_zero); + + H_col.setZero(); + H_col = id_delta - id_zero; + + H_id.block<6, 1>(0, i) = H_col; + } + + CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC); +} + +TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsSpherical) { + Body base_body(1., Vector3d (0., 0., 0.), Vector3d (1., 2., 3.)); + + model->AddBody(0, SpatialTransform(), Joint(JointTypeSpherical), base_body); + VectorNd Q = VectorNd::Constant ((size_t) model->q_size, 0.); + model->SetQuaternion (1, Quaternion(), Q); + MatrixNd H = MatrixNd::Constant ((size_t) model->qdot_size, (size_t) model->qdot_size, 0.); + CompositeRigidBodyAlgorithm (*model, Q, H, true); + + Matrix3d H_ref ( + 1., 0., 0., + 0., 2., 0., + 0., 0., 3. + ); + + CHECK_ARRAY_CLOSE (H_ref.data(), H.data(), 9, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/ContactsTests.cc b/3rdparty/rbdl/tests/ContactsTests.cc new file mode 100644 index 0000000..2042573 --- /dev/null +++ b/3rdparty/rbdl/tests/ContactsTests.cc @@ -0,0 +1,724 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +#include "Fixtures.h" +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + +struct FixedBase6DoF9DoF { + FixedBase6DoF9DoF () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body 1 (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, child); + + // child body (3 DoF) + child_2 = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_2_id = model->AddBody (child_id, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, child_2); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + contact_body_id = child_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF9DoF () { + delete model; + } + Model *model; + + unsigned int base_id, child_id, child_2_id; + + Body base, child, child_2; + + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; + + unsigned int contact_body_id; + Vector3d contact_point; + Vector3d contact_normal; + ConstraintSet constraint_set; +}; + +// +// ForwardDynamicsConstraintsDirect +// +TEST ( TestForwardDynamicsConstraintsDirectSimple ) { + Model model; + model.gravity = Vector3d (0., -9.81, 0.); + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[1] = 1.; + QDot[0] = 1.; + QDot[3] = -1.; + + unsigned int contact_body_id = base_body_id; + Vector3d contact_point ( 0., -1., 0.); + + ConstraintSet constraint_set; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z"); + + constraint_set.Bind (model); + + ClearLogOutput(); + +// cout << constraint_set.acceleration.transpose() << endl; + ForwardDynamicsConstraintsDirect (model, Q, QDot, Tau, constraint_set, QDDot); + +// cout << "A = " << endl << constraint_set.A << endl << endl; +// cout << "H = " << endl << constraint_set.H << endl << endl; +// cout << "b = " << endl << constraint_set.b << endl << endl; +// cout << "x = " << endl << constraint_set.x << endl << endl; +// cout << constraint_set.b << endl; +// cout << "QDDot = " << QDDot.transpose() << endl; + + Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 0., 0.).data(), + point_acceleration.data(), + 3, + TEST_PREC + ); + + // cout << "LagrangianSimple Logoutput Start" << endl; + // cout << LogOutput.str() << endl; + // cout << "LagrangianSimple Logoutput End" << endl; +} + +TEST ( TestForwardDynamicsConstraintsDirectMoving ) { + Model model; + model.gravity = Vector3d (0., -9.81, 0.); + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[0] = 0.1; + Q[1] = 0.2; + Q[2] = 0.3; + Q[3] = 0.4; + Q[4] = 0.5; + Q[5] = 0.6; + QDot[0] = 1.1; + QDot[1] = 1.2; + QDot[2] = 1.3; + QDot[3] = -1.4; + QDot[4] = -1.5; + QDot[5] = -1.6; + + unsigned int contact_body_id = base_body_id; + Vector3d contact_point ( 0., -1., 0.); + + ConstraintSet constraint_set; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z"); + + constraint_set.Bind (model); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (model, Q, QDot, Tau, constraint_set, QDDot); + + Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 0., 0.).data(), + point_acceleration.data(), + 3, + TEST_PREC + ); + + // cout << "LagrangianSimple Logoutput Start" << endl; + // cout << LogOutput.str() << endl; + // cout << "LagrangianSimple Logoutput End" << endl; +} + +// +// ForwardDynamicsContacts +// +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) { + contact_normal.set (0., 1., 0.); + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); +// cout << LogOutput.str() << endl; + ClearLogOutput(); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) { + Q[0] = 0.6; + Q[3] = M_PI * 0.6; + Q[4] = 0.1; + + contact_normal.set (0., 1., 0.); + + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_opt; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true); + + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +// +// Similiar to the previous test, this test compares the results of +// - ForwardDynamicsConstraintsDirect +// - ForwardDynamcsContactsOpt +// for the example model in FixedBase6DoF and a moving state (i.e. a +// nonzero QDot) +// +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving) { + Q[0] = 0.6; + Q[3] = M_PI * 0.6; + Q[4] = 0.1; + + QDot[0] = -0.3; + QDot[1] = 0.1; + QDot[2] = -0.5; + QDot[3] = 0.8; + + contact_normal.set (0., 1., 0.); + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << LogOutput.str() << endl; + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); +// cout << LogOutput.str() << endl; + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContactsOld match + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal); + constraint_set.AddContactConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContacts match + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + // check whether the point accelerations match + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + + // check whether the generalized accelerations match + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) { + // makes sure that all variables in the constraint set gets reset + // properly when making repeated calls to ForwardDynamicsContacts. + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal); + constraint_set.AddContactConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + // Call ForwardDynamicsContacts multiple times such that old values might + // be re-used and thus cause erroneus values. + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContacts match + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + // check whether the point accelerations match + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + + // check whether the generalized accelerations match + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + // we rotate the joints so that we have full mobility at the contact + // point: + // + // O X (contact point) + // \ / + // \ / + // \ / + // * + // + + Q[0] = M_PI * 0.25; + Q[1] = 0.2; + Q[3] = M_PI * 0.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl; + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Q[0] = 0.1; + Q[1] = -0.1; + Q[2] = 0.1; + Q[3] = -0.1; + Q[4] = -0.1; + Q[5] = 0.1; + + QDot[0] = 1.; + QDot[1] = -1.; + QDot[2] = 1; + QDot[3] = -1.5; + QDot[4] = 1.5; + QDot[5] = -1.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->dof_count, 0.); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; +// cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) { + VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size())); + VectorNd qddot_sparse (VectorNd::Zero(qddot.size())); + + randomizeStates(); + + ConstraintSet constraint_upper_trunk; + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyUpperTrunk], Vector3d (1.1, 2.2, 3.3), Vector3d (1., 0., 0.)); + constraint_upper_trunk.Bind (*model_3dof); + + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot); + + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.); +} + +TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) { + VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size())); + + for (int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot_3dof[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + Vector3d heel_point (-0.03, 0., -0.03); + + ConstraintSet constraint_upper_trunk; + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (1., 0., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (0., 1., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (0., 0., 1.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (1., 0., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (0., 1., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (0., 0., 1.)); + constraint_upper_trunk.Bind (*model_3dof); + + VectorNd qdotplus (VectorNd::Zero (qdot.size())); + + ComputeConstraintImpulsesDirect (*model_3dof, q, qdot, constraint_upper_trunk, qdotplus); + + Vector3d heel_left_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootLeft], heel_point); + Vector3d heel_right_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootRight], heel_point); + + CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_left_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_right_velocity.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/CustomJointMultiBodyTests.cc b/3rdparty/rbdl/tests/CustomJointMultiBodyTests.cc new file mode 100644 index 0000000..fdfe616 --- /dev/null +++ b/3rdparty/rbdl/tests/CustomJointMultiBodyTests.cc @@ -0,0 +1,1044 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * Copyright (c) 2016 Matthew Millard + */ + + +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" +#include + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; +const int NUMBER_OF_MODELS = 3; +const int NUMBER_OF_BODIES = 3; + +//============================================================================== +/* + + The purpose of this test is to test that all of the code in RBDL + related to a multibody mechanism that includes a custom joint functions. + Specifically this test is for the multi-pass algorithms in rbdl ... namely + the CompositeRigidBodyAlgorithm. However, because these tests have already + been written for CustomJointSingleBodyTests.cc, we'll run them all on the + multibody models that we will be testing. + + We will be testing 3 models to get good coverage of the + CompositeRigidBodyAlgorithm: + + 1. Rx - multidof - custom + 2. Rx - custom - multidof + 3. custom - multidof - Rx + + As before, to test that the model works, we will create a model using + standard RBDL versions (the reference model), and then we will create + a model using a custom joint in the correct location. The following + algorithms will be tested in this manner: + + UpdateKinematicsCustom + Jacobians + InverseDynamics + CompositeRigidBodyAlgorithm + ForwardDynamics + CalcMInvTimestau + ForwardDynamicsContactsKokkevis + +*/ +//============================================================================== + +struct CustomJointTypeRevoluteX : public CustomJoint { + CustomJointTypeRevoluteX(){ + mDoFCount = 1; + S = MatrixNd::Zero(6,1); + S(0,0)=1.0; + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + model.X_J[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]); + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + model.X_lambda[joint_id] = + Xrotx (q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + } + +}; + +struct CustomEulerZYXJoint : public CustomJoint { + CustomEulerZYXJoint () { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ); + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0., 0., 0. + ); + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + + + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + } +}; + +//============================================================================== +//Test Fixture +//============================================================================== + + +struct CustomJointMultiBodyFixture { + CustomJointMultiBodyFixture () { + + reference_model.resize(NUMBER_OF_MODELS); + custom_model.resize(NUMBER_OF_MODELS); + + body.resize(NUMBER_OF_MODELS); + custom_joint.resize(NUMBER_OF_MODELS); + + reference_body_id.resize(NUMBER_OF_MODELS); + custom_body_id.resize(NUMBER_OF_MODELS); + + for(int i=0; i < NUMBER_OF_MODELS; ++i){ + body.at(i).resize(3); + custom_joint.at(i).resize(1); + + reference_body_id.at(i).resize(3); + custom_body_id.at(i).resize(3); + + } + + q.resize(NUMBER_OF_MODELS); + qdot.resize(NUMBER_OF_MODELS); + qddot.resize(NUMBER_OF_MODELS); + tau.resize(NUMBER_OF_MODELS); + + //======================================================== + //Test Model 1: Rx - multidof3 - custom + //======================================================== + + custom_rx_joint1 = CustomJointTypeRevoluteX(); + + Matrix3d inertia1 = Matrix3d::Identity(3,3); + + Body body11 = Body (1., Vector3d (1.1, 1.2, 1.3), inertia1); + Body body12 = Body (2., Vector3d (2.1, 2.2, 2.3), inertia1); + Body body13 = Body (3., Vector3d (3.1, 3.2, 3.3), inertia1); + + Model reference1, custom1; + + Vector3d r1 = Vector3d(0.78,-0.125,0.37); + + double th1 = M_PI/6.0; + double sinTh1 = sin(th1); + double cosTh1 = cos(th1); + + Matrix3d rm1 = Matrix3d( 1.0, 0., 0., + 0., cosTh1, -sinTh1, + 0., sinTh1, cosTh1); + + Vector3d r2 = Vector3d(-0.178,0.2125,-0.937); + + double th2 = M_PI/2.15; + double sinTh2 = sin(th2); + double cosTh2 = cos(th2); + + Matrix3d rm2 = Matrix3d( cosTh2, 0., sinTh2, + 0., 1., 0., + -sinTh2, 0., cosTh2); + + + + unsigned int reference_body_id10 = + reference1.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id11 = + reference1.AddBody (reference_body_id10, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int reference_body_id12 = + reference1.AddBody (reference_body_id11, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + + unsigned int custom_body_id10 = + custom1.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int custom_body_id11 = + custom1.AddBody ( custom_body_id10, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int custom_body_id12 = + custom1.AddBodyCustomJoint ( custom_body_id11, + SpatialTransform(rm2,r2), + &custom_rx_joint1, + body13); + + VectorNd q1 = VectorNd::Zero (reference1.q_size); + VectorNd qdot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd qddot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd tau1 = VectorNd::Zero (reference1.qdot_size); + + int idx = 0; + reference_model.at(idx) = (reference1); + custom_model.at(idx) = (custom1); + + reference_body_id.at(idx).at(0) = (reference_body_id10); + reference_body_id.at(idx).at(1) = (reference_body_id11); + reference_body_id.at(idx).at(2) = (reference_body_id12); + + custom_body_id.at(idx).at(0) = (custom_body_id10); + custom_body_id.at(idx).at(1) = (custom_body_id11); + custom_body_id.at(idx).at(2) = (custom_body_id12); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + q.at(idx) = (q1); + qdot.at(idx) = (qdot1); + qddot.at(idx) = (qddot1); + tau.at(idx) = (tau1); + + + + //======================================================== + //Test Model 2: Rx - custom - multidof3 + //======================================================== + + + Model reference2, custom2; + + unsigned int reference_body_id20 = + reference2.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id21 = + reference2.AddBody (reference_body_id20, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body12); + + unsigned int reference_body_id22 = + reference2.AddBody (reference_body_id21, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body13); + + unsigned int custom_body_id20 = + custom2.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int custom_body_id21 = + custom2.AddBodyCustomJoint ( custom_body_id20, + SpatialTransform(rm2,r2), + &custom_rx_joint1, + body12); + + unsigned int custom_body_id22 = + custom2.AddBody ( custom_body_id21, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body13); + + + + VectorNd q2 = VectorNd::Zero (reference2.q_size); + VectorNd qdot2 = VectorNd::Zero (reference2.qdot_size); + VectorNd qddot2 = VectorNd::Zero (reference2.qdot_size); + VectorNd tau2 = VectorNd::Zero (reference2.qdot_size); + + idx = 1; + reference_model.at(idx) = (reference2); + custom_model.at(idx) = (custom2); + + reference_body_id.at(idx).at(0) = (reference_body_id20); + reference_body_id.at(idx).at(1) = (reference_body_id21); + reference_body_id.at(idx).at(2) = (reference_body_id22); + + custom_body_id.at(idx).at(0) = (custom_body_id20); + custom_body_id.at(idx).at(1) = (custom_body_id21); + custom_body_id.at(idx).at(2) = (custom_body_id22); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + + q.at(idx) = (q2); + qdot.at(idx) = (qdot2); + qddot.at(idx) = (qddot2); + tau.at(idx) = (tau2); + + //======================================================== + //Test Model 3: custom - multidof3 - Rx + //======================================================== + + Model reference3, custom3; + + unsigned int reference_body_id30 = + reference3.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id31 = + reference3.AddBody (reference_body_id30, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int reference_body_id32 = + reference3.AddBody (reference_body_id31, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + unsigned int custom_body_id30 = + custom3.AddBodyCustomJoint ( 0, + SpatialTransform(), + &custom_rx_joint1, + body11); + + unsigned int custom_body_id31 = + custom3.AddBody ( custom_body_id30, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int custom_body_id32 = + custom3.AddBody ( custom_body_id31, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + VectorNd q3 = VectorNd::Zero (reference3.q_size); + VectorNd qdot3 = VectorNd::Zero (reference3.qdot_size); + VectorNd qddot3 = VectorNd::Zero (reference3.qdot_size); + VectorNd tau3 = VectorNd::Zero (reference3.qdot_size); + + idx = 2; + reference_model.at(idx) = (reference3); + custom_model.at(idx) = (custom3); + + reference_body_id.at(idx).at(0) = (reference_body_id30); + reference_body_id.at(idx).at(1) = (reference_body_id31); + reference_body_id.at(idx).at(2) = (reference_body_id32); + + custom_body_id.at(idx).at(0) = (custom_body_id30); + custom_body_id.at(idx).at(1) = (custom_body_id31); + custom_body_id.at(idx).at(2) = (custom_body_id32); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + q.at(idx) = (q3); + qdot.at(idx) = (qdot3); + qddot.at(idx) = (qddot3); + tau.at(idx) = (tau3); + + } + + /* + ~CustomJointMultiBodyFixture () { + }*/ + + vector < Model > reference_model; + vector < Model > custom_model; + + vector < vector < Body > > body; + vector < vector < CustomJoint* > > custom_joint; + + vector < vector< unsigned int > > reference_body_id; + vector < vector< unsigned int > > custom_body_id; + + vector < VectorNd > q; + vector < VectorNd > qdot; + vector < VectorNd > qddot; + vector < VectorNd > tau; + + CustomJointTypeRevoluteX custom_rx_joint1; + CustomJointTypeRevoluteX custom_rx_joint2; + CustomJointTypeRevoluteX custom_rx_joint3; +}; + + +//============================================================================== +// +// Tests +// UpdateKinematicsCustom +// Jacobians +// InverseDynamics +// CompositeRigidBodyAlgorithm +// ForwardDynamics +// CalcMInvTimestau +// ForwardDynamicsContactsKokkevis +// +//============================================================================== + +TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) { + + VectorNd test; + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + + int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof ; i++) { + q.at(idx)[i] = i * 0.1; + qdot.at(idx)[i] = i * 0.15; + qddot.at(idx)[i] = i * 0.17; + } + + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + + Matrix3d Eref = reference_model.at(idx).X_base[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E; + Matrix3d Ecus = custom_model.at(idx).X_base[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E; + + Matrix3d Eerr = Eref-Ecus; + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + custom_model.at(idx).X_base[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + 9, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).v[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).a[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), NULL, NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), NULL, NULL); + + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + custom_model.at(idx).X_base[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + 9, + TEST_PREC); + + + //velocity + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), NULL); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).v[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + + + //All + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).a[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + } + + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) { + + for(int idx = 0; idx < NUMBER_OF_MODELS; ++idx){ + int dof = reference_model.at(idx).dof_count; + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + //Check the Spatial Jacobian + MatrixNd Gref = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + MatrixNd Gcus = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + CalcBodySpatialJacobian ( reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + Gref); + + CalcBodySpatialJacobian ( custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + Gcus); + + for(int i=0; i<6;++i){ + for(int j=0; j 1){ + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + VectorNd qddot_ref = VectorNd::Zero(dof); + VectorNd qddot_cus = VectorNd::Zero(dof); + + VectorNd qdot_plus_ref = VectorNd::Zero(dof); + VectorNd qdot_plus_cus = VectorNd::Zero(dof); + + Vector3d contact_point ( 0., 1., 0.); + + ConstraintSet constraint_set_ref; + ConstraintSet constraint_set_cus; + + + //Reference + constraint_set_ref.AddContactConstraint( + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_ref.AddContactConstraint( + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_ref.Bind (reference_model.at(idx)); + + //Custom + constraint_set_cus.AddContactConstraint( + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_cus.AddContactConstraint( + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_cus.Bind (custom_model.at(idx)); + + ComputeConstraintImpulsesDirect(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_ref, + qdot_plus_ref); + + ForwardDynamicsContactsKokkevis (reference_model.at(idx), + q.at(idx), + qdot_plus_ref, + tau.at(idx), + constraint_set_ref, + qddot_ref); + + ComputeConstraintImpulsesDirect(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_cus, + qdot_plus_cus); + + ForwardDynamicsContactsKokkevis (custom_model.at(idx), + q.at(idx), + qdot_plus_cus, + tau.at(idx), + constraint_set_cus, + qddot_cus); + + VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus; + VectorNd qddot_error = qddot_ref - qddot_cus; + + CHECK_ARRAY_CLOSE (qdot_plus_ref.data(), + qdot_plus_cus.data(), + dof, + TEST_PREC); + + CHECK_ARRAY_CLOSE (qddot_ref.data(), + qddot_cus.data(), + dof, + TEST_PREC); + } + } + +} + +// +//Completed? +// x : implement test for UpdateKinematicsCustom +// x : implement test for Jacobians +// x : implement test for InverseDynamics +// x : implement test for CompositeRigidBodyAlgorithm +// x : implement test for ForwardDynamics +// x : implement test for CalcMInvTimestau +// x : implement test for ForwardDynamicsContactsKokkevis diff --git a/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc new file mode 100644 index 0000000..4f00573 --- /dev/null +++ b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc @@ -0,0 +1,868 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * Copyright (c) 2016 Matthew Millard + */ + + +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" +#include + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 2.0e-12; +const int NUMBER_OF_MODELS = 2; + +//============================================================================== +/* + + The purpose of this test is to test that all of the code in RBDL + related to a single CustomJoint functions. To do this we will implement + joints that already exist in RBDL but using the CustomJoint interface. The + test will then numerically compare the results produced by the CustomJoint + and the equivalent built-in joint in RBDL. The following algorithms will + be tested: + + UpdateKinematicsCustom + Jacobians + InverseDynamics + CompositeRigidBodyAlgorithm + ForwardDynamics + CalcMInvTimestau + ForwardDynamicsContactsKokkevis + +*/ +//============================================================================== +//============================================================================== +/* + As a note, below are the basic fields and functions that every CustomJoint + class member must provide. Refer to Featherstone's dynamics text if the field + names below don't make sense to you. + + 1. Extend from CustomJoint: + + struct CustomJointClass: public CustomJoint + + 2. Make a default constructor, and initialize member variables + mDoFCount + S + d_u + + e.g. + + CustomJointClass() + + 3. Implement the method jcalc. This method must populate X_J, v_J, c_J, and S. + + virtual void jcalc + model.X_J[joint_id] + model.v_J + model.c_J + model.mCustomJoints[joint.custom_joint_index]->S = S + + 4. Implement the method jcalc_X_lambda_S. This method must populate X_lambda + and S. + + virtual void jcalc_X_lambda_S + model.X_lambda + model.mCustomJoints[joint.custom_joint_index]->S = S; + + */ +//============================================================================== +//Custom Joint Code +//============================================================================== +struct CustomJointTypeRevoluteX : public CustomJoint +{ + CustomJointTypeRevoluteX(){ + mDoFCount = 1; + S = MatrixNd::Zero(6,1); + S(0, 0) = 1.0; + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + model.X_J[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]); + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + model.X_lambda[joint_id] = + Xrotx (q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + } + +}; + +struct CustomEulerZYXJoint : public CustomJoint +{ + CustomEulerZYXJoint () + { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ); + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0., 0., 0. + ); + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + //assert (false && "Not yet implemented!"); + } +}; + +//============================================================================== +//Test Fixture +//============================================================================== + +struct CustomJointSingleBodyFixture { + CustomJointSingleBodyFixture () { + + reference_model.resize(NUMBER_OF_MODELS); + custom_model.resize(NUMBER_OF_MODELS); + + body.resize(NUMBER_OF_MODELS); + custom_joint.resize(NUMBER_OF_MODELS); + + reference_body_id.resize(NUMBER_OF_MODELS); + custom_body_id.resize(NUMBER_OF_MODELS); + + q.resize(NUMBER_OF_MODELS); + qdot.resize(NUMBER_OF_MODELS); + qddot.resize(NUMBER_OF_MODELS); + tau.resize(NUMBER_OF_MODELS); + + //======================================================== + //Test Model 0: 3dof rotational custom joint vs. standard. + //======================================================== + + custom_joint0 = CustomEulerZYXJoint(); + + Matrix3d inertia0 = Matrix3d::Identity(3,3); + Body body0 = Body (1., Vector3d (1.1, 1.2, 1.3), inertia0); + + Model reference0, custom0; + + unsigned int reference_body_id0 = + reference0.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeEulerZYX), + body0); + + unsigned int custom_body_id0 = + custom0.AddBodyCustomJoint ( 0, + SpatialTransform(), + &custom_joint0, + body0); + + VectorNd q0 = VectorNd::Zero (reference0.q_size); + VectorNd qdot0 = VectorNd::Zero (reference0.qdot_size); + VectorNd qddot0 = VectorNd::Zero (reference0.qdot_size); + VectorNd tau0 = VectorNd::Zero (reference0.qdot_size); + + reference_model.at(0) = reference0; + custom_model.at(0) = custom0; + + reference_body_id.at(0) = (reference_body_id0); + custom_body_id.at(0) = (custom_body_id0); + + body.at(0) = (body0); + custom_joint.at(0) = (&custom_joint0); + + q.at(0) = (q0); + qdot.at(0) = (qdot0); + qddot.at(0) = (qddot0); + tau.at(0) = (tau0); + + //======================================================== + //Test Model 1: 1 dof rotational custom joint vs. standard. + //======================================================== + + custom_joint1 = CustomJointTypeRevoluteX(); + + Model reference1, custom1; + + unsigned int reference_body_id1 = + reference1.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body0); + + unsigned int custom_body_id1 = + custom1.AddBodyCustomJoint (0, + SpatialTransform(), + &custom_joint1, + body0); + + VectorNd q1 = VectorNd::Zero (reference1.q_size); + VectorNd qdot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd qddot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd tau1 = VectorNd::Zero (reference1.qdot_size); + + reference_model.at(1) = (reference1); + custom_model.at(1) = (custom1); + + reference_body_id.at(1) = (reference_body_id1); + custom_body_id.at(1) = (custom_body_id1); + + body.at(1) = (body0); + custom_joint.at(1) = (&custom_joint1); + + q.at(1) = (q1); + qdot.at(1) = (qdot1); + qddot.at(1) = (qddot1); + tau.at(1) = (tau1); + + } + + /* + ~CustomJointSingleBodyFixture () { + delete reference_model; + delete custom_model; + + delete body; + delete custom_joint; + + delete reference_body_id; + delete custom_body_id; + + delete q; + delete qdot; + delete qddot; + delete tau; + }*/ + + vector < Model > reference_model; + vector < Model > custom_model; + + vector < Body > body; + vector < CustomJoint* > custom_joint; + + vector < unsigned int > reference_body_id; + vector < unsigned int > custom_body_id; + + vector < VectorNd > q; + vector < VectorNd > qdot; + vector < VectorNd > qddot; + vector < VectorNd > tau; + CustomJointTypeRevoluteX custom_joint1; + CustomEulerZYXJoint custom_joint0; + +}; + +//============================================================================== +// +// Tests +// UpdateKinematicsCustom +// Jacobians +// InverseDynamics +// CompositeRigidBodyAlgorithm +// ForwardDynamics +// CalcMInvTimestau +// ForwardDynamicsContactsKokkevis +// +//============================================================================== + +TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) { + + VectorNd test; + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + + int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof ; i++) { + q.at(idx)[i] = i * 0.1; + qdot.at(idx)[i] = i * 0.15; + qddot.at(idx)[i] = i * 0.17; + } + + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(), + custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(), + 9, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[reference_body_id.at(idx)].data(), + custom_model.at(idx).v[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[reference_body_id.at(idx)].data(), + custom_model.at(idx).a[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), NULL, NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), NULL, NULL); + + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(), + custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(), + 9, + TEST_PREC); + + + //velocity + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + NULL); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[reference_body_id.at(idx)].data(), + custom_model.at(idx).v[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + + + //All + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[reference_body_id.at(idx)].data(), + custom_model.at(idx).a[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + } + + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + int dof = reference_model.at(idx).dof_count; + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + //Check the Spatial Jacobian + MatrixNd Gref = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + MatrixNd Gcus = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + CalcBodySpatialJacobian ( reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx), + Gref); + + CalcBodySpatialJacobian ( custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx), + Gcus); + + for(int i=0; i<6;++i){ + for(int j=0; j 1){ + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + VectorNd qddot_ref = VectorNd::Zero(dof); + VectorNd qddot_cus = VectorNd::Zero(dof); + + VectorNd qdot_plus_ref = VectorNd::Zero(dof); + VectorNd qdot_plus_cus = VectorNd::Zero(dof); + + Vector3d contact_point ( 0., 1., 0.); + + ConstraintSet constraint_set_ref; + ConstraintSet constraint_set_cus; + + //Reference + constraint_set_ref.AddContactConstraint( reference_body_id.at(idx), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_ref.AddContactConstraint( reference_body_id.at(idx), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_ref.Bind (reference_model.at(idx)); + + //Custom + constraint_set_cus.AddContactConstraint( custom_body_id.at(idx), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_cus.AddContactConstraint( custom_body_id.at(idx), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_cus.Bind (custom_model.at(idx)); + + ComputeConstraintImpulsesDirect(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_ref, + qdot_plus_ref); + + ForwardDynamicsContactsKokkevis (reference_model.at(idx), + q.at(idx), + qdot_plus_ref, + tau.at(idx), + constraint_set_ref, + qddot_ref); + + ComputeConstraintImpulsesDirect(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_cus, + qdot_plus_cus); + + ForwardDynamicsContactsKokkevis (custom_model.at(idx), + q.at(idx), + qdot_plus_cus, + tau.at(idx), + constraint_set_cus, + qddot_cus); + + VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus; + VectorNd qddot_error = qddot_ref - qddot_cus; + + CHECK_ARRAY_CLOSE (qdot_plus_ref.data(), + qdot_plus_cus.data(), + dof, + TEST_PREC); + + CHECK_ARRAY_CLOSE (qddot_ref.data(), + qddot_cus.data(), + dof, + TEST_PREC); + } + } + +} + + diff --git a/3rdparty/rbdl/tests/CustomJointTests.cc b/3rdparty/rbdl/tests/CustomJointTests.cc new file mode 100644 index 0000000..9f19344 --- /dev/null +++ b/3rdparty/rbdl/tests/CustomJointTests.cc @@ -0,0 +1,137 @@ +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct CustomEulerZYXJoint : public CustomJoint { + CustomEulerZYXJoint () { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + }; + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ); + + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + - c1 * qdot0 * qdot1, + -s1 * s2 * qdot0 * qdot1 + c1 * c2 * qdot0 * qdot2 - s2 * qdot1 * qdot2, + -s1 * c2 * qdot0 * qdot1 - c1 * s2 * qdot0 * qdot2 - c2 * qdot1 * qdot2, + 0., 0., 0. + ); + } + virtual void jcalc_X_lambda_S (Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ) { + // TODO + assert (false && "Not yet implemented!"); + } +}; + +struct CustomJointFixture { + CustomJointFixture () { + custom_joint = new CustomEulerZYXJoint(); + + Matrix3d inertia = Matrix3d::Identity(3,3); + body = Body (1., Vector3d (1.1, 1.2, 1.3), inertia); + reference_body_id = reference_model.AddBody (0,SpatialTransform(), Joint(JointTypeEulerZYX), body); + custom_body_id = custom_model.AddBodyCustomJoint (0, SpatialTransform(), custom_joint, body); + + q = VectorNd::Zero (reference_model.q_size); + qdot = VectorNd::Zero (reference_model.qdot_size); + qddot = VectorNd::Zero (reference_model.qdot_size); + tau = VectorNd::Zero (reference_model.qdot_size); + } + + ~CustomJointFixture () { + delete custom_joint; + } + + Model reference_model; + Model custom_model; + + Body body; + CustomJoint* custom_joint; + + unsigned int reference_body_id; + unsigned int custom_body_id; + + VectorNd q; + VectorNd qdot; + VectorNd qddot; + VectorNd tau; +}; + +TEST_FIXTURE ( CustomJointFixture, UpdateKinematics ) { + for (unsigned int i = 0; i < 3; i++) { + q[i] = i * 0.1; + qdot[i] = i * 0.15; + qddot[i] = i * 0.17; + } + + UpdateKinematics (reference_model, q, qdot, qddot); + UpdateKinematics (custom_model, q, qdot, qddot); + + CHECK_ARRAY_EQUAL (reference_model.X_base[reference_body_id].E.data(), custom_model.X_base[custom_body_id].E.data(), 9); + + CHECK_ARRAY_EQUAL (reference_model.v[reference_body_id].data(), custom_model.v[custom_body_id].data(), 6); + + CHECK_ARRAY_EQUAL (reference_model.a[reference_body_id].data(), custom_model.a[custom_body_id].data(), 6); +} + +// TODO: implement test for UpdateKinematicsCustom +// TODO: implement test for Jacobians +// TODO: implement test for InverseDynamics +// TODO: implement test for CompositeRigidBodyAlgorithm +// TODO: implement test for ForwardDynamics +// TODO: implement test for CalcMInvTimestau +// TODO: implement test for ForwardDynamicsContacts diff --git a/3rdparty/rbdl/tests/DynamicsTests.cc b/3rdparty/rbdl/tests/DynamicsTests.cc new file mode 100644 index 0000000..2ab0a33 --- /dev/null +++ b/3rdparty/rbdl/tests/DynamicsTests.cc @@ -0,0 +1,715 @@ +#include + +#include +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-13; + +struct DynamicsFixture { + DynamicsFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~DynamicsFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) { + Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + CHECK_EQUAL (-4.905, QDDot[0]); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) { + // This function checks the value for a non-trivial spatial inertia + Body body(1., Vector3d (1.5, 1., 1.), Vector3d (1., 2., 3.)); + + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + CHECK_EQUAL (-2.3544, QDDot[0]); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + +// cout << "--- Double Chain ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-5.88600000000000E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 3.92400000000000E+00, QDDot[1], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicTripleChain) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + Body body_c (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_c ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(2, Xtrans(Vector3d(1., 0., 0.)), joint_c, body_c); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- Triple Chain ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-6.03692307692308E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 3.77307692307692E+00, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 1.50923076923077E+00, QDDot[2], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain3D) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- Double Chain 3D ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-3.92400000000000E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 0.00000000000000E+00, QDDot[1], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSimpleTree3D) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b1 (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b1 ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b1, body_b1); + + Body body_c1 (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + Joint joint_c1 ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c1, body_c1); + + Body body_b2 (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b2 ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(-0.5, 0., 0.)), joint_b2, body_b2); + + Body body_c2 (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + Joint joint_c2 ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + model->AddBody(4, Xtrans(Vector3d(0., -0.5, 0.)), joint_c2, body_c2); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- SimpleTree ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-1.60319066147860E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE (-5.34396887159533E-01, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 4.10340466926070E+00, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 2.67198443579767E-01, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 5.30579766536965E+00, QDDot[4], TEST_PREC); +} + +TEST (TestForwardDynamicsLagrangian) { + Model model; + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + VectorNd Tau = VectorNd::Zero (model.dof_count); + + VectorNd QDDot_aba = VectorNd::Zero (model.dof_count); + VectorNd QDDot_lagrangian = VectorNd::Zero (model.dof_count); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(model, Q, QDot, Tau, QDDot_aba); + ForwardDynamicsLagrangian(model, Q, QDot, Tau, QDDot_lagrangian); + + CHECK_EQUAL (QDDot_aba.size(), QDDot_lagrangian.size()); + CHECK_ARRAY_CLOSE (QDDot_aba.data(), QDDot_lagrangian.data(), QDDot_aba.size(), TEST_PREC); +} + +/* + * A simple test for a model with 3 rotational dof. The reference value was + * computed with Featherstones spatial_v1 code. This test was written + * because my benchmark tool showed up inconsistencies, however this was + * due to the missing gravity term. But as the test now works, I just leave + * it here. + */ +TEST (TestForwardDynamics3DoFModel) { + Model model; + + model.gravity = Vector3d (0., -9.81, 0.); + + Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.)); + Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + unsigned int base_id_rot_z, base_id_rot_y; + + // we can reuse both bodies and joints as they are copied + base_id_rot_z = model.AddBody (0, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, null_body); + base_id_rot_y = model.AddBody (base_id_rot_z, Xtrans (Vector3d(0., 0., 0.)), joint_rot_y, null_body); + model.AddBody (base_id_rot_y, Xtrans (Vector3d(0., 0., 0.)), joint_rot_x, base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot_ref = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[0] = 1.; + + ClearLogOutput(); + + ForwardDynamics (model, Q, QDot, Tau, QDDot); + +// cout << LogOutput.str() << endl; + + QDDot_ref[0] = 3.301932144386186; + + CHECK_ARRAY_CLOSE (QDDot_ref.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +/* + * Another simple 3 dof model test which showed some problems when + * computing forward dynamics with the Lagrangian formulation. A proplem + * occured as the CRBA does not update the kinematics of the model, hence + * invalid body transformations and joint axis were used in the CRBA. + * Running the CRBA after the InverseDynamics calculation fixes this. This + * test ensures that the error does not happen when calling ForwardLagrangian. + */ +TEST (TestForwardDynamics3DoFModelLagrangian) { + Model model; + + model.gravity = Vector3d (0., -9.81, 0.); + + Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.)); + Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + unsigned int base_id_rot_z, base_id_rot_y; + + // we can reuse both bodies and joints as they are copied + base_id_rot_z = model.AddBody (0, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, null_body); + base_id_rot_y = model.AddBody (base_id_rot_z, Xtrans (Vector3d(0., 0., 0.)), joint_rot_y, null_body); + model.AddBody (base_id_rot_y, Xtrans (Vector3d(0., 0., 0.)), joint_rot_x, base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + VectorNd QDDot_ab = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot_lagrangian = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[1] = 1.; + ClearLogOutput(); + + Q[0] = 0.; + Q[1] = 1.; + Q[2] = 0.; + ForwardDynamicsLagrangian (model, Q, QDot, Tau, QDDot_lagrangian); + Q[0] = 0.; + Q[1] = 0.; + Q[2] = 1.; + ForwardDynamicsLagrangian (model, Q, QDot, Tau, QDDot_lagrangian); + ForwardDynamics (model, Q, QDot, Tau, QDDot_ab); + +// cout << QDDot_lagrangian << endl; +// cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (QDDot_ab.data(), QDDot_lagrangian.data(), QDDot_ab.size(), TEST_PREC); +} + +/* + * This is a test for a model where I detected incosistencies between the + * Lagragian method and the ABA. + */ +TEST (TestForwardDynamicsTwoLegModelLagrangian) { + Model *model = NULL; + + unsigned int hip_id, + upper_leg_right_id, + lower_leg_right_id, + foot_right_id, + upper_leg_left_id, + lower_leg_left_id, + foot_left_id; + Body hip_body, + upper_leg_right_body, + lower_leg_right_body, + foot_right_body, + upper_leg_left_body, + lower_leg_left_body, + foot_left_body; + + Joint joint_rot_z, joint_rot_y, joint_rot_x; + Joint joint_trans_z, joint_trans_y, joint_trans_x; + + ConstraintSet CS_right; + ConstraintSet CS_left; + ConstraintSet CS_both; + + model = new Model(); + + model->gravity = Vector3d (0., -9.81, 0.); + + joint_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + joint_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + joint_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + joint_trans_z = Joint ( SpatialVector (0., 0., 0., 0., 0., 1.)); + joint_trans_y = Joint ( SpatialVector (0., 0., 0., 0., 1., 0.)); + joint_trans_x = Joint ( SpatialVector (0., 0., 0., 1., 0., 0.)); + + Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + + // hip + hip_body = Body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + + // lateral right + upper_leg_right_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + lower_leg_right_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + foot_right_body = Body (1., Vector3d (0.15, -0.1, 0.), Vector3d (1., 1., 1.)); + + // lateral left + upper_leg_left_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + lower_leg_left_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + foot_left_body = Body (1., Vector3d (0.15, -0.1, 0.), Vector3d (1., 1., 1.)); + + // temporary value to store most recent body id + unsigned int temp_id; + + // add hip to the model (planar, 3 DOF) + temp_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_trans_x, null_body); + temp_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_trans_y, null_body); + hip_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, hip_body); + + // + // right leg + // + + // add right upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_right_body); + upper_leg_right_id = temp_id; + + // add the right lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, lower_leg_right_body); + lower_leg_right_id = temp_id; + + // add the right foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, foot_right_body); + foot_right_id = temp_id; + + // + // left leg + // + + // add left upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_left_body); + upper_leg_left_id = temp_id; + + // add the left lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, lower_leg_left_body); + lower_leg_left_id = temp_id; + + // add the left foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, foot_left_body); + foot_left_id = temp_id; + + LOG << "--- model created (" << model->dof_count << " DOF) ---" << endl; + + // contact data + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x"); + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y"); + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z"); + + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x"); + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y"); + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z"); + + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x"); + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y"); + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z"); + + CS_right.Bind(*model); + CS_left.Bind(*model); + CS_both.Bind(*model); + + VectorNd Q(model->dof_count); + VectorNd QDot(model->dof_count); + VectorNd Tau(model->dof_count); + VectorNd QDDot(model->dof_count); + VectorNd QDDotABA(model->dof_count); + + Q[0] = 0.8; + Q[1] = -7.76326e-06; + Q[2] = -1.58205e-07; + Q[3] = 1.57391e-07; + Q[4] = -1.03029e-09; + Q[5] = 7.92143e-08; + Q[6] = 1.57391e-07; + Q[7] = -1.03029e-09; + Q[8] = 7.92143e-08; + + QDot[0] = -1.77845e-06; + QDot[1] = -0.00905283; + QDot[2] = -0.000184484; + QDot[3] = 0.000183536; + QDot[4] = -1.20144e-06; + QDot[5] = 9.23727e-05; + QDot[6] = 0.000183536; + QDot[7] = -1.20144e-06; + QDot[8] = 9.23727e-05; + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 0.1; + Tau[4] = 0.1; + Tau[5] = 0.1; + Tau[6] = 0.1; + Tau[7] = 0.1; + Tau[8] = 0.1; + + // QDDot = 6.31843e-07 -6.12442e-07 9.22595e-14 3.3712e-07 4.27368e-07 -7.91795e-07 3.3712e-07 4.27368e-07 -7.91795e-07 + // QDDAB = -0.00192794 -9.81419 -0.2 0.198972 -0.00130243 0.100141 0.198972 -0.00130243 0.100141 + + ForwardDynamics (*model, Q, QDot, Tau, QDDotABA); + ClearLogOutput(); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot); + +// cout << LogOutput.str() << endl; + + // run it again to make sure the calculations give the same results and + // no invalid state information lingering in the model structure is being used + ForwardDynamics (*model, Q, QDot, Tau, QDDotABA); + ClearLogOutput(); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE (QDDotABA.data(), QDDot.data(), QDDotABA.size(), TEST_PREC); + + delete model; +} + +TEST_FIXTURE(FixedAndMovableJoint, TestForwardDynamicsFixedJoint) { + Q_fixed[0] = 1.1; + Q_fixed[1] = 2.2; + + QDot_fixed[0] = -3.2; + QDot_fixed[1] = -2.3; + + Tau_fixed[0] = 1.2; + Tau_fixed[1] = 2.1; + + Q = CreateDofVectorFromReducedVector (Q_fixed); + QDot = CreateDofVectorFromReducedVector (QDot_fixed); + + QDDot.setZero(); + + InverseDynamics (*model_movable, Q, QDot, QDDot, C_movable); + CompositeRigidBodyAlgorithm (*model_movable, Q, H_movable); + + H_fixed = CreateReducedInertiaMatrix (H_movable); + + C_fixed[0] = C_movable[0]; + C_fixed[1] = C_movable[2]; + + VectorNd QDDot_fixed_emulate(2); + CHECK (LinSolveGaussElimPivot (H_fixed, C_fixed * -1. + Tau_fixed, QDDot_fixed_emulate)); + + ForwardDynamics (*model_fixed, Q_fixed, QDot_fixed, Tau_fixed, QDDot_fixed); + + CHECK_ARRAY_CLOSE (QDDot_fixed_emulate.data(), QDDot_fixed.data(), 2, TEST_PREC); +} + +TEST_FIXTURE(FixedAndMovableJoint, TestInverseDynamicsFixedJoint) { + Q_fixed[0] = 1.1; + Q_fixed[1] = 2.2; + + QDot_fixed[0] = -3.2; + QDot_fixed[1] = -2.3; + + QDDot_fixed[0] = 1.2; + QDDot_fixed[1] = 2.1; + + Q = CreateDofVectorFromReducedVector (Q_fixed); + QDot = CreateDofVectorFromReducedVector (QDot_fixed); + QDDot = CreateDofVectorFromReducedVector (QDDot_fixed); + + InverseDynamics (*model_movable, Q, QDot, QDDot, Tau); + InverseDynamics (*model_fixed, Q_fixed, QDot_fixed, QDDot_fixed, Tau_fixed); + + VectorNd Tau_2dof (2); + Tau_2dof[0] = Tau[0]; + Tau_2dof[1] = Tau[2]; + + CHECK_ARRAY_CLOSE (Tau_2dof.data(), Tau_fixed.data(), 2, TEST_PREC); +} + +TEST_FIXTURE ( FloatingBase12DoF, TestForwardDynamicsLagrangianPrealloc ) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = static_cast(i + 1) * 0.1; + QDot[i] = static_cast(i + 1) * 1.1; + Tau[i] = static_cast(i + 1) * -1.2; + } + + ForwardDynamicsLagrangian (*model, + Q, + QDot, + Tau, + QDDot, + Math::LinearSolverPartialPivLU, + NULL, + NULL, + NULL + ); + + MatrixNd H (MatrixNd::Zero(model->dof_count, model->dof_count)); + VectorNd C (VectorNd::Zero(model->dof_count)); + VectorNd QDDot_prealloc (VectorNd::Zero (model->dof_count)); + ForwardDynamicsLagrangian (*model, + Q, + QDot, + Tau, + QDDot_prealloc, + Math::LinearSolverPartialPivLU, + NULL, + &H, + &C + ); + + CHECK_ARRAY_EQUAL (QDDot.data(), QDDot_prealloc.data(), model->dof_count); +} + +TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = rand() / static_cast(RAND_MAX); + Tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, Q, M); + + VectorNd qddot_solve_llt = M.llt().solve(Tau); + + VectorNd qddot_minv (Q); + CalcMInvTimesTau (*model, Q, Tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC); +} + +TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = rand() / static_cast(RAND_MAX); + Tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, Q, M); + + VectorNd qddot_solve_llt = M.llt().solve(Tau); + + VectorNd qddot_minv (Q); + CalcMInvTimesTau (*model, Q, Tau, qddot_minv); + + for (unsigned int j = 0; j < 1; j++) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Tau[i] = rand() / static_cast(RAND_MAX); + } + + CompositeRigidBodyAlgorithm (*model, Q, M); + qddot_solve_llt = M.llt().solve(Tau); + + CalcMInvTimesTau (*model, Q, Tau, qddot_minv, false); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC); + } +} diff --git a/3rdparty/rbdl/tests/Fixtures.h b/3rdparty/rbdl/tests/Fixtures.h new file mode 100644 index 0000000..983247b --- /dev/null +++ b/3rdparty/rbdl/tests/Fixtures.h @@ -0,0 +1,698 @@ +#include "rbdl/rbdl.h" + +struct FixedBase3DoF { + FixedBase3DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector(0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~FixedBase3DoF () { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +struct FixedBase6DoF { + FixedBase6DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_base_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + base_rot_z_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_z, base_rot_z); + + base_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_base_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + base_rot_y_id = model->AppendBody (Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_y, base_rot_y); + + base_rot_x = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_base_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + base_rot_x_id = model->AddBody (base_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_x, base_rot_x); + + // child body (3 DoF) + child_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_rot_z_id = model->AddBody (base_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_rot_z, child_rot_z); + + child_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_rot_y_id = model->AddBody (child_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_y, child_rot_y); + + child_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_rot_x_id = model->AddBody (child_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_x, child_rot_x); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + contact_body_id = child_rot_x_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_rot_z_id, base_rot_y_id, base_rot_x_id, + child_rot_z_id, child_rot_y_id, child_rot_x_id, + base_body_id; + + RigidBodyDynamics::Body base_rot_z, base_rot_y, base_rot_x, + child_rot_z, child_rot_y, child_rot_x; + + RigidBodyDynamics::Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x, + joint_child_rot_z, joint_child_rot_y, joint_child_rot_x; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + unsigned int contact_body_id; + RigidBodyDynamics::Math::Vector3d contact_point; + RigidBodyDynamics::Math::Vector3d contact_normal; + RigidBodyDynamics::ConstraintSet constraint_set; +}; + +struct FloatingBase12DoF { + FloatingBase12DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + base_rot_x = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + base_rot_x_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_rot_x); + + // child body 1 (3 DoF) + child_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_rot_z_id = model->AddBody (base_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_rot_z, child_rot_z); + + child_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_rot_y_id = model->AddBody (child_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_y, child_rot_y); + + child_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_rot_x_id = model->AddBody (child_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_x, child_rot_x); + + // child body (3 DoF) + child_2_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_2_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_2_rot_z_id = model->AddBody (child_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_2_rot_z, child_2_rot_z); + + child_2_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_2_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_2_rot_y_id = model->AddBody (child_2_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_2_rot_y, child_2_rot_y); + + child_2_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_2_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_2_rot_x_id = model->AddBody (child_2_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_2_rot_x, child_2_rot_x); + + Q = VectorNd::Constant (model->dof_count, 0.); + QDot = VectorNd::Constant (model->dof_count, 0.); + QDDot = VectorNd::Constant (model->dof_count, 0.); + Tau = VectorNd::Constant (model->dof_count, 0.); + + ClearLogOutput(); + } + + ~FloatingBase12DoF () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_rot_z_id, base_rot_y_id, base_rot_x_id, + child_rot_z_id, child_rot_y_id, child_rot_x_id, + child_2_rot_z_id, child_2_rot_y_id,child_2_rot_x_id, + base_body_id; + + RigidBodyDynamics::Body base_rot_z, base_rot_y, base_rot_x, + child_rot_z, child_rot_y, child_rot_x, + child_2_rot_z, child_2_rot_y, child_2_rot_x; + + RigidBodyDynamics::Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x, + joint_child_rot_z, joint_child_rot_y, joint_child_rot_x, + joint_child_2_rot_z, joint_child_2_rot_y, joint_child_2_rot_x; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; +}; + +struct SimpleFixture { + SimpleFixture () { + ClearLogOutput(); + model = new RigidBodyDynamics::Model; + model->gravity = RigidBodyDynamics::Math::Vector3d (0., -9.81, 0.); + } + ~SimpleFixture () { + delete model; + } + void ResizeVectors () { + Q = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + QDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + QDDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + Tau = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; +}; + +struct FixedJoint2DoF { + FixedJoint2DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint (JointTypeFixed); + + body_b_id = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~FixedJoint2DoF () { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +/** \brief Fixture that contains two models of which one has one joint fixed. +*/ +struct FixedAndMovableJoint { + FixedAndMovableJoint () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model_movable = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model_movable->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model_movable->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model_movable->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + C_movable = VectorNd::Zero ((size_t) model_movable->dof_count); + H_movable = MatrixNd::Zero ((size_t) model_movable->dof_count, (size_t) model_movable->dof_count); + + // Assemble the fixed joint model + model_fixed = new Model; + + body_a_fixed_id = model_fixed->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + Joint joint_fixed (JointTypeFixed); + body_b_fixed_id = model_fixed->AddBody(body_a_fixed_id, Xtrans(Vector3d(1., 0., 0.)), joint_fixed, body_b); + body_c_fixed_id = model_fixed->AddBody(body_b_fixed_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + QDot_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + QDDot_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + Tau_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + C_fixed = VectorNd::Zero ((size_t) model_fixed->dof_count); + H_fixed = MatrixNd::Zero ((size_t) model_fixed->dof_count, (size_t) model_fixed->dof_count); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + + ~FixedAndMovableJoint () { + delete model_movable; + delete model_fixed; + } + RigidBodyDynamics::Math::VectorNd CreateDofVectorFromReducedVector (const RigidBodyDynamics::Math::VectorNd &q_fixed) { + assert (q_fixed.size() == model_fixed->dof_count); + + RigidBodyDynamics::Math::VectorNd q_movable (model_movable->dof_count); + + q_movable[0] = q_fixed[0]; + q_movable[1] = 0.; + q_movable[2] = q_fixed[1]; + + return q_movable; + } + + RigidBodyDynamics::Math::MatrixNd CreateReducedInertiaMatrix(const RigidBodyDynamics::Math::MatrixNd &H_movable) { + assert (H_movable.rows() == model_movable->dof_count); + assert (H_movable.cols() == model_movable->dof_count); + RigidBodyDynamics::Math::MatrixNd H (model_fixed->dof_count, model_fixed->dof_count); + + H (0,0) = H_movable(0,0); H (0,1) = H_movable(0,2); + H (1,0) = H_movable(2,0); H (1,1) = H_movable(2,2); + + return H; + } + + RigidBodyDynamics::Model *model_fixed; + RigidBodyDynamics::Model *model_movable; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + unsigned int body_a_fixed_id, body_b_fixed_id, body_c_fixed_id; + + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + RigidBodyDynamics::Math::VectorNd C_movable; + RigidBodyDynamics::Math::MatrixNd H_movable; + + RigidBodyDynamics::Math::VectorNd Q_fixed; + RigidBodyDynamics::Math::VectorNd QDot_fixed; + RigidBodyDynamics::Math::VectorNd QDDot_fixed; + RigidBodyDynamics::Math::VectorNd Tau_fixed; + RigidBodyDynamics::Math::VectorNd C_fixed; + RigidBodyDynamics::Math::MatrixNd H_fixed; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +/** Model with two moving bodies and one fixed body +*/ +struct RotZRotZYXFixed { + RotZRotZYXFixed() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Body body_a(1., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + Body body_b(2., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + Body body_fixed(10., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + + fixture_transform_a = Xtrans (RigidBodyDynamics::Math::Vector3d(1., 2., 3.)); + fixture_transform_b = Xtrans (RigidBodyDynamics::Math::Vector3d(4., 5., 6.)); + fixture_transform_fixed = Xtrans (RigidBodyDynamics::Math::Vector3d(-1., -2., -3.)); + + body_a_id = model->AddBody (0, fixture_transform_a, joint_rot_z, body_a); + body_b_id = model->AppendBody (fixture_transform_b, joint_rot_zyx, body_b); + body_fixed_id = model->AppendBody (fixture_transform_fixed, Joint(JointTypeFixed), body_fixed); + + ClearLogOutput(); + } + ~RotZRotZYXFixed() { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_fixed_id; + + RigidBodyDynamics::Math::SpatialTransform fixture_transform_a; + RigidBodyDynamics::Math::SpatialTransform fixture_transform_b; + RigidBodyDynamics::Math::SpatialTransform fixture_transform_fixed; +}; + +struct TwoArms12DoF { + TwoArms12DoF() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * *----O----* + * | | + * | | + * * * + * | | + * | | + * + */ + + Body body_upper = Body (1., Vector3d (0., -0.2, 0.), Vector3d (1.1, 1.3, 1.5)); + Body body_lower = Body (0.5, Vector3d(0., -0.15, 0.), Vector3d (0.3, 0.5, 0.2)); + + Joint joint_zyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + right_upper_arm = model->AppendBody (Xtrans (Vector3d (0., 0., -0.3)), joint_zyx, body_upper, "RightUpper"); + // model->AppendBody (Xtrans (Vector3d (0., -0.4, 0.)), joint_zyx, body_lower, "RightLower"); + left_upper_arm = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.3)), joint_zyx, body_upper, "LeftUpper"); + // model->AppendBody (Xtrans (Vector3d (0., -0.4, 0.)), joint_zyx, body_lower, "LeftLower"); + + q = VectorNd::Constant ((size_t) model->dof_count, 0.); + qdot = VectorNd::Constant ((size_t) model->dof_count, 0.); + qddot = VectorNd::Constant ((size_t) model->dof_count, 0.); + tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ClearLogOutput(); + } + ~TwoArms12DoF() { + delete model; + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qdot; + RigidBodyDynamics::Math::VectorNd qddot; + RigidBodyDynamics::Math::VectorNd tau; + + unsigned int right_upper_arm, left_upper_arm; + +}; + +struct FixedBase6DoF12DoFFloatingBase { + FixedBase6DoF12DoFFloatingBase () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + base_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + // child body 1 (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + // child body (3 DoF) + child_2 = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_2_id = model->AddBody (child_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child_2); + + Q = VectorNd::Constant (model->dof_count, 0.); + QDot = VectorNd::Constant (model->dof_count, 0.); + QDDot = VectorNd::Constant (model->dof_count, 0.); + Tau = VectorNd::Constant (model->dof_count, 0.); + + contact_body_id = child_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF12DoFFloatingBase () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_id, child_id, child_2_id; + + RigidBodyDynamics::Body base, child, child_2; + + RigidBodyDynamics::Joint joint_rotzyx; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + unsigned int contact_body_id; + RigidBodyDynamics::Math::Vector3d contact_point; + RigidBodyDynamics::Math::Vector3d contact_normal; + RigidBodyDynamics::ConstraintSet constraint_set; +}; diff --git a/3rdparty/rbdl/tests/FloatingBaseTests.cc b/3rdparty/rbdl/tests/FloatingBaseTests.cc new file mode 100644 index 0000000..b77b310 --- /dev/null +++ b/3rdparty/rbdl/tests/FloatingBaseTests.cc @@ -0,0 +1,550 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct FloatingBaseFixture { + FloatingBaseFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + + base = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + } + ~FloatingBaseFixture () { + delete model; + } + Model *model; + Body base; + unsigned int base_body_id; + + VectorNd q, qdot, qddot, tau; +}; + +TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) { + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + q = VectorNd::Constant(model->dof_count, 0.); + qdot = VectorNd::Constant(model->dof_count, 0.); + qddot = VectorNd::Constant(model->dof_count, 0.); + tau = VectorNd::Constant(model->dof_count, 0.); + + q[1] = 1.; + ForwardDynamics (*model, q, qdot, tau, qddot); + + Vector3d test_point; + + test_point = CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false); + CHECK_ARRAY_CLOSE (Vector3d (0., -1., 0.).data(), test_point.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) { + // floating base + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + // body_a + Body body_a (1., Vector3d (1., 0., 0), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(base_body_id, Xtrans(Vector3d(2., 0., 0.)), joint_a, body_a); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Zero ((size_t) model->dof_count); + VectorNd QDot = VectorNd::Zero ((size_t) model->dof_count); + VectorNd QDDot = VectorNd::Zero ((size_t) model->dof_count); + VectorNd Tau = VectorNd::Zero ((size_t) model->dof_count); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC); + + // We rotate the base... let's see what happens... + Q[3] = 0.8; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC); + + // We apply a torqe let's see what happens... + Q[3] = 0.; + /* + rot_B[0] = 0.0; + X_B = XtransRotZYXEuler(pos_B, rot_B); + */ + + Tau[6] = 1.; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-8.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE (-1.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 2.0000, QDDot[6], TEST_PREC); +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) { + // floating base + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when moving along the X axis + QDot[0] = 1.; + Vector3d point_position(1., 0., 0.); + Vector3d point_velocity; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(1., point_velocity[0], TEST_PREC); + CHECK_CLOSE(0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; + + ClearLogOutput(); + + // Now we calculate the velocity when rotating around the Z axis + QDot[0] = 0.; + QDot[3] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(0., point_velocity[0], TEST_PREC); + CHECK_CLOSE(1., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; + + // Now we calculate the velocity when rotating around the Z axis and the + // base is rotated around the z axis by 90 degrees + ClearLogOutput(); + Q[3] = M_PI * 0.5; + QDot[3] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(-1., point_velocity[0], TEST_PREC); + CHECK_CLOSE(0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (1., 0., 0.); + Vector3d point_base_position; + Vector3d point_base_velocity; + Vector3d point_base_velocity_reference; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + point_base_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + point_base_velocity_reference = Vector3d ( + -3.888503432977729e-01, + -3.171179347202455e-01, + 1.093894197498446e+00 + ); + + CHECK_ARRAY_CLOSE (point_base_velocity_reference.data(), point_base_velocity.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero qddot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * Here we omit the term of the generalized acceleration by setting qddot + * to zero. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + // call ForwardDynamics to update the model + ForwardDynamics(*model, q, qdot, tau, qddot); + qddot = VectorNd::Zero (qddot.size()); + + qdot = qdot; + + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + // we set the generalized acceleration to zero + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00 + ); + Vector3d humans_point_velocity ( + 3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00 + ); + Vector3d humans_point_acceleration ( + -5.302760158847160e+00, 6.541369639625232e+00, -4.795115077652286e+00 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero q and qdot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * + * Here we set q and qdot to zero and only take into account values that + * are dependent on qddot. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + qddot = VectorNd::Zero (qddot.size()); + + qddot[0] = 0.1; + qddot[1] = 1.1; + qddot[2] = 1.2; + qddot[3] = 1.3; + qddot[4] = 1.5; + qddot[5] = 1.7; + + // cout << "ref_body_id = " << ref_body_id << endl; + // cout << "point_body_position = " << point_body_position << endl; + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -1.900000000000000e+00, -1.800000000000000e+00, 0.000000000000000e+00 + ); + Vector3d humans_point_velocity ( + 0.000000000000000e+00, 0.000000000000000e+00, 0.000000000000000e+00 + ); + Vector3d humans_point_acceleration ( + 2.440000000000000e+00, -1.370000000000000e+00, 9.899999999999999e-01 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero q and qdot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * + * Here we set q and qdot to zero and only take into account values that + * are dependent on qddot. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationFull) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + qddot[0] = 0.1; + qddot[1] = 1.1; + qddot[2] = 1.2; + qddot[3] = 1.3; + qddot[4] = 1.5; + qddot[5] = 1.7; + + // cout << "ref_body_id = " << ref_body_id << endl; + // cout << "point_body_position = " << point_body_position << endl; + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00 + ); + Vector3d humans_point_velocity ( + 3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00 + ); + Vector3d humans_point_acceleration ( + -4.993637532756404e+00, 1.043238173517606e+01, -6.948370826218673e-01 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + + diff --git a/3rdparty/rbdl/tests/Human36Fixture.h b/3rdparty/rbdl/tests/Human36Fixture.h new file mode 100644 index 0000000..088cb7f --- /dev/null +++ b/3rdparty/rbdl/tests/Human36Fixture.h @@ -0,0 +1,461 @@ +#ifndef RBDL_HUMAN36_FIXTURE +#define RBDL_HUMAN36_FIXTURE + +#include "rbdl/rbdl.h" + +struct Human36 { + RigidBodyDynamics::Model *model; + RigidBodyDynamics::Model *model_emulated; + RigidBodyDynamics::Model *model_3dof; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qdot; + RigidBodyDynamics::Math::VectorNd qddot; + RigidBodyDynamics::Math::VectorNd tau; + + RigidBodyDynamics::Math::VectorNd qddot_emulated; + RigidBodyDynamics::Math::VectorNd qddot_3dof; + + RigidBodyDynamics::ConstraintSet constraints_1B1C_emulated; + RigidBodyDynamics::ConstraintSet constraints_1B4C_emulated; + RigidBodyDynamics::ConstraintSet constraints_4B4C_emulated; + + RigidBodyDynamics::ConstraintSet constraints_1B1C_3dof; + RigidBodyDynamics::ConstraintSet constraints_1B4C_3dof; + RigidBodyDynamics::ConstraintSet constraints_4B4C_3dof; + + enum SegmentName { + SegmentPelvis = 0, + SegmentThigh, + SegmentShank, + SegmentFoot, + SegmentMiddleTrunk, + SegmentUpperTrunk, + SegmentUpperArm, + SegmentLowerArm, + SegmentHand, + SegmentHead, + SegmentNameLast + }; + + enum BodyName { + BodyPelvis, + BodyThighRight, + BodyShankRight, + BodyFootRight, + BodyThighLeft, + BodyShankLeft, + BodyFootLeft, + BodyMiddleTrunk, + BodyUpperTrunk, + BodyUpperArmRight, + BodyLowerArmRight, + BodyHandRight, + BodyUpperArmLeft, + BodyLowerArmLeft, + BodyHandLeft, + BodyHead, + BodyNameLast + }; + + enum DofNames { + PelvisTX, + PelvisTY, + PelvisTZ, + PelvisRY, + PelvisRX, + PelvisRZ, + HipRightRY, + HipRightRX, + HipRightRZ, + KneeRightRY, + AnkleRightRY, + AnkleRightRZ, + HipLeftRY, + HipLeftRX, + HipLeftRZ, + KneeLeftRY, + AnkleLeftRY, + AnkleLeftRZ, + LumbarRY, + LumbarRX, + LumbarRZ, + ShoulderRightRY, + ShoulderRightRX, + ShoulderRightRZ, + ElbowRightRY, + WristRightRY, + WristRightRZ, + ShoulderLeftRY, + ShoulderLeftRX, + ShoulderLeftRZ, + ElbowLeftRY, + WristLeftRY, + WristLeftRZ, + NeckRY, + NeckRX, + NeckRZ, + DofNameCount + }; + + double SegmentLengths[SegmentNameLast]; + double SegmentMass[SegmentNameLast]; + double SegmentCOM[SegmentNameLast][3]; + double SegmentRadiiOfGyration[SegmentNameLast][3]; + + unsigned int body_id_emulated[BodyNameLast]; + unsigned int body_id_3dof[BodyNameLast]; + + void initParameters () { + SegmentLengths[SegmentPelvis ] = 0.1457; + SegmentLengths[SegmentThigh ] = 0.4222; + SegmentLengths[SegmentShank ] = 0.4403; + SegmentLengths[SegmentFoot ] = 0.1037; + SegmentLengths[SegmentMiddleTrunk] = 0.2155; + SegmentLengths[SegmentUpperTrunk ] = 0.2421; + SegmentLengths[SegmentUpperArm ] = 0.2817; + SegmentLengths[SegmentLowerArm ] = 0.2689; + SegmentLengths[SegmentHand ] = 0.0862; + SegmentLengths[SegmentHead ] = 0.2429; + + SegmentMass[SegmentPelvis ] = 0.8154; + SegmentMass[SegmentThigh ] = 10.3368; + SegmentMass[SegmentShank ] = 3.1609; + SegmentMass[SegmentFoot ] = 1.001; + SegmentMass[SegmentMiddleTrunk] = 16.33; + SegmentMass[SegmentUpperTrunk ] = 15.96; + SegmentMass[SegmentUpperArm ] = 1.9783; + SegmentMass[SegmentLowerArm ] = 1.1826; + SegmentMass[SegmentHand ] = 0.4453; + SegmentMass[SegmentHead ] = 5.0662; + + SegmentCOM[SegmentPelvis ][0] = 0.; + SegmentCOM[SegmentPelvis ][1] = 0.; + SegmentCOM[SegmentPelvis ][2] = 0.0891; + + SegmentCOM[SegmentThigh ][0] = 0.; + SegmentCOM[SegmentThigh ][1] = 0.; + SegmentCOM[SegmentThigh ][2] = -0.1729; + + SegmentCOM[SegmentShank ][0] = 0.; + SegmentCOM[SegmentShank ][1] = 0.; + SegmentCOM[SegmentShank ][2] = -0.1963; + + SegmentCOM[SegmentFoot ][0] = 0.1254; + SegmentCOM[SegmentFoot ][1] = 0.; + SegmentCOM[SegmentFoot ][2] = -0.0516; + + SegmentCOM[SegmentMiddleTrunk][0] = 0.; + SegmentCOM[SegmentMiddleTrunk][1] = 0.; + SegmentCOM[SegmentMiddleTrunk][2] = 0.1185; + + SegmentCOM[SegmentUpperTrunk ][0] = 0.; + SegmentCOM[SegmentUpperTrunk ][1] = 0.; + SegmentCOM[SegmentUpperTrunk ][2] = 0.1195; + + SegmentCOM[SegmentUpperArm ][0] = 0.; + SegmentCOM[SegmentUpperArm ][1] = 0.; + SegmentCOM[SegmentUpperArm ][2] = -0.1626; + + SegmentCOM[SegmentLowerArm ][0] = 0.; + SegmentCOM[SegmentLowerArm ][1] = 0.; + SegmentCOM[SegmentLowerArm ][2] = -0.1230; + + SegmentCOM[SegmentHand ][0] = 0.; + SegmentCOM[SegmentHand ][1] = 0.; + SegmentCOM[SegmentHand ][2] = -0.0680; + + SegmentCOM[SegmentHead ][0] = 0.; + SegmentCOM[SegmentHead ][1] = 0.; + SegmentCOM[SegmentHead ][2] = 1.1214; + + SegmentRadiiOfGyration[SegmentPelvis ][0] = 0.0897; + SegmentRadiiOfGyration[SegmentPelvis ][1] = 0.0855; + SegmentRadiiOfGyration[SegmentPelvis ][2] = 0.0803; + + SegmentRadiiOfGyration[SegmentThigh ][0] = 0.1389; + SegmentRadiiOfGyration[SegmentThigh ][1] = 0.0629; + SegmentRadiiOfGyration[SegmentThigh ][2] = 0.1389; + + SegmentRadiiOfGyration[SegmentShank ][0] = 0.1123; + SegmentRadiiOfGyration[SegmentShank ][1] = 0.0454; + SegmentRadiiOfGyration[SegmentShank ][2] = 0.1096; + + SegmentRadiiOfGyration[SegmentFoot ][0] = 0.0267; + SegmentRadiiOfGyration[SegmentFoot ][1] = 0.0129; + SegmentRadiiOfGyration[SegmentFoot ][2] = 0.0254; + + SegmentRadiiOfGyration[SegmentMiddleTrunk][0] = 0.0970; + SegmentRadiiOfGyration[SegmentMiddleTrunk][1] = 0.1009; + SegmentRadiiOfGyration[SegmentMiddleTrunk][2] = 0.0825; + + SegmentRadiiOfGyration[SegmentUpperTrunk ][0] = 0.1273; + SegmentRadiiOfGyration[SegmentUpperTrunk ][1] = 0.1172; + SegmentRadiiOfGyration[SegmentUpperTrunk ][2] = 0.0807; + + SegmentRadiiOfGyration[SegmentUpperArm ][0] = 0.0803; + SegmentRadiiOfGyration[SegmentUpperArm ][1] = 0.0758; + SegmentRadiiOfGyration[SegmentUpperArm ][2] = 0.0445; + + SegmentRadiiOfGyration[SegmentLowerArm ][0] = 0.0742; + SegmentRadiiOfGyration[SegmentLowerArm ][1] = 0.0713; + SegmentRadiiOfGyration[SegmentLowerArm ][2] = 0.0325; + + SegmentRadiiOfGyration[SegmentHand ][0] = 0.0541; + SegmentRadiiOfGyration[SegmentHand ][1] = 0.0442; + SegmentRadiiOfGyration[SegmentHand ][2] = 0.0346; + + SegmentRadiiOfGyration[SegmentHead ][0] = 0.0736; + SegmentRadiiOfGyration[SegmentHead ][1] = 0.0634; + SegmentRadiiOfGyration[SegmentHead ][2] = 0.0765; + }; + + RigidBodyDynamics::Body create_body (SegmentName segment) { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + Matrix3d inertia_C (Matrix3d::Zero()); + inertia_C(0,0) = pow(SegmentRadiiOfGyration[segment][0] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(1,1) = pow(SegmentRadiiOfGyration[segment][1] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(2,2) = pow(SegmentRadiiOfGyration[segment][2] * SegmentLengths[segment], 2) * SegmentMass[segment]; + + return RigidBodyDynamics::Body ( + SegmentMass[segment], + RigidBodyDynamics::Math::Vector3d ( + SegmentCOM[segment][0], + SegmentCOM[segment][1], + SegmentCOM[segment][2] + ), + inertia_C); + } + + void generate () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + Body pelvis_body = create_body (SegmentPelvis); + Body thigh_body = create_body (SegmentThigh); + Body shank_body = create_body (SegmentShank); + Body foot_body = create_body (SegmentFoot); + Body middle_trunk_body = create_body (SegmentMiddleTrunk); + Body upper_trunk_body = create_body (SegmentUpperTrunk); + Body upperarm_body = create_body (SegmentUpperArm); + Body lowerarm_body = create_body (SegmentLowerArm); + Body hand_body = create_body (SegmentHand); + Body head_body = create_body (SegmentHead); + + Matrix3d zero_matrix (Matrix3d::Zero(3,3)); + Body null_body (0., Vector3d (0., 0., 0.), zero_matrix); + + Joint free_flyer ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_yxz_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint trans_xyz = Joint(JointTypeTranslationXYZ); + + Joint rot_yxz_3dof = Joint(JointTypeEulerYXZ); + + Joint rot_yz ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + Joint fixed (JointTypeFixed); + + // Generate emulated model + model_emulated->gravity = Vector3d (0., 0., -9.81); + + body_id_emulated[BodyPelvis] = model_emulated->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + body_id_emulated[BodyThighRight] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_r"); + body_id_emulated[BodyShankRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + body_id_emulated[BodyFootRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + body_id_emulated[BodyThighLeft] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_l"); + body_id_emulated[BodyShankLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + body_id_emulated[BodyFootLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + body_id_emulated[BodyMiddleTrunk] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_emulated, middle_trunk_body, "middletrunk"); + body_id_emulated[BodyUpperTrunk] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + body_id_emulated[BodyUpperArmRight] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_r"); + body_id_emulated[BodyLowerArmRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + body_id_emulated[BodyHandRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + body_id_emulated[BodyUpperArmLeft] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_l"); + body_id_emulated[BodyLowerArmLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + body_id_emulated[BodyHandLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + body_id_emulated[BodyHead] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "head"); + + // Generate 3dof model + model_3dof->gravity = Vector3d (0., 0., -9.81); + + unsigned int pelvis_trans = model_3dof->AddBody (0, Xtrans(Vector3d (0., 0., 0.)), trans_xyz, null_body, "pelvis_trans_xyz"); + + body_id_3dof[BodyPelvis] = model_3dof->AddBody (pelvis_trans, Xtrans (Vector3d (0., 0., 0.)), rot_yxz_3dof, pelvis_body, "pelvis"); + // body_id_3dof[BodyPelvis] = model_3dof->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + body_id_3dof[BodyThighRight] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_r"); + body_id_3dof[BodyShankRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + body_id_3dof[BodyFootRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + body_id_3dof[BodyThighLeft] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_l"); + body_id_3dof[BodyShankLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + body_id_3dof[BodyFootLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + body_id_3dof[BodyMiddleTrunk] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_3dof, middle_trunk_body, "middletrunk"); + body_id_3dof[BodyUpperTrunk] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + body_id_3dof[BodyUpperArmRight] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_r"); + body_id_3dof[BodyLowerArmRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + body_id_3dof[BodyHandRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + body_id_3dof[BodyUpperArmLeft] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_l"); + body_id_3dof[BodyLowerArmLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + body_id_3dof[BodyHandLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + body_id_3dof[BodyHead] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "head"); + } + + void initConstraintSets () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + unsigned int foot_r_emulated = model_emulated->GetBodyId ("foot_r"); + unsigned int foot_l_emulated = model_emulated->GetBodyId ("foot_l"); + unsigned int hand_r_emulated = model_emulated->GetBodyId ("hand_r"); + unsigned int hand_l_emulated = model_emulated->GetBodyId ("hand_l"); + + constraints_1B1C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B1C_emulated.Bind (*model_emulated); + + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_emulated.Bind (*model_emulated); + + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.Bind (*model); + + unsigned int foot_r_3dof = model_3dof->GetBodyId ("foot_r"); + unsigned int foot_l_3dof = model_3dof->GetBodyId ("foot_l"); + unsigned int hand_r_3dof = model_3dof->GetBodyId ("hand_r"); + unsigned int hand_l_3dof = model_3dof->GetBodyId ("hand_l"); + + constraints_1B1C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B1C_3dof.Bind (*model_3dof); + + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_3dof.Bind (*model_3dof); + + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.Bind (*model_3dof); + } + + void randomizeStates () { + for (int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + qddot_emulated = qddot; + qddot_3dof = qddot; + } + + Human36 () { + ClearLogOutput(); + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + initParameters(); + model_emulated = new RigidBodyDynamics::Model(); + model_3dof = new RigidBodyDynamics::Model(); + model = model_emulated; + generate(); + initConstraintSets(); + + q = VectorNd::Zero (model_emulated->q_size); + qdot = VectorNd::Zero (model_emulated->qdot_size); + qddot = VectorNd::Zero (model_emulated->qdot_size); + tau = VectorNd::Zero (model_emulated->qdot_size); + + qddot_emulated = VectorNd::Zero (model_emulated->qdot_size); + qddot_3dof= VectorNd::Zero (model_emulated->qdot_size); + }; + ~Human36 () { + delete model_emulated; + delete model_3dof; + } + +}; + +#endif diff --git a/3rdparty/rbdl/tests/ImpulsesTests.cc b/3rdparty/rbdl/tests/ImpulsesTests.cc new file mode 100644 index 0000000..2543a5f --- /dev/null +++ b/3rdparty/rbdl/tests/ImpulsesTests.cc @@ -0,0 +1,279 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ImpulsesFixture { + ImpulsesFixture () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + // base body + base = Body ( + 1., + Vector3d (0., 1., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body (3 DoF) + child = Body ( + 1., + Vector3d (0., 1., 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + Q = VectorNd::Zero(model->dof_count); + QDot = VectorNd::Zero(model->dof_count); + QDDot = VectorNd::Zero(model->dof_count); + Tau = VectorNd::Zero(model->dof_count); + + contact_body_id = child_id; + contact_point = Vector3d (0., 1., 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~ImpulsesFixture () { + delete model; + } + Model *model; + + unsigned int base_id, child_id; + Body base, child; + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; + + unsigned int contact_body_id; + Vector3d contact_point; + Vector3d contact_normal; + ConstraintSet constraint_set; +}; + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 0.; + constraint_set.v_plus[1] = 0.; + constraint_set.v_plus[2] = 0.; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 0.; + constraint_set.v_plus[1] = 0.; + constraint_set.v_plus[2] = 0.; + + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) { + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + ConstraintSet constraint_set_rangespace; + constraint_set_rangespace = constraint_set.Copy(); + constraint_set_rangespace.Bind (*model); + + VectorNd qdot_post_direct (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post_direct); + + VectorNd qdot_post_rangespace (QDot.size()); + ComputeConstraintImpulsesRangeSpaceSparse (*model, Q, QDot, constraint_set_rangespace, qdot_post_rangespace); + + Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true); + Vector3d point_velocity_rangespace = CalcPointVelocity (*model, Q, qdot_post_rangespace, contact_body_id, contact_point, true); + + CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_rangespace.data(), qdot_post_direct.rows(), TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_rangespace.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) { + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + ConstraintSet constraint_set_nullspace; + constraint_set_nullspace = constraint_set.Copy(); + constraint_set_nullspace.Bind (*model); + + VectorNd qdot_post_direct (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post_direct); + + VectorNd qdot_post_nullspace (QDot.size()); + ComputeConstraintImpulsesNullSpace (*model, Q, QDot, constraint_set, qdot_post_nullspace); + + Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true); + Vector3d point_velocity_nullspace = CalcPointVelocity (*model, Q, qdot_post_nullspace, contact_body_id, contact_point, true); + + CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_nullspace.data(), qdot_post_direct.rows(), TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_nullspace.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/InverseDynamicsTests.cc b/3rdparty/rbdl/tests/InverseDynamicsTests.cc new file mode 100644 index 0000000..943bd8d --- /dev/null +++ b/3rdparty/rbdl/tests/InverseDynamicsTests.cc @@ -0,0 +1,76 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct InverseDynamicsFixture { + InverseDynamicsFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~InverseDynamicsFixture () { + delete model; + } + Model *model; +}; + +#ifndef USE_SLOW_SPATIAL_ALGEBRA +TEST_FIXTURE(InverseDynamicsFixture, TestInverseForwardDynamicsFloatingBase) { + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd TauInv = VectorNd::Constant ((size_t) model->dof_count, 0.); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + InverseDynamics(*model, Q, QDot, QDDot, TauInv); + + CHECK_ARRAY_CLOSE (Tau.data(), TauInv.data(), Tau.size(), TEST_PREC); +} +#endif diff --git a/3rdparty/rbdl/tests/InverseKinematicsTests.cc b/3rdparty/rbdl/tests/InverseKinematicsTests.cc new file mode 100644 index 0000000..a50065a --- /dev/null +++ b/3rdparty/rbdl/tests/InverseKinematicsTests.cc @@ -0,0 +1,272 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +void print_ik_set (const InverseKinematicsConstraintSet &cs) { + int label_width = 18; + cout.width (label_width); + cout << "lambda: " << cs.lambda << endl; + cout.width (label_width); + cout << "num_steps: " << cs.num_steps << endl; + cout.width (label_width); + cout << "max_steps: " << cs.max_steps << endl; + cout.width (label_width); + cout << "step_tol: " << cs.step_tol << endl; + cout.width (label_width); + cout << "constraint_tol: " << cs.constraint_tol << endl; + cout.width (label_width); + cout << "error_norm: " << cs.error_norm << endl; +} + +/// Checks whether a single point in a 3-link chain can be reached +TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + + Vector3d local_point (1., 0., 0.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point); + + q.setZero(); + + InverseKinematicsConstraintSet cs; + cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point, target_position); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + if (!result) { + print_ik_set (cs); + } + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); +} + + +TEST_FIXTURE ( Human36, ManyPointConstraints ) { + + randomizeStates(); + + Vector3d local_point1 (1., 0., 0.); + Vector3d local_point2 (-1., 0., 0.); + Vector3d local_point3 (0., 1., 0.); + Vector3d local_point4 (1., 0., 1.); + Vector3d local_point5 (0.,0.,-1.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1); + Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2); + Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3); + Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4); + Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5); + + q.setZero(); + UpdateKinematicsCustom (*model, &q, NULL, NULL); + + InverseKinematicsConstraintSet cs; + cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1); + cs.AddPointConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2); + cs.AddPointConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3); + cs.AddPointConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4); + cs.AddPointConstraint (body_id_emulated[BodyHead], local_point5, target_position5); + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1); + Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2); + Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3); + Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4); + Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5); + + CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); +} + +/// Checks whether the end of a 3-link chain can aligned with a given +// orientation. +TEST_FIXTURE ( Human36, ChainSingleBodyOrientation ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + + InverseKinematicsConstraintSet cs; + cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation); + + VectorNd qres (q); + q.setZero(); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); +} + +TEST_FIXTURE ( Human36, ManyBodyOrientations ) { + + randomizeStates(); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false); + Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false); + Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false); + Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false); + + q.setZero(); + + InverseKinematicsConstraintSet cs; + cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation1); + cs.AddOrientationConstraint (body_id_emulated[BodyFootLeft], target_orientation2); + cs.AddOrientationConstraint (body_id_emulated[BodyHandRight], target_orientation3); + cs.AddOrientationConstraint (body_id_emulated[BodyHandLeft], target_orientation4); + cs.AddOrientationConstraint (body_id_emulated[BodyHead], target_orientation5); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false); + Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false); + Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false); + Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false); + Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false); + + CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); +} + +TEST_FIXTURE ( Human36, ChainSingleBodyFullConstraint ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + Vector3d local_point (1., 0., 0.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point); + + InverseKinematicsConstraintSet cs; + cs.AddFullConstraint(body_id_emulated[BodyFootRight],local_point,target_position, target_orientation); + + VectorNd qres (q); + q.setZero(); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); +} + +TEST_FIXTURE ( Human36, ManyBodyFullConstraints ) { + + randomizeStates(); + + Vector3d local_point1 (1., 0., 0.); + Vector3d local_point2 (-1., 0., 0.); + Vector3d local_point3 (0., 1., 0.); + Vector3d local_point4 (1., 0., 1.); + Vector3d local_point5 (0.,0.,-1.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + + Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1); + Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2); + Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3); + Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4); + Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5); + + Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false); + Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false); + Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false); + Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false); + + InverseKinematicsConstraintSet cs; + cs.AddFullConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1, target_orientation1); + cs.AddFullConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2, target_orientation2); + cs.AddFullConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3, target_orientation3); + cs.AddFullConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4, target_orientation4); + cs.AddFullConstraint (body_id_emulated[BodyHead], local_point5, target_position5, target_orientation5); + cs.step_tol = 1e-12; + + q.setZero(); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, cs.step_tol); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false); + Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false); + Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false); + Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false); + Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false); + + Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1); + Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2); + Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3); + Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4); + Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5); + + CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); + + CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/KinematicsTests.cc b/3rdparty/rbdl/tests/KinematicsTests.cc new file mode 100644 index 0000000..e939431 --- /dev/null +++ b/3rdparty/rbdl/tests/KinematicsTests.cc @@ -0,0 +1,680 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct KinematicsFixture { + KinematicsFixture () { + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * X + * * + * _/ + * _/ (-Z) + * Z / + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + body_d = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + body_d_id = model->AddBody(body_c_id, Xtrans(Vector3d(0., 0., -1.)), joint_c, body_d); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ClearLogOutput(); + } + + ~KinematicsFixture () { + delete model; + } + Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, body_d_id; + Body body_a, body_b, body_c, body_d; + Joint joint_a, joint_b, joint_c, joint_d; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; +}; + +struct KinematicsFixture6DoF { + KinematicsFixture6DoF () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + } + + ~KinematicsFixture6DoF () { + delete model; + } + Model *model; + + unsigned int base_id, child_id; + Body base, child; + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; +}; + + + +TEST_FIXTURE(KinematicsFixture, TestPositionNeutral) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotated90Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + + Q[0] = 0.5 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + // cout << LogOutput.str() << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (-1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (-1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotatedNeg45Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + + Q[0] = -0.25 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + // cout << LogOutput.str() << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0.707106781186547, -0.707106781186547, 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotated90Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + Q[1] = 0.5 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotatedNeg45Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + Q[1] = -0.25 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1 + 0.707106781186547, 1., -0.707106781186547), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinates) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 2., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)), + 3, TEST_PREC + ); +} + +TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinatesRotated) { + Q[2] = 0.5 * M_PI; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 1., 0.).data(), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false).data(), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 1., 0.).data(), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false).data(), + 3, TEST_PREC + ); + + // Rotate the other way round + Q[2] = -0.5 * M_PI; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (2., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), + 3, TEST_PREC + ); + + // Rotate around the base + Q[0] = 0.5 * M_PI; + Q[2] = 0.; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (-1., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (-2., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), + 3, TEST_PREC + ); + + // cout << LogOutput.str() << endl; +} + +TEST(TestCalcPointJacobian) { + Model model; + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + MatrixNd G = MatrixNd::Constant (3, model.dof_count, 0.); + Vector3d point_position (1.1, 1.2, 2.1); + Vector3d point_velocity_ref; + Vector3d point_velocity; + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.7; + Q[4] = 0.8; + Q[5] = 0.9; + + QDot[0] = -1.1; + QDot[1] = 2.2; + QDot[2] = 1.3; + QDot[3] = -2.7; + QDot[4] = 1.8; + QDot[5] = -2.9; + + // Compute the reference velocity + point_velocity_ref = CalcPointVelocity (model, Q, QDot, base_body_id, point_position); + + G.setZero(); + CalcPointJacobian (model, Q, base_body_id, point_position, G); + + point_velocity = G * QDot; + + CHECK_ARRAY_CLOSE ( + point_velocity_ref.data(), + point_velocity.data(), + 3, TEST_PREC + ); +} + +TEST_FIXTURE(KinematicsFixture, TestInverseKinematicSimple) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = body_d_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (1.3, 0., 0.); + + body_ids.push_back (body_d_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres); + // cout << LogOutput.str() << endl; + CHECK_EQUAL (true, res); + + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false); + + CHECK_ARRAY_CLOSE (target.data(), effector.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicUnreachable) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = child_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (2.2, 0., 0.); + + body_ids.push_back (body_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-8, 0.9, 1000); + // cout << LogOutput.str() << endl; + CHECK_EQUAL (true, res); + + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false); + + CHECK_ARRAY_CLOSE (Vector3d (2.0, 0., 0.).data(), effector.data(), 3, 1.0e-7); +} + +TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicTwoPoints) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = child_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (2., 0., 0.); + + body_ids.push_back (body_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + body_ids.push_back (base_id); + body_points.push_back (Vector3d (0.6, 1.0, 0.)); + target_pos.push_back (Vector3d (0.5, 1.1, 0.)); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-3, 0.9, 200); + CHECK_EQUAL (true, res); + + // cout << LogOutput.str() << endl; + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + + // testing with very low precision + effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[0], body_points[0], false); + CHECK_ARRAY_CLOSE (target_pos[0].data(), effector.data(), 3, 1.0e-1); + + effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[1], body_points[1], false); + CHECK_ARRAY_CLOSE (target_pos[1].data(), effector.data(), 3, 1.0e-1); +} + +TEST ( FixedJointBodyCalcBodyToBase ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 1., 0.1)); + + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 0.1).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBodyToBaseRotated ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector(0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + + ClearLogOutput(); + Q[0] = M_PI * 0.5; + Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q, fixed_body_id, Vector3d (1., 0., 0.)); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBaseToBody ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 2., 0.1)); + + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.1).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBaseToBodyRotated ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + + ClearLogOutput(); + Q[0] = M_PI * 0.5; + Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q, fixed_body_id, Vector3d (0., 2., 0.)); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyWorldOrientation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xrotz(0.25) * Xtrans (Vector3d (1., 2., 3.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Matrix3d orientation = CalcBodyWorldOrientation (model, Q_zero, fixed_body_id); + + Matrix3d reference = transform.E; + + CHECK_ARRAY_CLOSE (reference.data(), orientation.data(), 9, TEST_PREC); +} + +TEST ( FixedJointCalcPointJacobian ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xrotz(0.25) * Xtrans (Vector3d (1., 2., 3.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + Q[0] = 1.1; + QDot[0] = 1.2; + + Vector3d point_position (1., 0., 0.); + + MatrixNd G = MatrixNd::Zero (3, model.dof_count); + CalcPointJacobian (model, Q, fixed_body_id, point_position, G); + Vector3d point_velocity_jacobian = G * QDot; + Vector3d point_velocity_reference = CalcPointVelocity (model, Q, QDot, fixed_body_id, point_position); + + CHECK_ARRAY_CLOSE (point_velocity_reference.data(), point_velocity_jacobian.data(), 3, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SpatialJacobianSimple ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + + CalcBodySpatialJacobian (*model, q, foot_r_id, G); + + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_body = SpatialVector(G * qdot); + + CHECK_ARRAY_CLOSE (model->v[foot_r_id].data(), v_body.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SpatialJacobianFixedBody ) { + randomizeStates(); + + unsigned int uppertrunk_id = model->GetBodyId ("uppertrunk"); + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + + CalcBodySpatialJacobian (*model, q, uppertrunk_id, G); + + unsigned int fixed_body_id = uppertrunk_id - model->fixed_body_discriminator; + unsigned int movable_parent = model->mFixedBodies[fixed_body_id].mMovableParent; + + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_body = SpatialVector(G * qdot); + + SpatialVector v_fixed_body = model->mFixedBodies[fixed_body_id].mParentTransform.apply (model->v[movable_parent]); + + CHECK_ARRAY_CLOSE (v_fixed_body.data(), v_body.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointJacobian6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D velocity using the 6-D Jacobian + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + CalcPointJacobian6D (*model, q, foot_r_id, point_local, G); + SpatialVector v_foot_0_jac = SpatialVector (G * qdot); + + // Compute the 6-D velocity by transforming the body velocity to the + // reference point and aligning it with the base coordinate system + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id])); + + CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0_jac.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointVelocity6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D velocity + SpatialVector v_foot_0 = CalcPointVelocity6D (*model, q, qdot, foot_r_id, point_local); + + // Compute the 6-D velocity by transforming the body velocity to the + // reference point and aligning it with the base coordinate system + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id])); + + CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointAcceleration6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D acceleration + SpatialVector a_foot_0 = CalcPointAcceleration6D (*model, q, qdot, qddot, foot_r_id, point_local); + + // Compute the 6-D acceleration by adding the coriolis term to the + // acceleration of the body and transforming the result to the + // point and align it with the base coordinate system. + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + Vector3d v_foot_0 = CalcPointVelocity (*model, q, qdot, foot_r_id, point_local); + SpatialVector rdot (0., 0., 0., v_foot_0[0], v_foot_0[1], v_foot_0[2]); + + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector a_foot_0_ref = X_foot.apply( + model->X_base[foot_r_id].inverse().apply(model->a[foot_r_id]) + - crossm(rdot, + model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id]) + ) + ); + + CHECK_ARRAY_CLOSE (a_foot_0_ref.data(), a_foot_0.data(), 6, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/LoopConstraintsTests.cc b/3rdparty/rbdl/tests/LoopConstraintsTests.cc new file mode 100644 index 0000000..d8baf49 --- /dev/null +++ b/3rdparty/rbdl/tests/LoopConstraintsTests.cc @@ -0,0 +1,2146 @@ +#include +#include "rbdl/rbdl.h" +#include + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + +// Reduce an angle to the (-pi, pi] range. +static double inRange(double angle) { + while(angle > M_PI) { + angle -= 2. * M_PI; + } + while(angle <= -M_PI) { + angle += 2. * M_PI; + } + return angle; +} + +struct FourBarLinkage { + + FourBarLinkage() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(2.) + , l2(2.) + , m1(2.) + , m2(2.) + , idB1(0) + , idB2(0) + , idB3(0) + , idB4(0) + , idB5(0) + , X_p(Xtrans(Vector3d(l2, 0., 0.))) + , X_s(Xtrans(Vector3d(0., 0., 0.))) { + + Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.) + , Vector3d(0., 0., m1 * l1 * l1 / 3.)); + Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.) + , Vector3d(0., 0., m2 * l2 * l2 / 3.)); + Vector3d vector3d_zero = Vector3d::Zero(); + Body virtual_body(0., vector3d_zero, vector3d_zero); + Joint joint_rev_z(JointTypeRevoluteZ); + + idB1 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1); + idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2); + idB3 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1); + idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2); + idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), joint_rev_z + , virtual_body); + + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB1; + unsigned int idB2; + unsigned int idB3; + unsigned int idB4; + unsigned int idB5; + + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct FloatingFourBarLinkage { + + FloatingFourBarLinkage() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(2.) + , l2(2.) + , m1(2.) + , m2(2.) + , idB0(0) + , idB1(0) + , idB2(0) + , idB3(0) + , idB4(0) + , idB5(0) + , X_p(Xtrans(Vector3d(l2, 0., 0.))) + , X_s(Xtrans(Vector3d(0., 0., 0.))) { + + Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.) + , Vector3d(0., 0., m1 * l1 * l1 / 3.)); + Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.) + , Vector3d(0., 0., m2 * l2 * l2 / 3.)); + Vector3d vector3d_zero = Vector3d::Zero(); + Body virtual_body(0., vector3d_zero, vector3d_zero); + Joint joint_trans(JointTypeTranslationXYZ); + Joint joint_rev_z(JointTypeRevoluteZ); + + idB0 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_trans + , virtual_body); + idB1 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1); + idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2); + idB3 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1); + idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2); + idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), joint_rev_z + , virtual_body); + + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(1,0,0)); + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0,1,0)); + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0,0,1)); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB0; + unsigned int idB1; + unsigned int idB2; + unsigned int idB3; + unsigned int idB4; + unsigned int idB5; + + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct SliderCrank3D { + + SliderCrank3D() + : model() + , cs() + , q() + , qd() + , id_p(0) + , id_s(0) + , X_p() + , X_s() { + + double slider_mass = 5.; + double slider_height = 0.1; + double crank_link1_mass = 3.; + double crank_link1_length = 1.; + double crank_link2_mass = 1.; + double crank_link2_radius = 0.2; + double crank_link2_length = 3.; + double crank_link1_height = crank_link2_length - crank_link1_length + + slider_height; + + Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.)); + Body crankLink1(crank_link1_mass + , Vector3d(0.5 * crank_link1_length, 0., 0.) + , Vector3d(0., 0. + , crank_link1_mass * crank_link1_length * crank_link1_length / 3.)); + Body crankLink2(crank_link2_mass + , Vector3d(0.5 * crank_link2_length, 0., 0.) + , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius / 2. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12.)); + + Joint joint_rev_z(JointTypeRevoluteZ); + Joint joint_sphere(JointTypeEulerZYX); + Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.)); + + id_p = model.AddBody(0 + , SpatialTransform() + , joint_prs_x, slider); + unsigned int id_b1 = model.AddBody(0 + , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) + , joint_rev_z, crankLink1); + id_s = model.AddBody(id_b1 + , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.)) + , joint_sphere, crankLink2); + + X_p = Xtrans(Vector3d(0., 0., slider_height)); + X_s = SpatialTransform(roty(-0.5 * M_PI), Vector3d(crank_link2_length, 0, 0)); + + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,0,1), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + assert(rot_ps(0,0) - 1. < TEST_PREC); + assert(rot_ps(1,1) - 1. < TEST_PREC); + assert(rot_ps(2,2) - 1. < TEST_PREC); + assert(rot_ps(0,1) < TEST_PREC); + assert(rot_ps(0,2) < TEST_PREC); + assert(rot_ps(1,0) < TEST_PREC); + assert(rot_ps(1,2) < TEST_PREC); + assert(rot_ps(2,0) < TEST_PREC); + assert(rot_ps(2,1) < TEST_PREC); + assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + unsigned int id_p; + unsigned int id_s; + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct SliderCrank3DSphericalJoint { + + SliderCrank3DSphericalJoint() + : model() + , cs() + , q() + , qd() + , id_p(0) + , id_s(0) + , X_p() + , X_s() { + + double slider_mass = 5.; + double slider_height = 0.1; + double crank_link1_mass = 3.; + double crank_link1_length = 1.; + double crank_link2_mass = 1.; + double crank_link2_radius = 0.2; + double crank_link2_length = 3.; + double crank_link1_height = crank_link2_length - crank_link1_length + + slider_height; + + Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.)); + Body crankLink1(crank_link1_mass + , Vector3d(0.5 * crank_link1_length, 0., 0.) + , Vector3d(0., 0. + , crank_link1_mass * crank_link1_length * crank_link1_length / 3.)); + Body crankLink2(crank_link2_mass + , Vector3d(0.5 * crank_link2_length, 0., 0.) + , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius / 2. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12.)); + + Joint joint_rev_z(JointTypeRevoluteZ); + Joint joint_sphere(JointTypeSpherical); + Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.)); + + id_p = model.AddBody(0 + , SpatialTransform() + , joint_prs_x, slider); + unsigned int id_b1 = model.AddBody(0 + , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) + , joint_rev_z, crankLink1); + id_s = model.AddBody(id_b1 + , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.)) + , joint_sphere, crankLink2); + + X_p = Xtrans(Vector3d(0., 0., slider_height)); + X_s = SpatialTransform(roty(-0.5 * M_PI), Vector3d(crank_link2_length, 0, 0)); + + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,0,1), 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.q_size); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + assert(rot_ps(0,0) - 1. < TEST_PREC); + assert(rot_ps(1,1) - 1. < TEST_PREC); + assert(rot_ps(2,2) - 1. < TEST_PREC); + assert(rot_ps(0,1) < TEST_PREC); + assert(rot_ps(0,2) < TEST_PREC); + assert(rot_ps(1,0) < TEST_PREC); + assert(rot_ps(1,2) < TEST_PREC); + assert(rot_ps(2,0) < TEST_PREC); + assert(rot_ps(2,1) < TEST_PREC); + assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + unsigned int id_p; + unsigned int id_s; + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { + VectorNd err = VectorNd::Zero(cs.size()); + Vector3d pos1; + Vector3d pos2; + Vector3d posErr; + Matrix3d rot_p; + double angleErr; + + // Test in zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(0., err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(0., err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = M_PI * 3 / 4; + q[1] = -M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = 0.; + angleErr = sin(-0.5 * M_PI); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + assert(std::fabs(posErr[1]) < TEST_PREC); + assert(std::fabs(posErr[2]) < TEST_PREC); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = M_PI + 0.1; + q[3] = 0.; + q[4] = 0.; + angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(posErr[1], err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = 0.8; + q[1] = -0.4; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = 0.; + angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(posErr[1], err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + + // Zero Q configuration, both arms of the 4-bar laying on the x-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -1.; + qd[3] = -1.; + qd[4] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Both arms of the 4-bar laying on the y-axis + q[0] = 0.5 * M_PI; + q[1] = 0.; + q[2] = 0.5 * M_PI; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -1.; + qd[3] = -1.; + qd[4] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Arms symmetric wrt y axis. + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + + // Arms symmetric wrt y axis. + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + + // Invalid velocities. + qd[0] = -1.; + qd[1] = -1.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { + VectorNd weights(q.size()); + VectorNd err(cs.size()); + VectorNd errRef(VectorNd::Zero(cs.size())); + + weights[0] = 1.; + weights[1] = 0.; + weights[2] = 1.; + weights[3] = 0.; + weights[4] = 0.; + + VectorNd qRef = VectorNd::Zero(q.size()); + qRef[0] = M_PI * 3 / 4; + qRef[1] = -0.5 * M_PI; + qRef[2] = M_PI - qRef[0]; + qRef[3] = -qRef[1]; + qRef[4] = qRef[0] + qRef[1] - qRef[2] - qRef[3]; + assert(qRef[0] + qRef[1] - qRef[2] - qRef[3] - qRef[4] == 0.); + + bool success; + + // Feasible initial guess. + VectorNd qInit = VectorNd::Zero(q.size()); + qInit = qRef; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0]; + qInit[1] = qRef[1]; + qInit[2] = qRef[2]; + qInit[3] = qRef[3]; + qInit[4] = qRef[4] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0] - 0.2; + qInit[1] = qRef[1] - 0.; + qInit[2] = qRef[2] + 0.1; + qInit[3] = qRef[3] - 0.03; + qInit[4] = qRef[4] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0] + 0.01; + qInit[1] = qRef[1] + 0.02; + qInit[2] = qRef[2] - 0.03; + qInit[3] = qRef[3] - 0.02; + qInit[4] = qRef[4] + 0.01; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) { + VectorNd weights(q.size()); + + weights[0] = 1.; + weights[1] = 0.; + weights[2] = 1.; + weights[3] = 0.; + weights[4] = 0.; + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + VectorNd qdInit = VectorNd::Zero(q.size()); + qdInit[0] = 0.01; + qdInit[1] = 0.5; + qdInit[2] = -0.7; + qdInit[3] = -0.5; + qdInit[4] = 0.3; + + CalcAssemblyQDot(model, q, qdInit, cs, qd, weights); + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + CalcConstraintsJacobian(model, q, cs, G); + err = G * qd; + + CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); + CHECK_CLOSE(qdInit[2], qd[2], TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { + VectorNd qddDirect; + VectorNd qddNullSpace; + + cs.SetSolver(LinearSolverColPivHouseholderQR); + +#ifndef RBDL_USE_SIMPLE_MATH + // SimpleMath has no solver that can solve the system in this configuration. + // Configuration 1. + + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 1.; + tau[1] = -2.; + tau[2] = 3.; + tau[3] = -5.; + tau[4] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +#endif + + // Configuration 2. + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 1.; + tau[1] = -2.; + tau[2] = 3.; + tau[3] = -5.; + tau[4] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); + + // Note: + // The Range Space Sparse method can't be used because the H matrix has a 0 on + // the diagonal and the LTL factorization tries to divide by 0. + + // Note: + // LinearSolverPartialPivLU does not work because the A matrix in the dynamic + // system is not invertible. + + // Note: + // LinearSolverHouseholderQR sometimes does not work well when the system is + // in a singular configuration. +} + +TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errd(cs.size()); + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); +} + + + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) { + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + Vector3d rotationVec; + + // Test in zero position. + CalcConstraintsPositionError(model, q, cs, err); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Test in another configurations. + + q[0] = 0.4; + q[1] = 0.25 * M_PI; + q[2] = -0.25 * M_PI; + q[3] = 0.01; + q[4] = 0.01; + + CalcConstraintsPositionError(model, q, cs, err); + + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + rotationVec = - 0.5 * Vector3d + ( rot_ps(1,2) - rot_ps(2,1) + , rot_ps(2,0) - rot_ps(0,2) + , rot_ps(0,1) - rot_ps(1,0)); + errRef.block<3,1>(0,0) = pos_s - pos_p; + errRef[3] = rotationVec[2]; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + + // Test in zero position. + + G.setZero(); + CalcConstraintsJacobian(model, q, cs, G); + + VectorNd errRef(VectorNd::Zero(cs.size())); + VectorNd err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + VectorNd qWeights(q.size()); + VectorNd qInit(q.size()); + bool success; + + // Compute assembled configuration. + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + assert(success); + + // Some random velocity. + qd[0] = -0.2; + qd[1] = 0.1 * M_PI; + qd[2] = -0.1 * M_PI; + qd[3] = 0.; + qd[4] = 0.1 * M_PI; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQ) { + VectorNd weights(q.size()); + VectorNd qInit(q.size()); + + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + + bool success; + + weights[0] = 1.; + weights[1] = 1.; + weights[2] = 1.; + weights[3] = 1.; + weights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, TEST_PREC); + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + + CHECK(success); + CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); + CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) { + VectorNd qWeights(q.size()); + VectorNd qdWeights(q.size()); + VectorNd qInit(q.size()); + VectorNd qdInit(q.size()); + + SpatialVector vel_p; + SpatialVector vel_s; + + bool success; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + assert(success); + + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r); + vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + } + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { + VectorNd qWeights(q.size()); + VectorNd qdWeights(q.size()); + VectorNd qInit(q.size()); + VectorNd qdInit(q.size()); + + SpatialVector acc_p; + SpatialVector acc_s; + + bool success; + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Test with zero q and qdot. + + tau[0] = 0.12; + tau[1] = -0.3; + tau[2] = 0.05; + tau[3] = 0.7; + tau[4] = -0.1; + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +#endif + + // Compute non-zero assembly q and qdot; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + qdInit.setZero(); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + assert(success); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC); + assert(rot_ps(0,1) - rot_ps(0,1) < TEST_PREC); + assert((CalcPointVelocity6D(model, q, qd, id_p, X_p.r) + -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC); + + // Test with non-zero q and qdot. + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errdDirect(cs.size()); + VectorNd errdSpaceSparse(cs.size()); + VectorNd errdNullSpace(cs.size()); + + VectorNd qWeights(q.size()); + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + VectorNd qInit(q.size()); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + assert(success); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintErrors) { + + VectorNd err = VectorNd::Zero(cs.size()); + Vector3d pos0; + Vector3d pos1; + Vector3d pos2; + Vector3d posErr; + Matrix3d rot_p; + double angleErr; + + // Test in zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(0., err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(0., err[2], TEST_PREC); + CHECK_CLOSE(0., err[3], TEST_PREC); + CHECK_CLOSE(0., err[4], TEST_PREC); + CHECK_CLOSE(0., err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = 0.; + angleErr = sin(-0.5 * M_PI); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + assert(std::fabs(posErr[1]) < TEST_PREC); + assert(std::fabs(posErr[2]) < TEST_PREC); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(0., err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = 0.; + q[4] = 0.; + q[5] = M_PI + 0.1; + q[6] = 0.; + q[7] = 0.; + angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(posErr[1], err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = 0.8; + q[4] = -0.4; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = 0.; + angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(posErr[1], err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintJacobian) { + + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + + // Zero Q configuration, both arms of the 4-bar laying on the x-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -1.; + qd[6] = -1.; + qd[7] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Both arms of the 4-bar laying on the y-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.5 * M_PI; + q[4] = 0.; + q[5] = 0.5 * M_PI; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -1.; + qd[6] = -1.; + qd[7] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Arms symmetric wrt y axis. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintsVelocityErrors) { + + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + + // Arms symmetric wrt y axis. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + + // Invalid velocities. + qd[0] = -1.; + qd[1] = -1.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = 0.; + qd[6] = 0.; + qd[7] = 0.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { + VectorNd weights(q.size()); + VectorNd err(cs.size()); + VectorNd errRef(VectorNd::Zero(cs.size())); + + weights[0] = 0.; + weights[1] = 0.; + weights[2] = 0.; + weights[3] = 1.; + weights[4] = 0.; + weights[5] = 1.; + weights[6] = 0.; + weights[7] = 0.; + + VectorNd qRef = VectorNd::Zero(q.size()); + qRef[0] = 1.; + qRef[1] = 2.; + qRef[2] = 3.; + qRef[3] = M_PI * 3 / 4; + qRef[4] = -0.5 * M_PI; + qRef[5] = M_PI - qRef[3]; + qRef[6] = -qRef[4]; + qRef[7] = qRef[3] + qRef[4] - qRef[5] - qRef[6]; + + assert(qRef[3] + qRef[4] - qRef[5] - qRef[6] - qRef[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, qRef, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, qRef, idB5, X_s.r)).norm() < TEST_PREC); + + bool success; + + // Feasible initial guess. + VectorNd qInit = VectorNd::Zero(q.size()); + qInit = qRef; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3]; + qInit[4] = qRef[4]; + qInit[5] = qRef[5]; + qInit[6] = qRef[6]; + qInit[7] = qRef[7] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3] - 0.2; + qInit[4] = qRef[4] - 0.; + qInit[5] = qRef[5] + 0.1; + qInit[6] = qRef[6] - 0.03; + qInit[7] = qRef[7] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3] + 0.01; + qInit[4] = qRef[4] + 0.02; + qInit[5] = qRef[5] - 0.03; + qInit[6] = qRef[6] - 0.02; + qInit[7] = qRef[7] + 0.01; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) { + VectorNd weights(q.size()); + + weights[0] = 0.; + weights[1] = 0.; + weights[2] = 0.; + weights[3] = 1.; + weights[4] = 0.; + weights[5] = 1.; + weights[6] = 0.; + weights[7] = 0.; + + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + VectorNd qdInit = VectorNd::Zero(q.size()); + qdInit[0] = 1.; + qdInit[1] = 2.; + qdInit[2] = 3.; + qdInit[3] = 0.01; + qdInit[4] = 0.5; + qdInit[5] = -0.7; + qdInit[6] = -0.5; + qdInit[7] = 0.3; + + CalcAssemblyQDot(model, q, qdInit, cs, qd, weights); + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + CalcConstraintsJacobian(model, q, cs, G); + err = G * qd; + + CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + CHECK_CLOSE(qdInit[3], qd[3], TEST_PREC); + CHECK_CLOSE(qdInit[5], qd[5], TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageForwardDynamics) { + + VectorNd qddDirect; + VectorNd qddNullSpace; + + cs.SetSolver(LinearSolverColPivHouseholderQR); + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Configuration 1. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + qd[5] = 0.; + qd[6] = 0.; + qd[7] = 0.; + assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 0.; + tau[1] = 0.; + tau[2] = 0.; + tau[3] = 1.; + tau[4] = -2.; + tau[5] = 3.; + tau[6] = -5.; + tau[7] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +#endif + + // Configuration 2. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 0.; + tau[1] = 0.; + tau[2] = 0.; + tau[3] = 1.; + tau[4] = -2.; + tau[5] = 3.; + tau[6] = -5.; + tau[7] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errd(cs.size()); + + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + cs.v_plus[4] = 5.; + cs.v_plus[5] = 6.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + cs.v_plus[4] = 0.; + cs.v_plus[5] = 0.; + + qd[0] = 1.; + qd[2] = 2.; + qd[4] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintErrors) { + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + Vector3d rotationVec; + + // Test in zero position. + CalcConstraintsPositionError(model, q, cs, err); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Test in another configuration. + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.01, 0.01)); + q[0] = 0.4; + q[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, q); + + CalcConstraintsPositionError(model, q, cs, err); + + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + rotationVec = - 0.5 * Vector3d + ( rot_ps(1,2) - rot_ps(2,1) + , rot_ps(2,0) - rot_ps(0,2) + , rot_ps(0,1) - rot_ps(1,0)); + errRef.block<3,1>(0,0) = pos_s - pos_p; + errRef[3] = rotationVec[2]; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), model.dof_count)); + + // Test in zero position. + + G.setZero(); + CalcConstraintsJacobian(model, q, cs, G); + + VectorNd errRef(VectorNd::Zero(cs.size())); + VectorNd err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + VectorNd qWeights(model.dof_count); + VectorNd qInit(model.q_size); + bool success; + + // Compute assembled configuration. + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + assert(success); + + // Some random velocity. + qd[0] = -0.2; + qd[1] = 0.1 * M_PI; + qd[2] = -0.1 * M_PI; + qd[3] = 0.; + qd[4] = 0.1 * M_PI; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointAssemblyQ) { + VectorNd weights(model.dof_count); + VectorNd qInit(model.q_size); + + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + + bool success; + + weights[0] = 1.; + weights[1] = 1.; + weights[2] = 1.; + weights[3] = 1.; + weights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1e-14, 800); + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + + CHECK(success); + CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); + CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointAssemblyQDot) { + VectorNd qWeights(model.dof_count); + VectorNd qdWeights(model.dof_count); + VectorNd qInit(model.q_size); + VectorNd qdInit(model.dof_count); + + SpatialVector vel_p; + SpatialVector vel_s; + + bool success; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + assert(success); + + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r); + vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + } + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointForwardDynamics) { + + VectorNd qWeights(model.dof_count); + VectorNd qdWeights(model.dof_count); + VectorNd qInit(model.q_size); + VectorNd qdInit(model.dof_count); + + SpatialVector acc_p; + SpatialVector acc_s; + + bool success; + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Test with zero q and qdot. + + tau[0] = 0.12; + tau[1] = -0.3; + tau[2] = 0.05; + tau[3] = 0.7; + tau[4] = -0.1; + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +#endif + + // Compute non-zero assembly q and qdot; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + qdInit.setZero(); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + assert(success); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC); + assert(rot_ps(0,1) - rot_ps(0,1) < TEST_PREC); + assert((CalcPointVelocity6D(model, q, qd, id_p, X_p.r) + -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC); + + // Test with non-zero q and qdot. + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointImpulse) { + + VectorNd qdPlusDirect(model.dof_count); + VectorNd qdPlusRangeSpaceSparse(model.dof_count); + VectorNd qdPlusNullSpace(model.dof_count); + VectorNd errdDirect(cs.size()); + VectorNd errdSpaceSparse(cs.size()); + VectorNd errdNullSpace(cs.size()); + VectorNd qWeights(model.dof_count); + VectorNd qInit(model.q_size); + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + assert(success); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/MathTests.cc b/3rdparty/rbdl/tests/MathTests.cc new file mode 100644 index 0000000..d8d8fda --- /dev/null +++ b/3rdparty/rbdl/tests/MathTests.cc @@ -0,0 +1,109 @@ +#include + +#include "rbdl/Logging.h" +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" +#include + +const double TEST_PREC = 1.0e-14; + +using namespace std; +using namespace RigidBodyDynamics::Math; + +struct MathFixture { +}; + +TEST (GaussElimPivot) { + ClearLogOutput(); + + MatrixNd A; + A.resize(3,3); + VectorNd b(3); + VectorNd x(3); + + A(0,0) = 0; A(0,1) = 2; A(0,2) = 1; + A(1,0) = 1; A(1,1) = 1; A(1,2) = 5; + A(2,0) = 0; A(2,1) = 0; A(2,2) = 1; + + b[0] = 1; + b[1] = 2; + b[2] = 3; + + VectorNd test_result (3); + + test_result[0] = -12; + test_result[1] = -1; + test_result[2] = 3; + + LinSolveGaussElimPivot (A, b, x); + + CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC); + + A(0,0) = 0; A(0,1) = -2; A(0,2) = 1; + A(1,0) = 1; A(1,1) = 1; A(1,2) = 5; + A(2,0) = 0; A(2,1) = 0; A(2,2) = 1; + + LinSolveGaussElimPivot (A, b, x); + test_result[0] = -14; + test_result[1] = 1; + test_result[2] = 3; + + CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC); +} + +TEST (Dynamic_1D_initialize_value) { + VectorNd myvector_10 = VectorNd::Constant ((size_t) 10, 12.); + + double *test_values = new double[10]; + for (unsigned int i = 0; i < 10; i++) + test_values[i] = 12.; + + CHECK_ARRAY_EQUAL (test_values, myvector_10.data(), 10); + delete[] test_values; +} + +TEST (Dynamic_2D_initialize_value) { + MatrixNd mymatrix_10x10 = MatrixNd::Constant (10, 10, 12.); + + double *test_values = new double[10 * 10]; + for (unsigned int i = 0; i < 10; i++) + for (unsigned int j = 0; j < 10; j++) + test_values[i*10 + j] = 12.; + + CHECK_ARRAY_EQUAL (test_values, mymatrix_10x10.data(), 10*10); + delete[] test_values; +} + +TEST (SpatialMatrix_Multiplication) { + SpatialMatrix X_1 ( + 1., 2., 3., 4., 5., 6., + 11., 12., 13., 14., 15., 16., + 21., 22., 23., 24., 25., 26., + 31., 32., 33., 34., 35., 36., + 41., 42., 43., 44., 45., 46., + 51., 52., 53., 54., 55., 56. + ); + + SpatialMatrix X_2 (X_1); + + X_2 *= 2; + + SpatialMatrix correct_result ( + 1442, 1484, 1526, 1568, 1610, 1652, + 4562, 4724, 4886, 5048, 5210, 5372, + 7682, 7964, 8246, 8528, 8810, 9092, + 10802, 11204, 11606, 12008, 12410, 12812, + 13922, 14444, 14966, 15488, 16010, 16532, + 17042, 17684, 18326, 18968, 19610, 20252 + ); + + SpatialMatrix test_result = X_1 * X_2; + + CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC); + + // check the *= operator: + test_result = X_1; + test_result *= X_2; + + CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/ModelTests.cc b/3rdparty/rbdl/tests/ModelTests.cc new file mode 100644 index 0000000..1d46785 --- /dev/null +++ b/3rdparty/rbdl/tests/ModelTests.cc @@ -0,0 +1,604 @@ +#include + +#include + +#include "Fixtures.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ModelFixture { + ModelFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~ModelFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(ModelFixture, TestInit) { + CHECK_EQUAL (1u, model->lambda.size()); + CHECK_EQUAL (1u, model->mu.size()); + CHECK_EQUAL (0u, model->dof_count); + + CHECK_EQUAL (0u, model->q_size); + CHECK_EQUAL (0u, model->qdot_size); + + CHECK_EQUAL (1u, model->v.size()); + CHECK_EQUAL (1u, model->a.size()); + + CHECK_EQUAL (1u, model->mJoints.size()); + CHECK_EQUAL (1u, model->S.size()); + + CHECK_EQUAL (1u, model->c.size()); + CHECK_EQUAL (1u, model->IA.size()); + CHECK_EQUAL (1u, model->pA.size()); + CHECK_EQUAL (1u, model->U.size()); + CHECK_EQUAL (1u, model->d.size()); + CHECK_EQUAL (1u, model->u.size()); + CHECK_EQUAL (1u, model->Ic.size()); + CHECK_EQUAL (1u, model->I.size()); + + CHECK_EQUAL (1u, model->X_lambda.size()); + CHECK_EQUAL (1u, model->X_base.size()); + CHECK_EQUAL (1u, model->mBodies.size()); +} + +TEST_FIXTURE(ModelFixture, TestAddBodyDimensions) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int body_id = 0; + body_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + CHECK_EQUAL (1u, body_id); + CHECK_EQUAL (2u, model->lambda.size()); + CHECK_EQUAL (2u, model->mu.size()); + CHECK_EQUAL (1u, model->dof_count); + + CHECK_EQUAL (2u, model->v.size()); + CHECK_EQUAL (2u, model->a.size()); + + CHECK_EQUAL (2u, model->mJoints.size()); + CHECK_EQUAL (2u, model->S.size()); + + CHECK_EQUAL (2u, model->c.size()); + CHECK_EQUAL (2u, model->IA.size()); + CHECK_EQUAL (2u, model->pA.size()); + CHECK_EQUAL (2u, model->U.size()); + CHECK_EQUAL (2u, model->d.size()); + CHECK_EQUAL (2u, model->u.size()); + CHECK_EQUAL (2u, model->Ic.size()); + CHECK_EQUAL (2u, model->I.size()); + + SpatialVector spatial_zero; + spatial_zero.setZero(); + + CHECK_EQUAL (2u, model->X_lambda.size()); + CHECK_EQUAL (2u, model->X_base.size()); + CHECK_EQUAL (2u, model->mBodies.size()); +} + +TEST_FIXTURE(ModelFixture, TestFloatingBodyDimensions) { + Body body; + Joint float_base_joint (JointTypeFloatingBase); + + model->AppendBody (SpatialTransform(), float_base_joint, body); + + CHECK_EQUAL (3u, model->lambda.size()); + CHECK_EQUAL (3u, model->mu.size()); + CHECK_EQUAL (6u, model->dof_count); + CHECK_EQUAL (7u, model->q_size); + CHECK_EQUAL (6u, model->qdot_size); + + CHECK_EQUAL (3u, model->v.size()); + CHECK_EQUAL (3u, model->a.size()); + + CHECK_EQUAL (3u, model->mJoints.size()); + CHECK_EQUAL (3u, model->S.size()); + + CHECK_EQUAL (3u, model->c.size()); + CHECK_EQUAL (3u, model->IA.size()); + CHECK_EQUAL (3u, model->pA.size()); + CHECK_EQUAL (3u, model->U.size()); + CHECK_EQUAL (3u, model->d.size()); + CHECK_EQUAL (3u, model->u.size()); + + SpatialVector spatial_zero; + spatial_zero.setZero(); + + CHECK_EQUAL (3u, model->X_lambda.size()); + CHECK_EQUAL (3u, model->X_base.size()); + CHECK_EQUAL (3u, model->mBodies.size()); +} + +/** \brief Tests whether the joint and body information stored in the Model are computed correctly +*/ +TEST_FIXTURE(ModelFixture, TestAddBodySpatialValues) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + SpatialVector spatial_joint_axis(0., 0., 1., 0., 0., 0.); + CHECK_EQUAL (spatial_joint_axis, joint.mJointAxes[0]); + + // \Todo: Dynamic properties +} + +TEST_FIXTURE(ModelFixture, TestAddBodyTestBodyName) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body, "mybody"); + + unsigned int body_id = model->GetBodyId("mybody"); + + CHECK_EQUAL (1u, body_id); + CHECK_EQUAL (std::numeric_limits::max(), model->GetBodyId("unknownbody")); +} + +TEST_FIXTURE(ModelFixture, TestjcalcSimple) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, body); + + VectorNd Q = VectorNd::Zero (model->q_size); + VectorNd QDot = VectorNd::Zero (model->q_size); + + QDot[0] = 1.; + jcalc (*model, 1, Q, QDot); + + SpatialMatrix test_matrix ( + 1., 0., 0., 0., 0., 0., + 0., 1., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., 0., 0., 1. + ); + SpatialVector test_vector ( + 0., 0., 1., 0., 0., 0. + ); + SpatialVector test_joint_axis ( + 0., 0., 1., 0., 0., 0. + ); + + CHECK (SpatialMatrixCompareEpsilon (test_matrix, model->X_J[1].toMatrix(), 1.0e-16)); + CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16)); + CHECK_EQUAL (test_joint_axis, model->S[1]); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + + jcalc (*model, 1, Q, QDot); + + test_matrix.set ( + 0., 1., 0., 0., 0., 0., + -1., 0., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., -1., 0., 0., + 0., 0., 0., 0., 0., 1. + ); + + CHECK (SpatialMatrixCompareEpsilon (test_matrix, model->X_J[1].toMatrix(), 1.0e-16)); + CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16)); + CHECK_EQUAL (test_joint_axis, model->S[1]); +} + +TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) { + Body body; + + unsigned int body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + body); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + Vector3d base_coords (0., 0., 0.); + Vector3d body_coords; + Vector3d base_coords_back; + + UpdateKinematics (*model, q, qdot, qddot); + body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false); + base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false); + + CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC); + + q[0] = 1.; + q[1] = 0.2; + q[2] = -2.3; + q[3] = -2.3; + q[4] = 0.03; + q[5] = -0.23; + + UpdateKinematics (*model, q, qdot, qddot); + body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false); + base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false); + + CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC); +} + +TEST ( Model2DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( Model3DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + unsigned int body_id; + + // in total we add two bodies to make sure that the transformations are + // correct. + model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + // in total we add two bodies to make sure that the transformations are + // correct. + body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( Model6DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + unsigned int body_id; + + Joint joint_floating_base ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + body_id = model_std.AddBody (0, SpatialTransform(), joint_floating_base, body); + + model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + // in total we add two bodies to make sure that the transformations are + // correct. + body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_floating_base, body); + body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + assert (model_std.q_size == model_2.q_size); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( ModelFixedJointQueryBodyId ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + + CHECK_EQUAL (fixed_body_id, model.GetBodyId("fixed_body")); +} + +/* + * Makes sure that when appending a body to a fixed joint the parent of the + * newly added parent is actually the moving body that the fixed body is + * attached to. + */ +TEST ( ModelAppendToFixedBody ) { + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + // unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (movable_body + 1, appended_body_id); + CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); +} + +// Adds a fixed body to another fixed body. +TEST ( ModelAppendFixedToFixedBody ) { + Body null_body; + + double movable_mass = 1.1; + Vector3d movable_com (1., 0.4, 0.4); + + double fixed_mass = 1.2; + Vector3d fixed_com (1.1, 0.5, 0.5); + + Vector3d fixed_displacement (0., 1., 0.); + + Body body(movable_mass, movable_com, Vector3d (1., 1., 1.)); + Body fixed_body(fixed_mass, fixed_com, Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int fixed_body_2_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body_2"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (movable_body + 1, appended_body_id); + CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); + CHECK_EQUAL (movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass); + + CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent); + CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent); + + double new_mass = 3.5; + Vector3d new_com = (1. / new_mass) * (movable_mass * movable_com + fixed_mass * (fixed_com + fixed_displacement) + fixed_mass * (fixed_com + fixed_displacement * 2.)); + + CHECK_ARRAY_CLOSE (new_com.data(), model.mBodies[movable_body].mCenterOfMass.data(), 3, TEST_PREC); +} + +// Ensures that the transformations of the movable parent and fixed joint +// frame is in proper order +TEST ( ModelFixedJointRotationOrderTranslationRotation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.)); + SpatialTransform rot_z = Xrotz (45. * M_PI / 180.); + + model.AddBody (0, trans_x, joint_rot_z, body); + model.AppendBody (rot_z, Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + Q[0] = 45 * M_PI / 180.; + Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point.data(), 3, TEST_PREC); +} + +// Ensures that the transformations of the movable parent and fixed joint +// frame is in proper order +TEST ( ModelFixedJointRotationOrderRotationTranslation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + SpatialTransform rot_z = Xrotz (45. * M_PI / 180.); + SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.)); + + model.AddBody (0, rot_z, joint_rot_z, body); + model.AppendBody (trans_x, Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + Q[0] = 45 * M_PI / 180.; + Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (-1., 2., 0.).data(), point.data(), 3, TEST_PREC); +} + +TEST ( ModelGetBodyName ) { + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (string("fixed_body"), model.GetBodyName(fixed_body_id)); + CHECK_EQUAL (string("appended_body"), model.GetBodyName(appended_body_id)); + CHECK_EQUAL (string(""), model.GetBodyName(123)); +} + +TEST_FIXTURE ( RotZRotZYXFixed, ModelGetParentBodyId ) { + CHECK_EQUAL (0u, model->GetParentBodyId(0)); + CHECK_EQUAL (0u, model->GetParentBodyId(body_a_id)); + CHECK_EQUAL (body_a_id, model->GetParentBodyId(body_b_id)); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetParentIdFixed) { + CHECK_EQUAL (body_b_id, model->GetParentBodyId(body_fixed_id)); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrame) { + SpatialTransform transform_a = model->GetJointFrame (body_a_id); + SpatialTransform transform_b = model->GetJointFrame (body_b_id); + SpatialTransform transform_root = model->GetJointFrame (0); + + CHECK_ARRAY_EQUAL (fixture_transform_a.r.data(), transform_a.r.data(), 3); + CHECK_ARRAY_EQUAL (fixture_transform_b.r.data(), transform_b.r.data(), 3); + CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrameFixed) { + SpatialTransform transform_fixed = model->GetJointFrame (body_fixed_id); + + CHECK_ARRAY_EQUAL (fixture_transform_fixed.r.data(), transform_fixed.r.data(), 3); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelSetJointFrame) { + SpatialTransform new_transform_a = Xtrans (Vector3d(-1., -2., -3.)); + SpatialTransform new_transform_b = Xtrans (Vector3d(-4., -5., -6.)); + SpatialTransform new_transform_root = Xtrans (Vector3d(-99, -99., -99.)); + + model->SetJointFrame (body_a_id, new_transform_a); + model->SetJointFrame (body_b_id, new_transform_b); + model->SetJointFrame (0, new_transform_root); + + SpatialTransform transform_a = model->GetJointFrame (body_a_id); + SpatialTransform transform_b = model->GetJointFrame (body_b_id); + SpatialTransform transform_root = model->GetJointFrame (0); + + CHECK_ARRAY_EQUAL (new_transform_a.r.data(), transform_a.r.data(), 3); + CHECK_ARRAY_EQUAL (new_transform_b.r.data(), transform_b.r.data(), 3); + CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); +} + +TEST (CalcBodyWorldOrientationFixedJoint) { + Model model_fixed; + Model model_movable; + + Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.)); + Joint joint_fixed (JointTypeFixed); + Joint joint_rot_x = (SpatialVector (1., 0., 0., 0., 0., 0.)); + + model_fixed.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body); + unsigned int body_id_fixed = model_fixed.AppendBody (Xroty (45 * M_PI / 180), joint_fixed, body); + + model_movable.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body); + unsigned int body_id_movable = model_movable.AppendBody (Xroty (45 * M_PI / 180), joint_rot_x, body); + + VectorNd q_fixed (VectorNd::Zero (model_fixed.q_size)); + VectorNd q_movable (VectorNd::Zero (model_movable.q_size)); + + Matrix3d E_fixed = CalcBodyWorldOrientation (model_fixed, q_fixed, body_id_fixed); + Matrix3d E_movable = CalcBodyWorldOrientation (model_movable, q_movable, body_id_movable); + + CHECK_ARRAY_CLOSE (E_movable.data(), E_fixed.data(), 9, TEST_PREC); +} + diff --git a/3rdparty/rbdl/tests/MultiDofTests.cc b/3rdparty/rbdl/tests/MultiDofTests.cc new file mode 100644 index 0000000..a12d45c --- /dev/null +++ b/3rdparty/rbdl/tests/MultiDofTests.cc @@ -0,0 +1,1055 @@ +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct SphericalJoint { + SphericalJoint () { + ClearLogOutput(); + + emulated_model.gravity = Vector3d (0., 0., -9.81); + multdof3_model.gravity = Vector3d (0., 0., -9.81); + eulerzyx_model.gravity = Vector3d (0., 0., -9.81); + + body = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + joint_rot_zyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + joint_spherical = Joint (JointTypeSpherical); + joint_eulerzyx = Joint (JointTypeEulerZYX); + + joint_rot_y = Joint (SpatialVector (0., 1., 0., 0., 0., 0.)); + + emulated_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + emu_body_id = emulated_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_zyx, body); + emu_child_id = emulated_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + multdof3_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + sph_body_id = multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + sph_child_id = multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + eulerzyx_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + eulerzyx_body_id = eulerzyx_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_eulerzyx, body); + eulerzyx_child_id = eulerzyx_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + emuQ = VectorNd::Zero ((size_t) emulated_model.q_size); + emuQDot = VectorNd::Zero ((size_t) emulated_model.qdot_size); + emuQDDot = VectorNd::Zero ((size_t) emulated_model.qdot_size); + emuTau = VectorNd::Zero ((size_t) emulated_model.qdot_size); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + eulerzyxQ = VectorNd::Zero ((size_t) eulerzyx_model.q_size); + eulerzyxQDot = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + eulerzyxQDDot = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + eulerzyxTau = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + } + + Joint joint_rot_zyx; + Joint joint_spherical; + Joint joint_eulerzyx; + Joint joint_rot_y; + Body body; + + unsigned int emu_body_id, emu_child_id, + sph_body_id, sph_child_id, + eulerzyx_body_id, eulerzyx_child_id; + + Model emulated_model; + Model multdof3_model; + Model eulerzyx_model; + + VectorNd emuQ; + VectorNd emuQDot; + VectorNd emuQDDot; + VectorNd emuTau; + + VectorNd sphQ; + VectorNd sphQDot; + VectorNd sphQDDot; + VectorNd sphTau; + + VectorNd eulerzyxQ; + VectorNd eulerzyxQDot; + VectorNd eulerzyxQDDot; + VectorNd eulerzyxTau; +}; + +void ConvertQAndQDotFromEmulated ( + const Model &emulated_model, const VectorNd &q_emulated, const VectorNd &qdot_emulated, + const Model &multdof3_model, VectorNd *q_spherical, VectorNd *qdot_spherical) { + for (unsigned int i = 1; i < multdof3_model.mJoints.size(); i++) { + unsigned int q_index = multdof3_model.mJoints[i].q_index; + + if (multdof3_model.mJoints[i].mJointType == JointTypeSpherical) { + Quaternion quat = Quaternion::fromZYXAngles ( Vector3d ( + q_emulated[q_index + 0], q_emulated[q_index + 1], q_emulated[q_index + 2])); + multdof3_model.SetQuaternion (i, quat, (*q_spherical)); + + Vector3d omega = angular_velocity_from_angle_rates ( + Vector3d (q_emulated[q_index], q_emulated[q_index + 1], q_emulated[q_index + 2]), + Vector3d (qdot_emulated[q_index], qdot_emulated[q_index + 1], qdot_emulated[q_index + 2]) + ); + + (*qdot_spherical)[q_index] = omega[0]; + (*qdot_spherical)[q_index + 1] = omega[1]; + (*qdot_spherical)[q_index + 2] = omega[2]; + } else { + (*q_spherical)[q_index] = q_emulated[q_index]; + (*qdot_spherical)[q_index] = qdot_emulated[q_index]; + } + } +} + +TEST(TestQuaternionIntegration ) { + double timestep = 0.001; + + Vector3d zyx_angles_t0 (0.1, 0.2, 0.3); + Vector3d zyx_rates (3., 5., 2.); + Vector3d zyx_angles_t1 = zyx_angles_t0 + timestep * zyx_rates; + Quaternion q_zyx_t1 = Quaternion::fromZYXAngles (zyx_angles_t1); + + Quaternion q_t0 = Quaternion::fromZYXAngles (zyx_angles_t0); + Vector3d w_base = global_angular_velocity_from_rates (zyx_angles_t0, zyx_rates); + Quaternion q_t1 = q_t0.timeStep (w_base, timestep); + + // Note: we test with a rather crude precision. My guess for the error is + // that we compare two different things: + // A) integration under the assumption that the euler rates are + // constant + // B) integration under the assumption that the angular velocity is + // constant + // However I am not entirely sure about this... + CHECK_ARRAY_CLOSE (q_zyx_t1.data(), q_t1.data(), 4, 1.0e-5); +} + +TEST_FIXTURE(SphericalJoint, TestQIndices) { + CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); + CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); + CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); + + CHECK_EQUAL (5u, emulated_model.q_size); + CHECK_EQUAL (5u, emulated_model.qdot_size); + + CHECK_EQUAL (6u, multdof3_model.q_size); + CHECK_EQUAL (5u, multdof3_model.qdot_size); + CHECK_EQUAL (5u, multdof3_model.multdof3_w_index[2]); +} + +TEST_FIXTURE(SphericalJoint, TestGetQuaternion) { + multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + CHECK_EQUAL (10u, multdof3_model.q_size); + CHECK_EQUAL (8u, multdof3_model.qdot_size); + + CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); + CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); + CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); + CHECK_EQUAL (5u, multdof3_model.mJoints[4].q_index); + + CHECK_EQUAL (8u, multdof3_model.multdof3_w_index[2]); + CHECK_EQUAL (9u, multdof3_model.multdof3_w_index[4]); + + sphQ[0] = 100.; + sphQ[1] = 0.; + sphQ[2] = 1.; + sphQ[3] = 2.; + sphQ[4] = 100.; + sphQ[5] = -6.; + sphQ[6] = -7.; + sphQ[7] = -8; + sphQ[8] = 4.; + sphQ[9] = -9.; + + Quaternion reference_1 (0., 1., 2., 4.); + Quaternion quat_1 = multdof3_model.GetQuaternion (2, sphQ); + CHECK_ARRAY_EQUAL (reference_1.data(), quat_1.data(), 4); + + Quaternion reference_3 (-6., -7., -8., -9.); + Quaternion quat_3 = multdof3_model.GetQuaternion (4, sphQ); + CHECK_ARRAY_EQUAL (reference_3.data(), quat_3.data(), 4); +} + +TEST_FIXTURE(SphericalJoint, TestSetQuaternion) { + multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + Quaternion reference_1 (0., 1., 2., 3.); + multdof3_model.SetQuaternion (2, reference_1, sphQ); + Quaternion test = multdof3_model.GetQuaternion (2, sphQ); + CHECK_ARRAY_EQUAL (reference_1.data(), test.data(), 4); + + Quaternion reference_2 (11., 22., 33., 44.); + multdof3_model.SetQuaternion (4, reference_2, sphQ); + test = multdof3_model.GetQuaternion (4, sphQ); + CHECK_ARRAY_EQUAL (reference_2.data(), test.data(), 4); +} + +TEST_FIXTURE(SphericalJoint, TestOrientation) { + emuQ[0] = 1.1; + emuQ[1] = 1.1; + emuQ[2] = 1.1; + emuQ[3] = 1.1; + + for (unsigned int i = 0; i < emuQ.size(); i++) { + sphQ[i] = emuQ[i]; + } + + Quaternion quat = Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), emuQ[0]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), emuQ[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), emuQ[2]); + multdof3_model.SetQuaternion (2, quat, sphQ); + + Matrix3d emu_orientation = CalcBodyWorldOrientation (emulated_model, emuQ, emu_child_id); + Matrix3d sph_orientation = CalcBodyWorldOrientation (multdof3_model, sphQ, sph_child_id); + + CHECK_ARRAY_CLOSE (emu_orientation.data(), sph_orientation.data(), 9, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestUpdateKinematics) { + emuQ[0] = 1.; + emuQ[1] = 1.; + emuQ[2] = 1.; + emuQ[3] = 1.; + emuQ[4] = 1.; + + emuQDot[0] = 1.; + emuQDot[1] = 1.; + emuQDot[2] = 1.; + emuQDot[3] = 1.; + emuQDot[4] = 1.; + + emuQDDot[0] = 1.; + emuQDDot[1] = 1.; + emuQDDot[2] = 1.; + emuQDDot[3] = 1.; + emuQDDot[4] = 1.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDDot, multdof3_model, &sphQ, &sphQDDot); + + Vector3d a = angular_acceleration_from_angle_rates ( + Vector3d (emuQ[3], emuQ[2], emuQ[1]), + Vector3d (emuQDot[3], emuQDot[2], emuQDot[1]), + Vector3d (emuQDDot[3], emuQDDot[2], emuQDDot[1]) + ); + + sphQDDot[0] = emuQDDot[0]; + sphQDDot[1] = a[0]; + sphQDDot[2] = a[1]; + sphQDDot[3] = a[2]; + sphQDDot[4] = emuQDDot[4]; + + UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, &emuQDDot); + UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, &sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_body_id].data(), multdof3_model.v[sph_body_id].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (emulated_model.a[emu_body_id].data(), multdof3_model.a[sph_body_id].data(), 6, TEST_PREC); + + UpdateKinematics (multdof3_model, sphQ, sphQDot, sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestSpatialVelocities) { + emuQ[0] = 1.; + emuQ[1] = 2.; + emuQ[2] = 3.; + emuQ[3] = 4.; + + emuQDot[0] = 4.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 6.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, NULL); + UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, NULL); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestForwardDynamicsQAndQDot) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + + emuQDot[0] = 2.2; + emuQDot[1] = 2.3; + emuQDot[2] = 2.4; + emuQDot[3] = 2.5; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, emuQDDot); + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestDynamicsConsistencyRNEA_ABA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, sphQDDot); + + VectorNd tau_id (VectorNd::Zero (multdof3_model.qdot_size)); + InverseDynamics (multdof3_model, sphQ, sphQDot, sphQDDot, tau_id); + + CHECK_ARRAY_CLOSE (sphTau.data(), tau_id.data(), tau_id.size(), TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestCRBA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + MatrixNd H_crba (MatrixNd::Zero (multdof3_model.qdot_size, multdof3_model.qdot_size)); + + UpdateKinematicsCustom (multdof3_model, &sphQ, NULL, NULL); + CompositeRigidBodyAlgorithm (multdof3_model, sphQ, H_crba, false); + + MatrixNd H_id (MatrixNd::Zero (multdof3_model.qdot_size, multdof3_model.qdot_size)); + VectorNd H_col = VectorNd::Zero (multdof3_model.qdot_size); + VectorNd QDDot_zero = VectorNd::Zero (multdof3_model.qdot_size); + + for (unsigned int i = 0; i < multdof3_model.qdot_size; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (multdof3_model.qdot_size); + delta_a[i] = 1.; + + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (multdof3_model.qdot_size); + InverseDynamics (multdof3_model, sphQ, sphQDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (multdof3_model.qdot_size); + InverseDynamics (multdof3_model, sphQ, sphQDot, QDDot_zero, id_zero); + + H_col = id_delta - id_zero; + H_id.block(0, i, multdof3_model.qdot_size, 1) = H_col; + } + + CHECK_ARRAY_CLOSE (H_id.data(), H_crba.data(), multdof3_model.qdot_size * multdof3_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestForwardDynamicsLagrangianVsABA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + VectorNd QDDot_aba = VectorNd::Zero (multdof3_model.qdot_size); + VectorNd QDDot_lag = VectorNd::Zero (multdof3_model.qdot_size); + + ForwardDynamicsLagrangian (multdof3_model, sphQ, sphQDot, sphTau, QDDot_lag); + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, QDDot_aba); + + CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), multdof3_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestContactsLagrangian) { + ConstraintSet constraint_set_emu; + + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_emu.Bind(emulated_model); + + ConstraintSet constraint_set_sph; + + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_sph.Bind(multdof3_model); + + ForwardDynamicsConstraintsDirect (emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot); + VectorNd emu_force_lagrangian = constraint_set_emu.force; + ForwardDynamicsConstraintsDirect (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot); + VectorNd sph_force_lagrangian = constraint_set_sph.force; + + CHECK_ARRAY_CLOSE (emu_force_lagrangian.data(), sph_force_lagrangian.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestContacts) { + ConstraintSet constraint_set_emu; + + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_emu.Bind(emulated_model); + + ConstraintSet constraint_set_sph; + + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_sph.Bind(multdof3_model); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot); + VectorNd emu_force_kokkevis = constraint_set_emu.force; + ForwardDynamicsContactsKokkevis (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot); + VectorNd sph_force_kokkevis = constraint_set_sph.force; + + CHECK_ARRAY_CLOSE (emu_force_kokkevis.data(), sph_force_kokkevis.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedLagrangian ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedArticulatedBodyAlgorithm ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamics (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedContacts ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ConstraintSet CS_euler; + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (1., 1., 1.), Vector3d (1., 0., 0.)); + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.)); + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.)); + CS_euler.Bind (eulerzyx_model); + + ConstraintSet CS_emulated; + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (1., 1., 1.), Vector3d (1., 0., 0.)); + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.)); + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.)); + CS_emulated.Bind (emulated_model); + + ForwardDynamicsConstraintsDirect (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); + + ClearLogOutput(); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsContactsKokkevis (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm()); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm()); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedCRBA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + MatrixNd H_emulated (MatrixNd::Zero (emulated_model.q_size, emulated_model.q_size)); + MatrixNd H_eulerzyx (MatrixNd::Zero (eulerzyx_model.q_size, eulerzyx_model.q_size)); + + CompositeRigidBodyAlgorithm (emulated_model, emuQ, H_emulated); + CompositeRigidBodyAlgorithm (eulerzyx_model, emuQ, H_eulerzyx); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_eulerzyx.data(), emulated_model.q_size * emulated_model.q_size, TEST_PREC); +} + +TEST ( TestJointTypeTranslationZYX ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + Joint joint_3dof (JointTypeTranslationXYZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.1 * (static_cast(i + 1)); + qddot_3dof[i] = 0.21 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + qddot_emulated = qddot_3dof; + VectorNd id_emulated (tau); + VectorNd id_3dof(tau); + InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated); + InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof); + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm()); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_EQUAL (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size()); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST ( TestJointTypeEulerXYZ ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerXYZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated); + UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated); + + CHECK_ARRAY_EQUAL (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9); + CHECK_ARRAY_EQUAL (model_emulated.v[3].data(), model_3dof.v[1].data(), 6); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST ( TestJointTypeEulerYXZ ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot_3dof[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + qddot_emulated = qddot_3dof; + + UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated); + UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated); + + CHECK_ARRAY_CLOSE (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC); + + VectorNd id_emulated (tau); + VectorNd id_3dof(tau); + InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated); + InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof); + + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST_FIXTURE (Human36, TestUpdateKinematics) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd id_emulated (tau); + VectorNd id_3dof (tau); + + UpdateKinematics (*model_emulated, q, qdot, qddot); + UpdateKinematics (*model_3dof, q, qdot, qddot); + + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyPelvis]].data(), model_3dof->v[body_id_3dof[BodyPelvis]].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyThighRight]].data(), model_3dof->v[body_id_3dof[BodyThighRight]].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyShankRight]].data(), model_3dof->v[body_id_3dof[BodyShankRight]].data(), 6, TEST_PREC); +} + +TEST_FIXTURE (Human36, TestInverseDynamics) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd id_emulated (tau); + VectorNd id_3dof (tau); + + ClearLogOutput(); + InverseDynamics (*model_emulated, q, qdot, qddot, id_emulated); + InverseDynamics (*model_3dof, q, qdot, qddot, id_3dof); + + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestNonlinearEffects) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd nle_emulated (tau); + VectorNd nle_3dof (tau); + + ClearLogOutput(); + NonlinearEffects (*model_emulated, q, qdot, nle_emulated); + NonlinearEffects (*model_3dof, q, qdot, nle_3dof); + + CHECK_ARRAY_CLOSE (nle_emulated.data(), nle_3dof.data(), nle_emulated.size(), TEST_PREC * nle_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_kokkevis (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_kokkevis); + + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = -0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_sparse (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = -0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_nullspace (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + + for (unsigned int i = 0; i < q.size(); i++) { + CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + + for (unsigned int i = 0; i < q.size(); i++) { + CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]); + } + + VectorNd qddot_sparse (qddot_emulated); + VectorNd qddot_kokkevis (qddot_emulated); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_kokkevis (qddot_emulated); + VectorNd qddot_kokkevis_2 (qddot_emulated); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SolveMInvTimesTau) { + for (unsigned int i = 0; i < model->dof_count; i++) { + q[i] = rand() / static_cast(RAND_MAX); + tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, q, M); + + VectorNd qddot_solve_llt = M.llt().solve(tau); + + VectorNd qddot_minv (q); + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_minv.norm()); +} + +TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) { + for (unsigned int i = 0; i < model->dof_count; i++) { + q[i] = rand() / static_cast(RAND_MAX); + tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, q, M); + + VectorNd qddot_solve_llt = M.llt().solve(tau); + + VectorNd qddot_minv (q); + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + for (unsigned int j = 0; j < 1; j++) { + for (unsigned int i = 0; i < model->dof_count; i++) { + tau[i] = rand() / static_cast(RAND_MAX); + } + + CompositeRigidBodyAlgorithm (*model, q, M); + qddot_solve_llt = M.llt().solve(tau); + + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_solve_llt.norm()); + } +} diff --git a/3rdparty/rbdl/tests/ScrewJointTests.cc b/3rdparty/rbdl/tests/ScrewJointTests.cc new file mode 100644 index 0000000..e0a0680 --- /dev/null +++ b/3rdparty/rbdl/tests/ScrewJointTests.cc @@ -0,0 +1,115 @@ +#include + +#include +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + + + +const double TEST_PREC = 1.0e-12; + +struct ScrewJoint1DofFixedBase { + ScrewJoint1DofFixedBase() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Single screw joint with a fixed base. Rotation about Z, translation along X. + * A rolling log. + */ + + Body body = Body (1., Vector3d (0., 0, 0.), Vector3d (1., 1., 1.)); + + Joint joint = Joint (SpatialVector (0., 0., 1., 1., 0., 0.)); + + roller = model->AppendBody (Xtrans (Vector3d (0., 0., 0.)), joint, body, "Roller"); + + q = VectorNd::Constant ((size_t) model->dof_count, 0.); + qdot = VectorNd::Constant ((size_t) model->dof_count, 0.); + qddot = VectorNd::Constant ((size_t) model->dof_count, 0.); + tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + epsilon = 1e-8; + + ClearLogOutput(); + } + ~ScrewJoint1DofFixedBase() { + delete model; + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qdot; + RigidBodyDynamics::Math::VectorNd qddot; + RigidBodyDynamics::Math::VectorNd tau; + + unsigned int roller; + double epsilon; +}; + + +TEST_FIXTURE ( ScrewJoint1DofFixedBase, UpdateKinematics ) { + q[0] = 1; + qdot[0] = 2; + qddot[0] = 0; + + UpdateKinematics (*model, q, qdot, qddot); + + CHECK_ARRAY_EQUAL (Xrot(1,Vector3d(0,0,1)).E.data(), model->X_base[roller].E.data(), 9); + CHECK_ARRAY_EQUAL (Vector3d(1,0,0).data(), model->X_base[roller].r.data(), 3); + CHECK_ARRAY_EQUAL (SpatialVector(0.,0.,2.,cos(q[0])*2,-sin(q[0])*2.,0.).data(), model->v[roller].data(), 6); + + SpatialVector a0(model->a[roller]); + SpatialVector v0(model->v[roller]); + + q[0] = 1+2*epsilon; + qdot[0] = 2; + qddot[0] = 0; + + UpdateKinematics (*model, q, qdot, qddot); + + v0 = model->v[roller] - v0; + v0 /= epsilon; + + CHECK_ARRAY_CLOSE (a0.data(),v0.data(), 6, 1e-5); //finite diff vs. analytical derivative + +} + +TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) { + q[0] = 1; + qdot[0] = 0; + qddot[0] = 9; + + Vector3d refPt = Vector3d(1,0,3); + MatrixNd GrefPt = MatrixNd::Constant(3,1,0.); + MatrixNd Gexpected = MatrixNd::Constant(3,1,0.); + Vector3d refPtBaseCoord = Vector3d(); + + refPtBaseCoord = CalcBodyToBaseCoordinates(*model, q, roller, refPt); + + CHECK_ARRAY_EQUAL (Vector3d(1+cos(1), sin(1), 3).data(), refPtBaseCoord.data(), 3); + + CalcPointJacobian(*model, q, roller, refPt, GrefPt); + + Gexpected(0,0) = 1 - sin(1); + Gexpected(1,0) = cos(1); + Gexpected(2,0) = 0; + + CHECK_ARRAY_EQUAL (Gexpected.data(), GrefPt.data(), 3); +} diff --git a/3rdparty/rbdl/tests/SparseFactorizationTests.cc b/3rdparty/rbdl/tests/SparseFactorizationTests.cc new file mode 100644 index 0000000..ec50a17 --- /dev/null +++ b/3rdparty/rbdl/tests/SparseFactorizationTests.cc @@ -0,0 +1,268 @@ +#include + +#include + +#include "Fixtures.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +TEST_FIXTURE (FloatingBase12DoF, TestSparseFactorizationLTL) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + MatrixNd LTL = L.transpose() * L; + + CHECK_ARRAY_CLOSE (H.data(), LTL.data(), model->qdot_size * model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLx) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + VectorNd x = L * Q; + + SparseSolveLx (*model, L, x); + + CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLTx) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + VectorNd x = L.transpose() * Q; + + SparseSolveLTx (*model, L, x); + + CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsSparse) { + ConstraintSet constraint_set_var1; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_var1 = constraint_set.Copy(); + constraint_set_var1.Bind (*model); + constraint_set.Bind (*model); + + VectorNd QDDot_var1 = VectorNd::Constant (model->dof_count, 0.); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); + + ClearLogOutput(); + ForwardDynamicsConstraintsRangeSpaceSparse (*model, Q, QDot, Tau, constraint_set_var1, QDDot_var1); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_var1.data(), QDDot.size(), TEST_PREC); +} + +TEST ( TestSparseFactorizationMultiDof) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + Joint joint_rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_emulated = model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + model_emulated.AddBody (multdof_body_id_emulated, SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_3dof = model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + model_3dof.AddBody (multdof_body_id_3dof, SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + VectorNd b (VectorNd::Zero (q.size())); + VectorNd x_emulated (VectorNd::Zero (q.size())); + VectorNd x_3dof (VectorNd::Zero (q.size())); + + for (unsigned int i = 0; i < b.size(); i++) { + b[i] = static_cast (i + 1) * 2.152; + } + b = H_emulated * b; + + SparseFactorizeLTL (model_emulated, H_emulated); + SparseFactorizeLTL (model_3dof, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); + + x_emulated = b; + SparseSolveLx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); + + x_emulated = b; + SparseSolveLTx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLTx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); +} + +TEST ( TestSparseFactorizationMultiDofAndFixed) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + Joint joint_rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + SpatialTransform translate_x (Matrix3d::Identity(), Vector3d (1., 0., 0.)); + + model_emulated.AppendBody (SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_emulated = model_emulated.AppendBody (translate_x, joint_emulated, body); + model_emulated.AppendBody (translate_x, joint_emulated, body); + model_emulated.AddBody(multdof_body_id_emulated, translate_x, Joint(JointTypeFixed), body); + model_emulated.AppendBody (translate_x, joint_emulated, body); + + model_3dof.AppendBody (SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_3dof = model_3dof.AppendBody (translate_x, joint_3dof, body); + model_3dof.AppendBody (translate_x, joint_3dof, body); + model_3dof.AddBody (multdof_body_id_3dof, translate_x, Joint(JointTypeFixed), body); + model_3dof.AppendBody (translate_x, joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + VectorNd b (VectorNd::Zero (q.size())); + VectorNd x_emulated (VectorNd::Zero (q.size())); + VectorNd x_3dof (VectorNd::Zero (q.size())); + + for (unsigned int i = 0; i < b.size(); i++) { + b[i] = static_cast (i + 1) * 2.152; + } + b = H_emulated * b; + + SparseFactorizeLTL (model_emulated, H_emulated); + SparseFactorizeLTL (model_3dof, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); + + x_emulated = b; + SparseSolveLx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); + + x_emulated = b; + SparseSolveLTx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLTx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); +} diff --git a/3rdparty/rbdl/tests/SpatialAlgebraTests.cc b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc new file mode 100644 index 0000000..6fed310 --- /dev/null +++ b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc @@ -0,0 +1,586 @@ +#include + +#include +#include + +#include "rbdl/Body.h" +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +SpatialMatrix spatial_adjoint(const SpatialMatrix &m) { + SpatialMatrix res (m); + res.block<3,3>(3,0) = m.block<3,3>(0,3); + res.block<3,3>(0,3) = m.block<3,3>(3,0); + return res; +} + +SpatialMatrix spatial_inverse(const SpatialMatrix &m) { + SpatialMatrix res(m); + res.block<3,3>(0,0) = m.block<3,3>(0,0).transpose(); + res.block<3,3>(3,0) = m.block<3,3>(3,0).transpose(); + res.block<3,3>(0,3) = m.block<3,3>(0,3).transpose(); + res.block<3,3>(3,3) = m.block<3,3>(3,3).transpose(); + return res; +} + +Matrix3d get_rotation (const SpatialMatrix &m) { + return m.block<3,3>(0,0); +} + +Vector3d get_translation (const SpatialMatrix &m) { + return Vector3d (-m(4,2), m(3,2), -m(3,1)); +} + +/// \brief Checks the multiplication of a SpatialMatrix with a SpatialVector +TEST(TestSpatialMatrixTimesSpatialVector) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + SpatialVector s_vector ( + 1., 2., 3., 4., 5., 6. + ); + + SpatialVector result; + result = s_matrix * s_vector; + + SpatialVector test_result ( + 43., 44., 45., 34., 35., 40. + ); + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialVector +TEST(TestScalarTimesSpatialVector) { + SpatialVector s_vector ( + 1., 2., 3., 4., 5., 6. + ); + + SpatialVector result; + result = 3. * s_vector; + + SpatialVector test_result(3., 6., 9., 12., 15., 18.); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialMatrix +TEST(TestScalarTimesSpatialMatrix) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + + SpatialMatrix result; + result = 3. * s_matrix; + + SpatialMatrix test_result( + 3., 0., 0., 0., 0., 21., + 0., 6., 0., 0., 24., 0., + 0., 0., 9., 27., 0., 0., + 0., 0., 18., 12., 0., 0., + 0., 15., 0., 0., 15., 0., + 12., 0., 0., 0., 0., 18. + ); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialMatrix +TEST(TestSpatialMatrixTimesSpatialMatrix) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + + SpatialMatrix result; + result = s_matrix * s_matrix; + + SpatialMatrix test_result( + 29., 0., 0., 0., 0., 49., + 0., 44., 0., 0., 56., 0., + 0., 0., 63., 63., 0., 0., + 0., 0., 42., 70., 0., 0., + 0., 35., 0., 0., 65., 0., + 28., 0., 0., 0., 0., 64. + ); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the adjoint method +// +// This method computes a spatial force transformation from a spatial +// motion transformation and vice versa +TEST(TestSpatialMatrixTransformAdjoint) { + SpatialMatrix s_matrix ( + 1., 2., 3., 4., 5., 6., + 7., 8., 9., 10., 11., 12., + 13., 14., 15., 16., 17., 18., + 19., 20., 21., 22., 23., 24., + 25., 26., 27., 28., 29., 30., + 31., 32., 33., 34., 35., 36. + ); + + SpatialMatrix result = spatial_adjoint(s_matrix); + + SpatialMatrix test_result_matrix ( + 1., 2., 3., 19., 20., 21., + 7., 8., 9., 25., 26., 27., + 13., 14., 15., 31., 32., 33., + 4., 5., 6., 22., 23., 24., + 10., 11., 12., 28., 29., 30., + 16., 17., 18., 34., 35., 36.); + + CHECK_EQUAL (test_result_matrix, result); +} + +TEST(TestSpatialMatrixInverse) { + SpatialMatrix s_matrix ( + 0, 1, 2, 0, 1, 2, + 3, 4, 5, 3, 4, 5, + 6, 7, 8, 6, 7, 8, + 0, 1, 2, 0, 1, 2, + 3, 4, 5, 3, 4, 5, + 6, 7, 8, 6, 7, 8 + ); + + SpatialMatrix test_inv ( + 0, 3, 6, 0, 3, 6, + 1, 4, 7, 1, 4, 7, + 2, 5, 8, 2, 5, 8, + 0, 3, 6, 0, 3, 6, + 1, 4, 7, 1, 4, 7, + 2, 5, 8, 2, 5, 8 + ); + + CHECK_EQUAL (test_inv, spatial_inverse(s_matrix)); +} + +TEST(TestSpatialMatrixGetRotation) { + SpatialMatrix spatial_transform ( + 1., 2., 3., 0., 0., 0., + 4., 5., 6., 0., 0., 0., + 7., 8., 9., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0. + ); + + // Matrix3d rotation = spatial_transform.block<3,3>(0,0); + Matrix3d rotation = get_rotation (spatial_transform); + Matrix3d test_result ( + 1., 2., 3., + 4., 5., 6., + 7., 8., 9. + ); + + CHECK_EQUAL(test_result, rotation); +} + +TEST(TestSpatialMatrixGetTranslation) { + SpatialMatrix spatial_transform ( + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., -3., 2., 0., 0., 0., + 0., 0., -1., 0., 0., 0., + 0., 0., 0., 0., 0., 0. + ); + + Vector3d translation = get_translation(spatial_transform); + Vector3d test_result ( + 1., 2., 3. + ); + + CHECK_EQUAL( test_result, translation); +} + +TEST(TestSpatialVectorCross) { + SpatialVector s_vec (1., 2., 3., 4., 5., 6.); + + SpatialMatrix test_cross ( + 0., -3., 2., 0., 0., 0., + 3., 0., -1., 0., 0., 0., + -2., 1., 0., 0., 0., 0., + 0., -6., 5., 0., -3., 2., + 6., 0., -4., 3., 0., -1., + -5., 4., 0., -2., 1., 0. + ); + + SpatialMatrix s_vec_cross (crossm(s_vec)); + CHECK_EQUAL (test_cross, s_vec_cross); + + SpatialMatrix s_vec_crossf (crossf(s_vec)); + SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose(); + + CHECK_EQUAL (test_crossf, s_vec_crossf); +} + +TEST(TestSpatialVectorCrossmCrossf) { + SpatialVector s_vec (1., 2., 3., 4., 5., 6.); + SpatialVector t_vec (9., 8., 7., 6., 5., 4.); + + // by explicitly building the matrices (crossm/f with only one vector) + SpatialVector crossm_s_x_t = crossm(s_vec) * t_vec; + SpatialVector crossf_s_x_t = crossf(s_vec) * t_vec; + + // by using direct computation that avoids building of the matrix + SpatialVector crossm_s_t = crossm(s_vec, t_vec); + SpatialVector crossf_s_t = crossf(s_vec, t_vec); + + /* + cout << crossm_s_x_t << endl; + cout << "---" << endl; + cout << crossf_s_x_t << endl; + cout << "---" << endl; + cout << crossf_s_t << endl; + */ + + CHECK_EQUAL (crossm_s_x_t, crossm_s_t); + CHECK_EQUAL (crossf_s_x_t, crossf_s_t); +} + +TEST(TestSpatialTransformApply) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialTransform X_st; + X_st.r = trans; + + SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6)); + X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_st.E = X_66_matrix.block<3,3>(0,0); + + // cout << X_66_matrix << endl; + // cout << X_st.E << endl; + // cout << X_st.r.transpose() << endl; + + SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1); + SpatialVector v_66_res = X_66_matrix * v; + SpatialVector v_st_res = X_st.apply(v); + + // cout << (v_66_res - v_st_res).transpose() << endl; + + CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformApplyTranspose) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialTransform X_st; + X_st.r = trans; + + SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6)); + X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_st.E = X_66_matrix.block<3,3>(0,0); + + // cout << X_66_matrix << endl; + // cout << X_st.E << endl; + // cout << X_st.r.transpose() << endl; + + SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1); + SpatialVector v_66_res = X_66_matrix.transpose() * v; + SpatialVector v_st_res = X_st.applyTranspose(v); + + // cout << (v_66_res - v_st_res).transpose() << endl; + + CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformApplyAdjoint) { + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialMatrix X_adjoint = X.toMatrixAdjoint(); + + SpatialVector f (1.1, 2.1, 4.1, 9.2, 3.3, 0.8); + SpatialVector f_apply = X.applyAdjoint(f); + SpatialVector f_matrix = X_adjoint * f; + + CHECK_ARRAY_CLOSE (f_matrix.data(), f_apply.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrix) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // SpatialMatrix X_diff = X_st.toMatrix() - X_matrix; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix.data(), X_st.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrixAdjoint) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // SpatialMatrix X_diff = X_st.toMatrixAdjoint() - spatial_adjoint(X_matrix); + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (spatial_adjoint(X_matrix).data(), X_st.toMatrixAdjoint().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrixTranspose) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // we have to copy the matrix as it is only transposed via a flag and + // thus data() does not return the proper data. + SpatialMatrix X_matrix_transposed = X_matrix.transpose(); + // SpatialMatrix X_diff = X_st.toMatrixTranspose() - X_matrix_transposed; + // cout << "Error: " << endl << X_diff << endl; + // cout << "X_st: " << endl << X_st.toMatrixTranspose() << endl; + // cout << "X: " << endl << X_matrix_transposed() << endl; + + CHECK_ARRAY_CLOSE (X_matrix_transposed.data(), X_st.toMatrixTranspose().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformMultiply) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6)); + SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6)); + + X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st_1; + SpatialTransform X_st_2; + + X_st_1.E = X_matrix_1.block<3,3>(0,0); + X_st_1.r = trans; + X_st_2.E = X_matrix_2.block<3,3>(0,0); + X_st_2.r = trans; + + SpatialTransform X_st_res = X_st_1 * X_st_2; + SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2; + + // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformMultiplyEqual) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6)); + SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6)); + + X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st_1; + SpatialTransform X_st_2; + + X_st_1.E = X_matrix_1.block<3,3>(0,0); + X_st_1.r = trans; + X_st_2.E = X_matrix_2.block<3,3>(0,0); + X_st_2.r = trans; + + SpatialTransform X_st_res = X_st_1; + X_st_res *= X_st_2; + SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2; + + // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestXrotAxis) { + SpatialTransform X_rotX = Xrotx (M_PI * 0.15); + SpatialTransform X_rotX_axis = Xrot (M_PI * 0.15, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 36, TEST_PREC); + + // all the other axes + SpatialTransform X_rotX_90 = Xrotx (M_PI * 0.5); + SpatialTransform X_rotX_90_axis = Xrot (M_PI * 0.5, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 36, TEST_PREC); + + SpatialTransform X_rotY_90 = Xroty (M_PI * 0.5); + SpatialTransform X_rotY_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 36, TEST_PREC); + + SpatialTransform X_rotZ_90 = Xrotz (M_PI * 0.5); + SpatialTransform X_rotZ_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 0., 1.)); + + CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialMatrix rbi_matrix_added = rbi.toMatrix() + rbi.toMatrix(); + SpatialRigidBodyInertia rbi_added = rbi + rbi; + + // cout << "rbi = " << endl << rbi.toMatrix() << endl; + // cout << "rbi_added = " << endl << rbi_added.toMatrix() << endl; + // cout << "rbi_matrix_added = " << endl << rbi_matrix_added << endl; + // cout << "diff = " << endl << + // rbi_added.toMatrix() - rbi_matrix_added << endl; + + CHECK_ARRAY_CLOSE ( + rbi_matrix_added.data(), + rbi_added.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialRigidBodyInertia rbi_transformed = X.apply (rbi); + SpatialMatrix rbi_matrix_transformed = X.toMatrixAdjoint () * rbi.toMatrix() * X.inverse().toMatrix(); + + CHECK_ARRAY_CLOSE ( + rbi_matrix_transformed.data(), + rbi_transformed.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialRigidBodyInertia rbi_transformed = X.applyTranspose (rbi); + SpatialMatrix rbi_matrix_transformed = X.toMatrixTranspose() * rbi.toMatrix() * X.toMatrix(); + + CHECK_ARRAY_CLOSE ( + rbi_matrix_transformed.data(), + rbi_transformed.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) { + double mass = 1.1; + Vector3d com (0., 0., 0.); + Matrix3d inertia ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + ); + SpatialRigidBodyInertia body_rbi(mass, com , inertia); + + SpatialMatrix spatial_inertia = body_rbi.toMatrix(); + + SpatialRigidBodyInertia rbi; + rbi.createFromMatrix (spatial_inertia); + + CHECK_EQUAL (mass, rbi.m); + CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3); + Matrix3d rbi_I_matrix ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ); + CHECK_ARRAY_EQUAL (inertia.data(), rbi_I_matrix.data(), 9); +} + +#ifdef USE_SLOW_SPATIAL_ALGEBRA +TEST(TestSpatialLinSolve) { + SpatialVector b (1, 2, 0, 1, 1, 1); + SpatialMatrix A ( + 1., 2., 3., 0., 0., 0., + 3., 4., 5., 0., 0., 0., + 6., 7., 7., 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., 0., 0., 1. + ); + + SpatialVector x = SpatialLinSolve (A, b); + SpatialVector x_test (3.5, -6.5, 3.5, 1, 1, 1); + + CHECK_ARRAY_CLOSE (x_test.data(), x.data(), 6, TEST_PREC); +} +#endif diff --git a/3rdparty/rbdl/tests/TwolegModelTests.cc b/3rdparty/rbdl/tests/TwolegModelTests.cc new file mode 100644 index 0000000..ca5c379 --- /dev/null +++ b/3rdparty/rbdl/tests/TwolegModelTests.cc @@ -0,0 +1,404 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-13; + +unsigned int hip_id, + upper_leg_right_id, + lower_leg_right_id, + foot_right_id, + upper_leg_left_id, + lower_leg_left_id, + foot_left_id; + +Body hip_body, + upper_leg_right_body, + lower_leg_right_body, + foot_right_body, + upper_leg_left_body, + lower_leg_left_body, + foot_left_body; + +Joint joint_txtyrz ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); +Joint joint_rot_z (SpatialVector (0., 0., 1., 0., 0., 0.)); + +VectorNd Q; +VectorNd QDot; +VectorNd QDDot; +VectorNd Tau; + +ConstraintSet constraint_set_right; +ConstraintSet constraint_set_left; +ConstraintSet constraint_set_left_flat; +ConstraintSet constraint_set_both; + +enum ParamNames { + ParamSteplength = 0, + ParamNameLast +}; + +enum PosNames { + PosHipPosX, + PosHipPosY, + PosHipRotZ, + PosRightThighRotZ, + PosRightShankRotZ, + PosRightAnkleRotZ, + PosLeftThighRotZ, + PosLeftShankRotZ, + PosLeftAnkleRotZ, + PosNameLast +}; + +enum StateNames { + StateHipPosX, + StateHipPosY, + StateHipRotZ, + StateRightThighRotZ, + StateRightShankRotZ, + StateRightAnkleRotZ, + StateLeftThighRotZ, + StateLeftShankRotZ, + StateLeftAnkleRotZ, + StateHipVelX, + StateHipVelY, + StateHipRotVelZ, + StateRightThighRotVelZ, + StateRightShankRotVelZ, + StateRightAnkleRotVelZ, + StateLeftThighRotVelZ, + StateLeftShankRotVelZ, + StateLeftAnkleRotVelZ, + StateNameLast +}; + +enum ControlNames { + ControlRightThighRotZ, + ControlRightKneeRotZ, + ControlRightAnkleRotZ, + ControlLeftThighRotZ, + ControlLeftKneeRotZ, + ControlLeftAnkleRotZ, + ControlNameLast +}; + +enum SegmentLengthsNames { + SegmentLengthsHip = 0, + SegmentLengthsThigh, + SegmentLengthsShank, + SegmentLengthsFootHeight, + SegmentLengthsFoot, + SegmentLengthsNameLast +}; + +const double ModelMass = 73.; +const double ModelHeight = 1.741; + +// absolute lengths! +double segment_lengths[SegmentLengthsNameLast] = { + 0.4346, + 0.4222, + 0.4340, + 0.0317, + 0.2581 +}; + +enum JointLocations { + JointLocationHip = 0, + JointLocationKnee, + JointLocationAnkle, + JointLocationLast +}; + +Vector3d joint_location[JointLocationLast] = { + Vector3d (0., 0., 0.), + Vector3d (0., - 0.2425 * ModelHeight, 0.), + Vector3d (0., - 0.2529 * ModelHeight, 0.) +}; + +enum SegmentMassNames { + SegmentMassHip, + SegmentMassThigh, + SegmentMassShank, + SegmentMassFoot, + SegmentMassLast +}; + +double segment_mass[SegmentMassLast] = { + 0.4346 * ModelMass, + 0.1416 * ModelMass, + 0.0433 * ModelMass, + 0.0137 * ModelMass +}; + +enum COMNames { + COMHip, + COMThigh, + COMShank, + COMFoot, + COMNameLast +}; + +Vector3d com_position[COMNameLast] = { + Vector3d (0., 0.3469 * ModelHeight, 0.), + Vector3d (0., 0.2425 * ModelHeight, 0.), + Vector3d (0., 0.2529 * ModelHeight, 0.), + Vector3d (0.0182 * ModelHeight, 0., 0.) +}; + +enum RGyrationNames { + RGyrationHip, + RGyrationThigh, + RGyrationShank, + RGyrationFoot, + RGyrationLast +}; + +Vector3d rgyration[RGyrationLast] = { + Vector3d (0.1981, 0.1021, 0.1848), + Vector3d (0.1389, 0.0629, 0.1389), + Vector3d (0.1123, 0.0454, 0.1096), + Vector3d (0.0081, 0.0039, 0.0078) +}; + +Vector3d heel_point (0., 0., 0.); +Vector3d medial_point (0., 0., 0.); + +void init_model (Model* model) { + assert (model); + + constraint_set_right = ConstraintSet(); + constraint_set_left = ConstraintSet(); + constraint_set_left_flat = ConstraintSet(); + constraint_set_both = ConstraintSet(); + + model->gravity = Vector3d (0., -9.81, 0.); + + // hip + hip_body = Body (segment_mass[SegmentMassHip], com_position[COMHip], rgyration[RGyrationHip]); + + // lateral right + upper_leg_right_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]); + lower_leg_right_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]); + foot_right_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]); + + // lateral left + upper_leg_left_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]); + lower_leg_left_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]); + foot_left_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]); + + // add hip to the model (planar, 3 DOF) + hip_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_txtyrz, hip_body); + + // + // right leg + // + + unsigned int temp_id = 0; + + // add right upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_right_body); + upper_leg_right_id = temp_id; + + // add the right lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_right_body); + lower_leg_right_id = temp_id; + + // add the right foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_right_body); + foot_right_id = temp_id; + + // + // left leg + // + + // add left upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_left_body); + upper_leg_left_id = temp_id; + + // add the left lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_left_body); + lower_leg_left_id = temp_id; + + // add the left foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_left_body); + foot_left_id = temp_id; + + // cerr << "--- model created (" << model->dof_count << " DOF) ---" << endl; + + // contact data + + // the contact points for heel and toe + heel_point.set (-0.05, -0.0317, 0.); + medial_point.set (-0.05, -0.0317 + segment_lengths[SegmentLengthsFoot], 0.); + + constraint_set_right.AddContactConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x"); + constraint_set_right.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y"); + + constraint_set_left.AddContactConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x"); + constraint_set_left.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y"); + + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x"); + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y"); + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 0., 1.), "right_heel_z"); + + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x"); + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y"); + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 0., 1.), "left_heel_z"); + + constraint_set_right.Bind (*model); + constraint_set_left.Bind (*model); + constraint_set_both.Bind (*model); +} + +template +void copy_values (T *dest, const T *src, size_t count) { + memcpy (dest, src, count * sizeof (T)); +} + +TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) { + Model* model = new Model; + + init_model(model); + + Q.resize(model->dof_count); + QDot.resize(model->dof_count); + QDDot.resize(model->dof_count); + Tau.resize(model->dof_count); + + Q[0] = -0.2; + Q[1] = 0.9; + Q[2] = 0; + Q[3] = -0.15; + Q[4] = -0.15; + Q[5] = 0.1; + Q[6] = 0.15; + Q[7] = -0.15; + Q[8] = 0; + + QDot.setZero(); + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 1; + Tau[4] = 1; + Tau[5] = 1; + Tau[6] = 1; + Tau[7] = 1; + Tau[8] = 1; + + Vector3d contact_accel_left; + Vector3d contact_vel_left; + Vector3d contact_force = Vector3d::Zero(); + + VectorNd QDDot_aba (QDDot); + VectorNd QDDot_lag (QDDot); + ForwardDynamics (*model, Q, QDot, Tau, QDDot_aba); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot_lag); + + // cout << "QDDot_aba = " << QDDot_aba.transpose() << endl; + // cout << "QDDot_lag = " << QDDot_lag.transpose() << endl; + + unsigned int body_id = constraint_set_left.body[0]; + Vector3d contact_point = constraint_set_left.point[0]; + + MatrixNd G (3, Q.size()); + CalcPointJacobian (*model, Q, body_id, contact_point, G, true); + + // cout << G << endl; + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_left, QDDot); + + // cout << "C0: " << contact_data_left[0].body_id << ", " << contact_data_left[0].point.transpose() << endl; + // cout << "C1: " << contact_data_left[1].body_id << ", " << contact_data_left[1].point.transpose() << endl; + // cout << "td: " << foot_left_id << ", " << heel_point.transpose() << endl; + + contact_force[0] = constraint_set_left.force[0]; + contact_force[1] = constraint_set_left.force[1]; + + CHECK_EQUAL (body_id, foot_left_id); + CHECK_EQUAL (contact_point, heel_point); + + // cout << LogOutput.str() << endl; + contact_accel_left = CalcPointAcceleration (*model, Q, QDot, QDDot, foot_left_id, heel_point); + contact_vel_left = CalcPointVelocity (*model, Q, QDot, foot_left_id, heel_point); + // cout << contact_force << endl; + // cout << contact_accel_left << endl; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), contact_accel_left.data(), 3, TEST_PREC); + + delete model; +} + +TEST ( TestClearContactsInertiaMatrix ) { + Model* model = new Model; + + init_model(model); + + Q.resize(model->dof_count); + QDot.resize(model->dof_count); + QDDot.resize(model->dof_count); + Tau.resize(model->dof_count); + + Q[0] = -0.2; + Q[1] = 0.9; + Q[2] = 0; + Q[3] = -0.15; + Q[4] = -0.15; + Q[5] = 0.1; + Q[6] = 0.15; + Q[7] = -0.15; + Q[8] = 0; + + QDot.setZero(); + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 1; + Tau[4] = 1; + Tau[5] = 1; + Tau[6] = 1; + Tau[7] = 1; + Tau[8] = 1; + + VectorNd QDDot_aba (QDDot); + VectorNd QDDot_lag (QDDot); + + // initialize matrix with erroneous values + constraint_set_right.bound = false; + constraint_set_right.H = MatrixNd::Zero (model->dof_count, model->dof_count); + for (unsigned int i = 0; i < model->dof_count; i++) { + for (unsigned int j = 0; j < model->dof_count; j++) { + constraint_set_right.H(i,j) = 1.234; + } + } + + constraint_set_right.Bind (*model); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_right, QDDot_lag); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set_right, QDDot_aba); + + CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), QDDot.size(), TEST_PREC * QDDot_lag.norm()); + + delete model; +} diff --git a/3rdparty/rbdl/tests/UtilsTests.cc b/3rdparty/rbdl/tests/UtilsTests.cc new file mode 100644 index 0000000..4009d8c --- /dev/null +++ b/3rdparty/rbdl/tests/UtilsTests.cc @@ -0,0 +1,156 @@ +#include + +#include + +#include "Fixtures.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) { + VectorNd q = VectorNd::Zero(model->q_size); + VectorNd qdot = VectorNd::Zero(model->q_size); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.1 * i; + qdot[i] = 0.3 * i; + } + + MatrixNd H = MatrixNd::Zero (model->q_size, model->q_size); + CompositeRigidBodyAlgorithm (*model, q, H, true); + + double kinetic_energy_ref = 0.5 * qdot.transpose() * H * qdot; + double kinetic_energy = Utils::CalcKineticEnergy (*model, q, qdot); + + CHECK_EQUAL (kinetic_energy_ref, kinetic_energy); +} + +TEST(TestPotentialEnergy) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + Body body (0.5, Vector3d (0., 0., 0.), inertia); + Joint joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + + model.AppendBody (Xtrans (Vector3d::Zero()), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + double potential_energy_zero = Utils::CalcPotentialEnergy (model, q); + CHECK_EQUAL (0., potential_energy_zero); + + q[1] = 1.; + double potential_energy_lifted = Utils::CalcPotentialEnergy (model, q); + CHECK_EQUAL (4.905, potential_energy_lifted); +} + +TEST(TestCOMSimple) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + Body body (123., Vector3d (0., 0., 0.), inertia); + Joint joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + + model.AppendBody (Xtrans (Vector3d::Zero()), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + VectorNd qdot = VectorNd::Zero(model.qdot_size); + + double mass; + Vector3d com; + Vector3d com_velocity; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, &com_velocity); + + CHECK_EQUAL (123., mass); + CHECK_EQUAL (Vector3d (0., 0., 0.), com); + CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + + q[1] = 1.; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, &com_velocity); + CHECK_EQUAL (Vector3d (0., 1., 0.), com); + CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + + qdot[1] = 1.; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, &com_velocity); + CHECK_EQUAL (Vector3d (0., 1., 0.), com); + CHECK_EQUAL (Vector3d (0., 1., 0.), com_velocity); +} + +TEST(TestAngularMomentumSimple) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + inertia(0,0) = 1.1; + inertia(1,1) = 2.2; + inertia(2,2) = 3.3; + + Body body (0.5, Vector3d (1., 0., 0.), inertia); + Joint joint ( + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + model.AppendBody (Xtrans (Vector3d(0., 0., 0.)), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + VectorNd qdot = VectorNd::Zero(model.qdot_size); + + double mass; + Vector3d com; + Vector3d angular_momentum; + + qdot << 1., 0., 0.; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (1.1, 0., 0.), angular_momentum); + + qdot << 0., 1., 0.; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (0., 2.2, 0.), angular_momentum); + + qdot << 0., 0., 1.; + Utils::CalcCenterOfMass (model, q, qdot, mass, com, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (0., 0., 3.3), angular_momentum); +} + +TEST_FIXTURE (TwoArms12DoF, TestAngularMomentumSimple) { + double mass; + Vector3d com; + Vector3d angular_momentum; + + Utils::CalcCenterOfMass (*model, q, qdot, mass, com, NULL, &angular_momentum); + + CHECK_EQUAL (Vector3d (0., 0., 0.), angular_momentum); + + qdot[0] = 1.; + qdot[1] = 2.; + qdot[2] = 3.; + + Utils::CalcCenterOfMass (*model, q, qdot, mass, com, NULL, &angular_momentum); + + // only a rough guess from test calculation + CHECK_ARRAY_CLOSE (Vector3d (3.3, 2.54, 1.5).data(), angular_momentum.data(), 3, 1.0e-1); + + qdot[3] = -qdot[0]; + qdot[4] = -qdot[1]; + qdot[5] = -qdot[2]; + + ClearLogOutput(); + Utils::CalcCenterOfMass (*model, q, qdot, mass, com, NULL, &angular_momentum); + + CHECK (angular_momentum[0] == 0); + CHECK (angular_momentum[1] < 0); + CHECK (angular_momentum[2] == 0.); +} diff --git a/3rdparty/rbdl/tests/main.cc b/3rdparty/rbdl/tests/main.cc new file mode 100644 index 0000000..4608c22 --- /dev/null +++ b/3rdparty/rbdl/tests/main.cc @@ -0,0 +1,19 @@ +#include +#include +#include + +#include + +int main (int argc, char *argv[]) +{ + rbdl_check_api_version (RBDL_API_VERSION); + + if (argc > 1) { + std::string arg (argv[1]); + + if (arg == "-v" || arg == "--version") + rbdl_print_version(); + } + + return UnitTest::RunAllTests (); +} diff --git a/3rdparty/rbdl/utils/matlab/FrameTranslation.m b/3rdparty/rbdl/utils/matlab/FrameTranslation.m new file mode 100755 index 0000000..c95b7cf --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/FrameTranslation.m @@ -0,0 +1,30 @@ +function [F] = FrameTranslation (translation, euler_angles) + +% Here we store the result +Result = zeros (4,4); +Result(4,4) = 1; + +% Set the translation part: +Result (1:3, 4) = -translation; + +% Calculate the rotations +Rotation = eye (3,3); + +% Z Rotation +RotZ = eye (3,3); + +if (euler_angles(1) != 0.) + s = sin (euler_angles(1)); + c = cos (euler_angles(1)); + RotZ = [c, s, 0; + -s, c, 0; + 0, 0, 1]; +end + +Rotation = RotZ; + +Result (1:3, 1:3) = Rotation; + +F = Result; + +end; diff --git a/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m b/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m new file mode 100644 index 0000000..399c8c6 --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m @@ -0,0 +1,5 @@ +function R = VectorCrossMatrix (r) + +R = [0, -r(3), r(2); + r(3), 0, -r(1); + -r(2), r(1), 0]; diff --git a/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m b/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m new file mode 100644 index 0000000..b7870fd --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m @@ -0,0 +1,56 @@ +function [Rx, Ry, Rz, M] = ZYXEulerToMatrix (e) +% +% Calculates the rotation matrix from ZYX Euler Angles +% + +z = e(1); +y = e(2); +x = e(3); + +sz = sin (z); +cz = cos (z); +sy = sin (y); +cy = cos (y); +sx = sin (x); +cx = cos (x); + +Rx = [ +1, 0, 0; +0, cx, -sx; +0, sx, cx; +]; + +Ry = [ + cy, 0, sy; + 0, 1, 0; +-sy, 0, cy +]; + +Rz = [ +cz, -sz, 0; +sz, cz, 0; + 0, 0, 1; +]; + +% Rx * Ry * Rz +M = [ + cy * cz, - cy * sz, +sy; +sz*cx + sx*sy*cz, cz*cx - sx*sy*sz, -sx*cy; +sz*sx - cx*sy*cz, cz*sx + cx*sy*sz, cx*cy; +]; + +% Rz' * Ry' * Rx' +M = [ + cz * cy, -cz*sy*sx + sz* cx, cz*sy*cx + sz*sx; +-sz*cy, - sz*sy*sx + cz*cx, sz*sy*cx + cz*sx; +sy, -cy*sx, cy*cx; +]; + +% Rx' * Ry' * Rz' +M = [ +cy*cz, cy*sz, -sy; +-cx*sz+sx*sy*cz, cx*cz+sx*sy*sz, sx*cy; +sx*sz+cx*sy*cz, -sx*cz+cx*sy*sz, cx*cy; +]; + +M = Rz * Ry * Rx; diff --git a/CMakeLists.txt b/CMakeLists.txt index 2179626..bf8ce26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,9 @@ SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) +# For YouCompleteMe +SET (CMAKE_EXPORT_COMPILE_COMMANDS ON) + # Enable proper C++11 flags include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) @@ -26,6 +29,10 @@ endif() # enable strings in GDB set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_GLIBCXX_DEBUG") +# enable LuaModel addon for RBDL +SET (RBDL_BUILD_ADDON_LUAMODEL ON) +SET (RBDL_USE_SIMPLE_MATH ON) + INCLUDE_DIRECTORIES ( ${QT_INCLUDE_DIR}/QtOpenGL ${CMAKE_CURRENT_BINARY_DIR} @@ -35,6 +42,10 @@ INCLUDE_DIRECTORIES ( # Required to compile RCPP related code ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/bgfx + + # RBDL and its LuaModel addon + ${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/ + ${CMAKE_CURRENT_BINARY_DIR}/3rdparty/rbdl/include ) FIND_PACKAGE (X11 REQUIRED) @@ -62,6 +73,7 @@ SUBDIRS ( 3rdparty/glfw 3rdparty/bgfx + 3rdparty/rbdl 3rdparty/googletest ) @@ -129,11 +141,12 @@ SET (glfw_dependencies ) TARGET_LINK_LIBRARIES ( protot - #glew glfw ${glfw_dependencies} ${OPENGL_LIBRARIES} bgfx bgfx_aux glsl-optimizer + rbdl + rbdl_luamodel ) diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt index 2e43af3..f930e6b 100644 --- a/src/modules/CMakeLists.txt +++ b/src/modules/CMakeLists.txt @@ -1,5 +1,8 @@ INCLUDE_DIRECTORIES ( ${CMAKE_CURRENT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/3rdparty/rbdl/include + ${PROJECT_SOURCE_DIR}/3rdparty/rbdl/ + ${PROJECT_BINARY_DIR}/3rdparty/rbdl/include ) ADD_LIBRARY (RenderModule SHARED diff --git a/src/modules/CharacterModule.cc b/src/modules/CharacterModule.cc index adda342..58f89b5 100644 --- a/src/modules/CharacterModule.cc +++ b/src/modules/CharacterModule.cc @@ -16,6 +16,7 @@ #include "CharacterModule.h" using namespace std; +using namespace RigidBodyDynamics; const float cJumpVelocity = 4.0f; const float cVelocityDamping = 4.0f; @@ -26,9 +27,11 @@ const float cCharacterWidth = 1.f; CharacterEntity::CharacterEntity() { entity = gRenderer->createEntity(); - position = Vector3f (0.f, 0.0f, 0.0f); - cout << "Creating render entity ... success!" << endl; + mPosition = Vector3f (0.f, 0.0f, 0.0f); + mRigModel = new Model(); + + cout << "Creating render entity ... success!" << endl; cout << "Creating render entity mesh ..." << endl; // Mesh* base_mesh = Mesh::sCreateUVSphere(45, 45, 0.9); @@ -69,54 +72,56 @@ CharacterEntity::CharacterEntity() { Mesh::sCreateUVSphere (45, 45, 0.5) ); - // state->character->entity->mesh = bgfxutils::createCuboid (1.f, 1.f, 1.f); - // state->character->entity->mesh = bgfxutils::createCylinder (20); + // mState->character->entity->mesh = bgfxutils::createCuboid (1.f, 1.f, 1.f); + // mState->character->entity->mesh = bgfxutils::createCylinder (20); cout << "Creating render entity mesh ... success!" << endl; } CharacterEntity::~CharacterEntity() { gRenderer->destroyEntity(entity); entity = nullptr; + delete mRigModel; + mRigModel = nullptr; } static float cur_time = 0.0f; void CharacterEntity::update(float dt) { - Vector3f controller_acceleration ( - controller.direction[0] * cGroundAcceleration, - controller.direction[1] * cGroundAcceleration, - controller.direction[2] * cGroundAcceleration + Vector3f mController_acceleration ( + mController.mDirection[0] * cGroundAcceleration, + mController.mDirection[1] * cGroundAcceleration, + mController.mDirection[2] * cGroundAcceleration ); Vector3f gravity (0.0f, -cGravity, 0.0f); Vector3f damping ( - -velocity[0] * cVelocityDamping, + -mVelocity[0] * cVelocityDamping, 0.0f, - -velocity[2] * cVelocityDamping + -mVelocity[2] * cVelocityDamping ); - Vector3f acceleration = controller_acceleration + gravity + damping; + Vector3f acceleration = mController_acceleration + gravity + damping; - velocity = velocity + acceleration * dt; + mVelocity = mVelocity + acceleration * dt; - if (position[1] == 0.0f - && controller.state[CharacterController::ControlStateJump]) { - velocity[1] = cJumpVelocity; + if (mPosition[1] == 0.0f + && mController.mState[CharacterController::ControlStateJump]) { + mVelocity[1] = cJumpVelocity; } - // integrate position - position += velocity * dt; + // integrate mPosition + mPosition += mVelocity * dt; - if (position[1] < 0.f) { - position[1] = 0.f; - velocity[1] = 0.0f; + if (mPosition[1] < 0.f) { + mPosition[1] = 0.f; + mVelocity[1] = 0.0f; } // apply transformation entity->transform.translation.set( - position[0], - position[1], - position[2]); + mPosition[0], + mPosition[1], + mPosition[2]); gRenderer->drawDebugSphere (Vector3f (0.f, 1.3 + sin(cur_time * 2.f), 0.f), 2.2f); @@ -142,8 +147,8 @@ void ShowCharacterPropertiesWindow (CharacterEntity* character) { character->reset(); } - ImGui::DragFloat3 ("Position", character->position.data(), 0.01, -10.0f, 10.0f); - ImGui::DragFloat3 ("Velocity", character->velocity.data(), 0.01, -10.0f, 10.0f); + ImGui::DragFloat3 ("Position", character->mPosition.data(), 0.01, -10.0f, 10.0f); + ImGui::DragFloat3 ("Velocity", character->mVelocity.data(), 0.01, -10.0f, 10.0f); for (int i = 0; i < character->entity->mesh.meshes.size(); ++i) { @@ -188,7 +193,7 @@ template static void module_serialize ( struct module_state *state, Serializer* serializer) { -// SerializeVec3(*serializer, "protot.TestModule.entity.position", state->character->position); +// SerializeVec3(*serializer, "protot.TestModule.entity.mPosition", state->character->mPosition); } static void module_finalize(struct module_state *state) { diff --git a/src/modules/CharacterModule.h b/src/modules/CharacterModule.h index e861a82..a578615 100644 --- a/src/modules/CharacterModule.h +++ b/src/modules/CharacterModule.h @@ -10,6 +10,12 @@ #include "Globals.h" #include "imgui_protot_ext.h" +#include "rbdl/rbdl.h" +#include "rbdl/addons/luamodel/luamodel.h" + +namespace RigidBodyDynamics { + struct Model; +} struct CharacterController { enum ControllerState { @@ -17,16 +23,16 @@ struct CharacterController { ControlStateLast }; - bool state[ControlStateLast]; + bool mState[ControlStateLast]; - Vector3f direction = Vector3f::Zero(); + Vector3f mDirection = Vector3f::Zero(); void reset() { for (int i = 0; i < ControlStateLast; i++) { - state[i] = false; + mState[i] = false; } - direction.setZero(); + mDirection.setZero(); } CharacterController() { @@ -37,17 +43,19 @@ struct CharacterController { struct CharacterEntity { /// Render entity Entity *entity = nullptr; - Vector3f position; - Vector3f velocity; - CharacterController controller; + Vector3f mPosition; + Vector3f mVelocity; + CharacterController mController; + + RigidBodyDynamics::Model* mRigModel = nullptr; CharacterEntity (); ~CharacterEntity (); void reset() { - position.setZero(); - velocity.setZero(); - controller.reset(); + mPosition.setZero(); + mVelocity.setZero(); + mController.reset(); } void update(float dt); diff --git a/src/modules/TestModule.cc b/src/modules/TestModule.cc index 265a531..3a120af 100644 --- a/src/modules/TestModule.cc +++ b/src/modules/TestModule.cc @@ -132,7 +132,7 @@ void handle_keyboard (struct module_state *state, float dt) { active_camera->poi = poi; } else if (state->character != nullptr) { // Movement of the character - CharacterController& controller = state->character->controller; + CharacterController& controller = state->character->mController; controller.reset(); @@ -140,23 +140,23 @@ void handle_keyboard (struct module_state *state, float dt) { // Reset the character control state: if (glfwGetKey(gWindow, GLFW_KEY_W) == GLFW_PRESS) { - controller.direction += forward_plane; + controller.mDirection += forward_plane; } if (glfwGetKey(gWindow, GLFW_KEY_S) == GLFW_PRESS) { - controller.direction -= forward_plane; + controller.mDirection -= forward_plane; } if (glfwGetKey(gWindow, GLFW_KEY_D) == GLFW_PRESS) { - controller.direction += right; + controller.mDirection += right; } if (glfwGetKey(gWindow, GLFW_KEY_A) == GLFW_PRESS) { - controller.direction -= right; + controller.mDirection -= right; } if (glfwGetKey(gWindow, GLFW_KEY_SPACE) == GLFW_PRESS) { - controller.state[CharacterController::ControlStateJump] = true; + controller.mState[CharacterController::ControlStateJump] = true; } } @@ -186,8 +186,8 @@ template static void module_serialize ( struct module_state *state, Serializer* serializer) { - SerializeVec3(*serializer, "protot.TestModule.entity.position", state->character->position); - SerializeVec3(*serializer, "protot.TestModule.entity.velocity", state->character->velocity); + SerializeVec3(*serializer, "protot.TestModule.entity.mPosition", state->character->mPosition); + SerializeVec3(*serializer, "protot.TestModule.entity.mVelocity", state->character->mVelocity); SerializeBool(*serializer, "protot.TestModule.character_window.visible", state->character_properties_window_visible); SerializeBool(*serializer, "protot.TestModule.modules_window.visible", state->modules_window_visible); SerializeBool(*serializer, "protot.TestModule.imgui_demo_window_visible", state->imgui_demo_window_visible); @@ -205,7 +205,7 @@ static void module_reload(struct module_state *state, void* read_serializer) { cout << "Creating render entity ..." << endl; state->character = new CharacterEntity; - state->character->position = Vector3f (0.f, 0.f, 0.f); + state->character->mPosition = Vector3f (0.f, 0.f, 0.f); // load the state of the entity if (read_serializer != nullptr) {