martin
/
protot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge branch 'master' into docking

master
Martin Felis 2017-03-26 14:19:21 +02:00
parent 2f874b6d0d 992e0fe6a2
commit 39cd8bde19
267 changed files with 71727 additions and 368 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
3rdparty/rbdl/.editorconfig vendored Normal file
View File

@ -0,0 +1,10 @@
root = true
[*]
end_of_line = lf
indent_style = space
indent_size = 2
charset = utf-8
[Makefile]
indent_style = tab

6
3rdparty/rbdl/.hg_archival.txt vendored Normal file
View File

@ -0,0 +1,6 @@
repo: d536c26f2a238e943cf08a038d1134c782b1e66c
node: ee446c22d4fbfb51b29d2d708472a958e2eb38b6
branch: dev
latesttag: v2.5.0
latesttagdistance: 43
changessincelatesttag: 103

38
3rdparty/rbdl/.hgignore vendored Normal file
View File

@ -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

14
3rdparty/rbdl/.hgtags vendored Normal file
View File

@ -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

44
3rdparty/rbdl/CMake/FindCython.cmake vendored Normal file
View File

@ -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 )

80
3rdparty/rbdl/CMake/FindEigen3.cmake vendored Normal file
View File

@ -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, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
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)

40
3rdparty/rbdl/CMake/FindUnitTest++.cmake vendored Normal file
View File

@ -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
)

4
3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake vendored Normal file
View File

@ -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" )

295
3rdparty/rbdl/CMake/UseCython.cmake vendored Normal file
View File

@ -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( <module_name> <src1> <src2> ... <srcN> )
#
# To create a standalone executable, the function
#
# cython_add_standalone_executable( <executable_name> [MAIN_MODULE src1] <src1> <src2> ... <srcN> )
#
# 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
# <executable_name> is assumed to be the MAIN_MODULE.
#
# Where <module_name> is the name of the resulting Python module and
# <src1> <src2> ... are source files to be compiled into the module, e.g. *.pyx,
# *.py, *.c, *.cxx, etc. A CMake target is created with name <module_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( <name> 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()

214
3rdparty/rbdl/CMakeLists.txt vendored Normal file
View File

@ -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 <martin@fysx.org>")
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)

209
3rdparty/rbdl/CONTRIBUTING.md vendored Normal file
View File

@ -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<SpatialVector> v; // ok, v is an
std::vector<SpatialVector> S; // ok, S is commonly used in a reference algorithm
std::vector<double> u; // ok
std::vector<Vector3d> multdof3_u; // ok, 3-dof specialization of Model::u
std::vector<unsigned int> 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
<branchname>```.
### 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 <newfeature> && cd <newfeature>
# 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.

1837
3rdparty/rbdl/Doxyfile vendored Normal file
View File

File diff suppressed because it is too large Load Diff

23
3rdparty/rbdl/LICENSE vendored Normal file
View File

@ -0,0 +1,23 @@
RBDL - Rigid Body Dynamics Library
Copyright (c) 2011-2016 Martin Felis <martin@fysx.org>
(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.

171
3rdparty/rbdl/README.md vendored Normal file
View File

@ -0,0 +1,171 @@
RBDL - Rigid Body Dynamics Library
Copyright (c) 2011-2016 Martin Felis <martin@fysx.org>
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 <martin@fysx.org>
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 <martin@fysx.org>
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).

60
3rdparty/rbdl/addons/benchmark/CMakeLists.txt vendored Normal file
View File

@ -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)

158
3rdparty/rbdl/addons/benchmark/Human36Model.cc vendored Normal file
View File

@ -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");
}

11
3rdparty/rbdl/addons/benchmark/Human36Model.h vendored Normal file
View File

@ -0,0 +1,11 @@
#ifndef _HUMAN36MODEL_H
#define _HUMAN36MODEL_H
namespace RigidBodyDynamics {
class Model;
}
void generate_human36model (RigidBodyDynamics::Model *model);
/* _HUMAN36MODEL_H */
#endif

85
3rdparty/rbdl/addons/benchmark/SampleData.h vendored Normal file
View File

@ -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<double>(rand()) / static_cast<double>(RAND_MAX)) * 2. -1.;
qdot[si][i] = (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) * 2. -1.;
qddot[si][i] = (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) * 2. -1.;
tau[si][i] = (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) * 2. -1.;
}
}
}
};
#endif

29
3rdparty/rbdl/addons/benchmark/Timer.h vendored Normal file
View File

@ -0,0 +1,29 @@
#ifndef _TIMER_H
#define _TIMER_H
#include <ctime>
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<double>(timer->clock_end_value - timer->clock_start_value) * 1 / CLOCKS_PER_SEC;
return timer->duration_sec;
}
#endif

879
3rdparty/rbdl/addons/benchmark/benchmark.cc vendored Normal file
View File

@ -0,0 +1,879 @@
#include <iostream>
#include <algorithm>
#include <string>
#include <vector>
#include <cstdlib>
#include <iomanip>
#include <sstream>
#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<InverseKinematicsConstraintSet> &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<InverseKinematicsConstraintSet> cs_one_point;
std::vector<InverseKinematicsConstraintSet> cs_two_point_one_orientation;
std::vector<InverseKinematicsConstraintSet> cs_two_full_one_point;
std::vector<InverseKinematicsConstraintSet> cs_two_full_two_point_one_orientation;
std::vector<InverseKinematicsConstraintSet> 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 <sample_count>] [--depth|-d <depth>] <model.lua>" << endl;
#else
cout << "Usage: benchmark [--count|-c <sample_count>] [--depth|-d <depth>]" << endl;
#endif
cout << "Simple benchmark tool for the Rigid Body Dynamics Library." << endl;
cout << " --count | -c <sample_count> : sets the number of sample states that should" << endl;
cout << " be calculated (default: 1000)" << endl;
cout << " --depth | -d <depth> : sets maximum depth for the branched test model" << endl;
cout << " which is created increased from 1 to <depth> (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;
}

54
3rdparty/rbdl/addons/benchmark/model_generator.cc vendored Normal file
View File

@ -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);
}

11
3rdparty/rbdl/addons/benchmark/model_generator.h vendored Normal file
View File

@ -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

80
3rdparty/rbdl/addons/geometry/CMakeLists.txt vendored Normal file
View File

@ -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
)

525
3rdparty/rbdl/addons/geometry/Function.h vendored Normal file
View File

@ -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 <cassert>
#include <rbdl/rbdl_math.h>
#include <vector>
#include <cmath>
#include <cstdio>
/**
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_<double>.
Subclasses define particular mathematical functions. Predefined subclasses
are provided for several common function types: Function_<T>::Constant,
Function_<T>::Linear, Function_<T>::Polynomial, and Function_<T>::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 T>
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<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const = 0;
/** This provides compatibility with std::vector without requiring any
copying. **/
/*
T calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(std::vector<int>(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_<double> Function;
/**
* This is a Function_ subclass which simply returns a fixed value, independent
* of its arguments.
*/
template <class T>
class Function_<T>::Constant : public Function_<T> {
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<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const {
return static_cast<T>(0);
}
virtual int getArgumentSize() const {
return argumentSize;
}
int getMaxDerivativeOrder() const {
return std::numeric_limits<int>::max();
}
/** This provides compatibility with std::vector without requiring any
copying. **/
/*
T calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(std::vector<int>(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 T>
class Function_<T>::Linear : public Function_<T> {
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<T>(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<int>& 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<T>(0);
}
virtual int getArgumentSize() const {
return coefficients.size()-1;
}
int getMaxDerivativeOrder() const {
return std::numeric_limits<int>::max();
}
/** This provides compatibility with std::vector without requiring any
copying. **/
/*
T calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(ArrayViewConst_<int>(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 T>
class Function_<T>::Polynomial : public Function_<T> {
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<T>(0);
for (int i = 0; i < coefficients.size(); ++i)
value = value*arg + coefficients[i];
return value;
}
T calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const {
assert(x.size() == 1);
assert(derivComponents.size() > 0);
double arg = x[0];
T value = static_cast<T>(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<int>::max();
}
/** This provides compatibility with std::vector without requiring any
copying. **/
/*
T calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(ArrayViewConst_<int>(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_<double>::Sinusoid : public Function_<double> {
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_<T> 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<int>& 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<int>::max();
}
/** This provides compatibility with std::vector without requiring any
copying. **/
/*
double calcDerivative(const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(ArrayViewConst_<int>(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 T>
class Function_<T>::Step : public Function_<T> {
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_<T>::Step::ctor()",
"A zero-length switching interval is illegal; both ends were %g.", x0);
*/
assert(x0 != x1);
std::printf( "Function_<T>::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_<T>::Step::calcValue()",
"Expected just one input argument but got %d.", xin.size());
*/
assert(xin.size() == 1);
std::printf( "Function_<T>::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<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& xin) const {
/*
SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
"Function_<T>::Step::calcDerivative()",
"Expected just one input argument but got %d.", xin.size());
*/
assert(xin.size() == 1);
std::printf( "Function_<T>::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_<T>::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_<T>::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<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& x) const
{ return calcDerivative(ArrayViewConst_<int>(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_

23
3rdparty/rbdl/addons/geometry/LICENSE vendored Normal file
View File

@ -0,0 +1,23 @@
Rigid Body Dynamics Library Geometry Addon -
Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
(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.

202
3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt vendored Normal file
View File

@ -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.

24
3rdparty/rbdl/addons/geometry/NOTICE vendored Normal file
View File

@ -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

60
3rdparty/rbdl/addons/geometry/README.md vendored Normal file
View File

@ -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 <matthew.millard@iwr.uni-heidelberg.de>
\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.

1285
3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc vendored Normal file
View File

File diff suppressed because it is too large Load Diff

836
3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h vendored Normal file
View File

@ -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 <vector>
#include <rbdl/rbdl_math.h>
#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.
<B>Future Upgrades</B>
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.
<B>Computational Cost Details</B>
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.
<B> RBDL Port Notes </B>
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.
<B>Computational Costs</B>
\verbatim
~219 flops
\endverbatim
<B>Example:</B>
@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.
<B>Computational Costs</B>
Quoted for a Bezier curve set containing 1 to 5 curves.
\verbatim
~9-25
\endverbatim
<B>Example:</B>
@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<RigidBodyDynamics::Math::VectorNd>& 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:
<B>Computational Costs</B>
\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
<B>Example:</B>
@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.
<B>Computational Costs</B>
\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
<B>Example:</B>
@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
<B>Computational Costs</B>
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
<B>Example:</B>
@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:
<B>Computational Costs</B>
\verbatim
~55 flops
\endverbatim
<B>Example:</B>
@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.
<B>Computational Costs</B>
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.
<B>Example:</B>
@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<NUM_SAMPLE_PTS;i++){
u(i) = ( (double)i )/( (double)(NUM_SAMPLE_PTS-1) );
x(i) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u(i),_mX(s),_name);
if(_numBezierSections > 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<double>::
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<double>::
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_<SimTK::Spline>& 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_<double>& 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<double> > _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

844
3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc vendored Normal file
View File

@ -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 <fstream>
#include <ostream>
//=============================================================================
// 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<double>::epsilon()*1e6;
static double INTTOL = std::numeric_limits<double>::epsilon()*1e2;
static double SQRTEPS = std::sqrt(numeric_limits<double>::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<double>::
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_<int>(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<int>& 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<NUM_SAMPLE_PTS;i++){
u = ( (double)i )/( (double)(NUM_SAMPLE_PTS-1) );
x[i] = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,_mXVec[s]);
if(_numBezierSections > 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<int> 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<pts*10; j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
//generate some points in the mid region
for(int i=0; i< midX.size()-1;i++){
x0 = midX[i];
x1 = midX[i+1];
delta = (x1-x0)/pts;
for(int j=0;j<pts;j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
}
//generate some points in the extrapolated region
x0 = _x1;
x1 = _x1 + 0.1*(_x1-_x0);
if(domainMax > x1)
x1 = domainMax;
delta = (1.0/9.0)*(x1-x0)/(pts);
for(int j=0;j<pts*10;j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
//Populate the results matrix at the sample points
RigidBodyDynamics::Math::VectorNd ax(1);
for(int i=0; i < xsmpl.size(); i++){
ax[0] = xsmpl[i];
results(i,0) = ax[0];
if(i==48){
double here = 1.0;
}
results(i,1) = calcValue(ax);
if(maxOrder>=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<std::string> 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<std::string>& 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<data.cols()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}

589
3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.h vendored Normal file
View File

@ -0,0 +1,589 @@
#ifndef SMOOTHSEGMENTEDFUNCTION_H_
#define SMOOTHSEGMENTEDFUNCTION_H_
/* -------------------------------------------------------------------------- *
* OpenSim: SmoothSegmentedFunction.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 "Function.h"
#include "SegmentedQuinticBezierToolkit.h"
#include <vector>
#include <rbdl/rbdl_math.h>
/**
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.
<B>Computational Cost Details</B>
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.
<B> RBDL Port Notes </B>
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_<double>
{
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.
<B>Computational Costs</B>
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
<B>Computational Costs</B>
\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
<B>Computational Costs</B>
\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
<B>Computational Costs</B>
\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
<B>Computational Costs</B>
This varies depending on the curve (as mentioned above).
\verbatim
~97,400 to 487,000 flops
\endverbatim
<B>Example</B>
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_<SimTK::Spline> _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<RigidBodyDynamics::Math::VectorNd> _mXVec;
/**Bezier Y1,...,Yn control point locations. Control points are
stored in 6x1 vectors in the order above*/
std::vector<RigidBodyDynamics::Math::VectorNd> _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<std::string>& 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<int>& 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_<int>& 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

16
3rdparty/rbdl/addons/geometry/geometry.h vendored Normal file
View File

@ -0,0 +1,16 @@
/*
*
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*
* 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

67
3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt vendored Normal file
View File

@ -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)

534
3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc vendored Normal file
View File

@ -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 <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
#include <fstream>
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<data.cols()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
void printMatrixToFile(
const RigidBodyDynamics::Math::MatrixNd& data,
string& filename)
{
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.rows(); i++){
for(int j = 0; j < data.cols(); j++){
if(j<data.cols()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
RigidBodyDynamics::Math::VectorNd
calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x,
RigidBodyDynamics::Math::VectorNd& y,
bool extrap_endpoints){
RigidBodyDynamics::Math::VectorNd dy(x.size());
double dx1,dx2;
double dy1,dy2;
int size = x.size();
for(int i=1; i<size-1; i++){
dx1 = x[i] - x[i-1];
dx2 = x[i+1] - x[i];
dy1 = y[i] - y[i-1];
dy2 = y[i+1] - y[i];
dy[i]= 0.5*dy1/dx1 + 0.5*dy2/dx2;
}
if(extrap_endpoints == true){
dy1 = dy[2] - dy[1];
dx1 = x[2] - x[1];
dy[0] = dy[1] + (dy1/dx1)*(x[0]-x[1]);
dy2 = dy[size-2] - dy[size-3];
dx2 = x[size-2] - x[size-3];
dy[size-1] = dy[size-2] + (dy2/dx2)*(x[size-1]-x[size-2]);
}
return dy;
}
RigidBodyDynamics::Math::VectorNd
calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x,
SmoothSegmentedFunction& mcf,
double tol, int order){
RigidBodyDynamics::Math::VectorNd dyV(x.size());
RigidBodyDynamics::Math::VectorNd yV(x.size());
double y = 0;
double dy = 0;
double dyNUM = 0;
double err= 0;
double h = 0;
double xL = 0;
double xR = 0;
double c3 = 0;
double fL = 0;
double fR = 0;
double rootEPS = sqrt(EPSILON);
double y_C3min = 1e-10;
double y_C3max = 1e1;
for(int i=0; i<x.size(); i++){
yV[i] = mcf.calcDerivative(x[i],order-1);
}
for(int i=0; i< x.size(); i++){
c3 = abs(mcf.calcDerivative(x[i],order+2));
//singularity prevention
if(abs(c3) < y_C3min)
c3 = y_C3min;
//Compute h
y = abs(mcf.calcDerivative(x[i], order-1));
//preventing 0 from being assigned to y
if(y < y_C3min)
y = y_C3min;
//Dumb check
if(y/c3 < y_C3min){
c3 = 1;
y = y_C3min;
}
if(y/c3 > 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.size(); i++){
if(y[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 <y.size(); i++){
if(y[i] < y[i-1]-EPSILON*multEPS){
isMonotonic = false;
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){
isMonotonic = false;
}
return isMonotonic;
}
RigidBodyDynamics::Math::VectorNd calcTrapzIntegral(
RigidBodyDynamics::Math::VectorNd& x,
RigidBodyDynamics::Math::VectorNd& y,
bool flag_TrueIntForward_FalseIntBackward)
{
RigidBodyDynamics::Math::VectorNd inty
= RigidBodyDynamics::Math::VectorNd::Zero(y.size());
//inty = 0;
int startIdx = 1;
int endIdx = y.size()-1;
if(flag_TrueIntForward_FalseIntBackward == true){
double width = 0;
for(int i = 1; i <= endIdx; i=i+1){
width = abs(x[i]-x[i-1]);
inty[i] = inty[i-1] + width*(0.5)*(y[i]+y[i-1]);
}
}else{
double width = 0;
for(int i = endIdx-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<mcfSample.rows(); j++)
domainX[j] = mcfSample(j,0);
for(int i=0; i < maxDer; i++){
//Compute the relative error
numSample.col(i)=calcCentralDifference(domainX,mcf,tol,i+1);
for(int j=0; j<mcfSample.rows();++j ){
relError(j,i)= mcfSample(j,i+2)-numSample(j,i);
}
//compute a relative error where possible
for(int j=0; j < relError.rows(); j++){
if(abs(mcfSample(j,i+2)) > 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<maxDer;j++){
for(int i=0; i<mcfSample.rows(); i++){
if(relError(i,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<mcfSample.rows(); i++){
if(tolExceeded12V(i) == 1){
printf("%f %f %f %f %f %f %f",
mcfSample(i,0),relError(i,0),mcfSample(i,2),numSample(i,0),
relError(i,1),mcfSample(i,3),numSample(i,1));
}
}
}
flagError12=false;*/
//tolExceeded12V =
//RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows());
}
return (tolExceeded12 == 0);
}

289
3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.h vendored Normal file
View File

@ -0,0 +1,289 @@
/* -------------------------------------------------------------------------- *
* 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 <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
using namespace RigidBodyDynamics::Addons::Geometry;
using namespace std;
static double EPSILON = numeric_limits<double>::epsilon();
static double SQRTEPSILON = sqrt(EPSILON);
static double TOL = std::numeric_limits<double>::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);

483
3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc vendored Normal file
View File

@ -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 <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
#include <fstream>
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 <<"**************************************************"<<endl;
//cout << " TEST: Bezier Curve Derivative DU" << endl;
string name = "testSegmentedQuinticBezierDerivatives()";
RigidBodyDynamics::Math::VectorNd xPts(6);
RigidBodyDynamics::Math::VectorNd yPts(6);
xPts[0] = 0;
xPts[1] = 0.5;
xPts[2] = 0.5;
xPts[3] = 0.75;
xPts[4] = 0.75;
xPts[5] = 1;
yPts[0] = 0;
yPts[1] = 0.125;
yPts[2] = 0.125;
yPts[3] = 0.5;
yPts[4] = 0.5;
yPts[5] = 1;
double val = 0;
double d1 = 0;
double d2 = 0;
double d3 = 0;
double d4 = 0;
double d5 = 0;
double d6 = 0;
double u = 0;
int steps = 100;
RigidBodyDynamics::Math::MatrixNd analyticDerXU(steps,8);
RigidBodyDynamics::Math::MatrixNd analyticDerYU(steps,8);
RigidBodyDynamics::Math::VectorNd uV(steps);
for(int i = 0; i<steps; i++){
//int i = 10;
u = (double)i/(steps-1);
uV[i] = u;
val= SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,xPts);
d1 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,1);
d2 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,2);
d3 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,3);
d4 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,4);
d5 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,5);
d6 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,6);
analyticDerXU(i,0) = u;
analyticDerXU(i,1) = val;
analyticDerXU(i,2) = d1;
analyticDerXU(i,3) = d2;
analyticDerXU(i,4) = d3;
analyticDerXU(i,5) = d4;
analyticDerXU(i,6) = d5;
analyticDerXU(i,7) = d6;
val= SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,yPts);
d1 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,1);
d2 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,2);
d3 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,3);
d4 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,4);
d5 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,5);
d6 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,6);
analyticDerYU(i,0) = u;
analyticDerYU(i,1) = val;
analyticDerYU(i,2) = d1;
analyticDerYU(i,3) = d2;
analyticDerYU(i,4) = d3;
analyticDerYU(i,5) = d4;
analyticDerYU(i,6) = d5;
analyticDerYU(i,7) = d6;
}
int mxDU = 6-1;
RigidBodyDynamics::Math::MatrixNd numericDer(analyticDerXU.rows(), mxDU);
RigidBodyDynamics::Math::MatrixNd errorDer(analyticDerXU.rows(), mxDU);
double tol = (double)(1.0/steps);
tol = tol*tol*50;
//Numerical error in a central difference increases with the
//square of h.
//http://en.wikipedia.org/wiki/Finite_difference
RigidBodyDynamics::Math::VectorNd domainX =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
RigidBodyDynamics::Math::VectorNd rangeY =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
RigidBodyDynamics::Math::VectorNd analyticDerYX =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
for(int j=0; j<analyticDerXU.rows(); j++){
domainX[j] = analyticDerXU(j,0);
}
for(int i=0;i<mxDU;i++){
for(int j=0; j<analyticDerXU.rows(); j++){
rangeY[j] = analyticDerXU(j,i+1);
analyticDerYX[j] = analyticDerXU(j,i+2);
}
numericDer.col(i) = calcCentralDifference(domainX,
rangeY,true);
for(int j=0; j<analyticDerYX.rows();++j){
errorDer(j,i) = analyticDerYX[j]-numericDer(j,i);
}
//errorDer(i)= abs( errorDer(i).elementwiseDivide(numericDer(i)) );
//The end points can't be tested because a central difference
//cannot be accurately calculated at these locations
for(int j=1; j<analyticDerXU.rows()-1; j++){
assert( abs(errorDer(j,i))<tol );
//if(errorDer(j,i)>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; j<numericDerXY.cols();j++){
for(int k=0; k<numericDerXY.rows(); k++){
domainX[k] = analyticDerXU(k,1);
if(j == 0){
rangeY[k] = analyticDerYU(k,1);
}else{
rangeY[k] = analyticDerXY(k,j-1);
}
}
numericDerXY.col(j) = calcCentralDifference(domainX,
rangeY,true);
}
//Generate numerical derivative curves for the first 3 derivatives
/*
numericDerXY.col(0) = calcCentralDifference(analyticDerXU.col(1),
analyticDerYU.col(1),true);
numericDerXY.col(1) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(0),true);
numericDerXY.col(2) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(1),true);
numericDerXY.col(3) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(2),true);
numericDerXY.col(4) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(3),true);
numericDerXY.col(5) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(4),true);
*/
//Create the matrix of errors
RigidBodyDynamics::Math::MatrixNd errorDerXYNum(analyticDerXU.rows(), 6);
RigidBodyDynamics::Math::MatrixNd errorDerXYDen(analyticDerXU.rows(), 6);
RigidBodyDynamics::Math::MatrixNd errorDerXY(analyticDerXU.rows(), 6);
for(int i = 0; i < errorDerXYNum.rows(); ++i){
for(int j = 0; j < errorDerXYNum.cols(); ++j){
errorDerXYNum(i,j) = abs( analyticDerXY(i,j)-numericDerXY(i,j));
errorDerXYDen(i,j) = abs( analyticDerXY(i,j)+numericDerXY(i,j));
errorDerXY(i,j) = errorDerXYNum(i,j)/errorDerXYDen(i,j);
}
}
double relTol = 5e-2;
int relTolExceeded = 0;
for(int j=0;j<6;j++){
//can't test the first and last entries because a central diff.
//cannot calculate these values accurately.
for(int i=1;i<analyticDerXU.rows()-1;i++){
if(errorDerXY(i,j)>relTol){
//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 <<"**************************************************"<<endl;
}
TEST(QuinticBezierToolKitDerivatives)
{
int maximumNumberOfToleranceViolations = 10;
testSegmentedQuinticBezierDerivatives(10);
}
TEST(SmoothSegmentedFunctionProperties)
{
//1. Make a fake monotonic curve
RigidBodyDynamics::Math::VectorNd x(5);
RigidBodyDynamics::Math::VectorNd y(5);
RigidBodyDynamics::Math::VectorNd dydx(5);
for(int i=0; i<x.size();++i){
x[i] = i*0.5*M_PI/(x.size()-1);
y[i] = sin(x[i]) + x[i];
dydx[i] = cos(x[i]) + 1.0;
}
double c = 0.5;
RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4);
RigidBodyDynamics::Math::MatrixNd p0(6,2);
for(int i=0; i < 4; ++i){
p0 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCornerControlPoints( x[i], y[i], dydx[i],
x[i+1],y[i+1],dydx[i+1],c);
mX.col(i) = p0.col(0);
mY.col(i) = p0.col(1);
}
SmoothSegmentedFunction testCurve = SmoothSegmentedFunction();
testCurve.updSmoothSegmentedFunction( mX, mY,
x[0], x[4],
y[0], y[4],
dydx[0],dydx[4],
"testCurve");
//2. Test key points.
RigidBodyDynamics::Math::VectorNd yErr(5);
RigidBodyDynamics::Math::VectorNd dydxErr(5);
for(int i=0; i<x.size(); ++i){
yErr[i] = testCurve.calcValue(x[i]) - y[i];
dydxErr[i] = testCurve.calcDerivative(x[i],1) - dydx[i];
CHECK( abs(yErr[i]) < TOL_SMALL );
CHECK( abs(dydxErr[i]) < TOL_SMALL );
}
//3. Test derivatives for numerical consistency
RigidBodyDynamics::Math::MatrixNd testCurveSample
= testCurve.calcSampledCurve( 6, x[0]-0.1, x[4]+0.1);
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
testCurve,
testCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//4. Test C2 continuity
bool curveIsContinuous = isCurveC2Continuous(testCurve,
testCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//5. Test monotonicity
bool curveIsMonotonic = isCurveMontonic(testCurveSample);
CHECK(curveIsMonotonic);
}
TEST(ShiftScale)
{
//1. Make a curve
RigidBodyDynamics::Math::VectorNd xV(5);
RigidBodyDynamics::Math::VectorNd yV(5);
RigidBodyDynamics::Math::VectorNd dydxV(5);
for(int i=0; i<xV.size();++i){
xV[i] = i*0.5*M_PI/(xV.size()-1);
yV[i] = sin(xV[i]) + xV[i];
dydxV[i] = cos(xV[i]) + 1.0;
}
double c = 0.5;
RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4);
RigidBodyDynamics::Math::MatrixNd p0(6,2);
for(int i=0; i < 4; ++i){
p0 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCornerControlPoints( xV[i], yV[i], dydxV[i],
xV[i+1],yV[i+1],dydxV[i+1],c);
mX.col(i) = p0.col(0);
mY.col(i) = p0.col(1);
}
SmoothSegmentedFunction curve = SmoothSegmentedFunction();
curve.updSmoothSegmentedFunction( mX, mY,
xV[0], xV[4],
yV[0], yV[4],
dydxV[0],dydxV[4],
"testCurve");
SmoothSegmentedFunction shiftedCurve = SmoothSegmentedFunction();
shiftedCurve = curve;
double xShift = 1.0/3.0;
double yShift = 2.0/3.0;
shiftedCurve.shift(xShift,yShift);
//Test shift
double x ,y, dydx, d2ydx2;
double xS ,yS, dydxS, d2ydx2S;
double xmin = -0.1;
double xmax = 0.5*M_PI+0.1;
for(int i=0; i<xV.size();++i){
x = xmin + i*(xmax-xmin)/(xV.size()-1);
y = curve.calcValue(x);
dydx = curve.calcDerivative(x,1);
d2ydx2 = curve.calcDerivative(x,2);
xS = x + xShift;
yS = shiftedCurve.calcValue(xS) - yShift;
dydxS = shiftedCurve.calcDerivative(xS,1);
d2ydx2S = shiftedCurve.calcDerivative(xS,2);
CHECK( abs(y-yS) < TOL_SMALL );
CHECK( abs( dydx-dydxS) < TOL_SMALL );
CHECK( abs(d2ydx2-d2ydx2S) < TOL_SMALL );
}
//Test scale
SmoothSegmentedFunction scaledCurve = SmoothSegmentedFunction();
scaledCurve = curve;
double xScale = 1.0/2.0;
double yScale = 5.0/3.0;
double dydxScale = yScale/xScale;
scaledCurve.scale(xScale,yScale);
for(int i=0; i<xV.size();++i){
x = xmin + i*(xmax-xmin)/(xV.size()-1);
y = curve.calcValue(x);
dydx = curve.calcDerivative(x,1);
d2ydx2 = curve.calcDerivative(x,2);
xS = x*xScale;
yS = scaledCurve.calcValue(xS)/yScale;
dydxS = scaledCurve.calcDerivative(xS,1)/dydxScale ;
d2ydx2S = scaledCurve.calcDerivative(xS,2)*xScale/(dydxScale);
CHECK( abs(y-yS) < TOL_SMALL );
CHECK( abs( dydx-dydxS) < TOL_SMALL );
CHECK( abs(d2ydx2-d2ydx2S) < TOL_SMALL );
}
}
int main (int argc, char *argv[])
{
return UnitTest::RunAllTests ();
}

76
3rdparty/rbdl/addons/luamodel/CMakeLists.txt vendored Normal file
View File

@ -0,0 +1,76 @@
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
)
FIND_PACKAGE (Lua51 REQUIRED)
INCLUDE_DIRECTORIES (
${CMAKE_CURRENT_BINARY_DIR}/include/rbdl
${LUA_INCLUDE_DIR}
)
SET ( LUAMODEL_SOURCES
luamodel.cc
luatables.cc
)
ADD_EXECUTABLE (rbdl_luamodel_util rbdl_luamodel_util.cc)
IF (RBDL_BUILD_STATIC)
ADD_LIBRARY ( rbdl_luamodel-static STATIC ${LUAMODEL_SOURCES} )
IF (NOT WIN32)
SET_TARGET_PROPERTIES ( rbdl_luamodel-static PROPERTIES PREFIX "lib")
ENDIF (NOT WIN32)
SET_TARGET_PROPERTIES ( rbdl_luamodel-static PROPERTIES OUTPUT_NAME "rbdl_luamodel")
TARGET_LINK_LIBRARIES (rbdl_luamodel-static
rbdl-static
${LUA_LIBRARIES}
)
TARGET_LINK_LIBRARIES (rbdl_luamodel_util
rbdl_luamodel-static
rbdl-static
)
INSTALL (TARGETS rbdl_luamodel-static rbdl_luamodel_util
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
)
ELSE (RBDL_BUILD_STATIC)
ADD_LIBRARY ( rbdl_luamodel SHARED ${LUAMODEL_SOURCES} )
SET_TARGET_PROPERTIES ( rbdl_luamodel PROPERTIES
VERSION ${RBDL_VERSION}
SOVERSION ${RBDL_SO_VERSION}
)
TARGET_LINK_LIBRARIES (rbdl_luamodel
rbdl
${LUA_LIBRARIES}
)
TARGET_LINK_LIBRARIES (rbdl_luamodel_util
rbdl_luamodel
rbdl
)
INSTALL (TARGETS rbdl_luamodel rbdl_luamodel_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/luamodel
)

51
3rdparty/rbdl/addons/luamodel/README vendored Normal file
View File

@ -0,0 +1,51 @@
rbdl_luamodel - load RBDL models from Lua files
Copyright (c) 2012-2016 Martin Felis <martin@fysx.org>
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 <martin@fysx.org>
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.

508
3rdparty/rbdl/addons/luamodel/luamodel.cc vendored Normal file
View File

@ -0,0 +1,508 @@
#include "rbdl/rbdl.h"
#include "luamodel.h"
#include <iostream>
#include <map>
#include "luatables.h"
extern "C"
{
#include <lua.h>
#include <lauxlib.h>
#include <lualib.h>
}
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
template<>
Vector3d LuaTableNode::getDefault<Vector3d>(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<SpatialVector>(
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<Matrix3d>(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<SpatialTransform>(
const SpatialTransform &default_value
) {
SpatialTransform result = default_value;
if (stackQueryValue()) {
LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
result.r = vector_table["r"].getDefault<Vector3d>(Vector3d::Zero(3));
result.E = vector_table["E"].getDefault<Matrix3d>(Matrix3d::Identity (3,3));
}
stackRestore();
return result;
}
template<>
Joint LuaTableNode::getDefault<Joint>(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<std::string>("");
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<SpatialVector>());
break;
case 2:
result = Joint(
vector_table[1].get<SpatialVector>(),
vector_table[2].get<SpatialVector>()
);
break;
case 3:
result = Joint(
vector_table[1].get<SpatialVector>(),
vector_table[2].get<SpatialVector>(),
vector_table[3].get<SpatialVector>()
);
break;
case 4:
result = Joint(
vector_table[1].get<SpatialVector>(),
vector_table[2].get<SpatialVector>(),
vector_table[3].get<SpatialVector>(),
vector_table[4].get<SpatialVector>()
);
break;
case 5:
result = Joint(
vector_table[1].get<SpatialVector>(),
vector_table[2].get<SpatialVector>(),
vector_table[3].get<SpatialVector>(),
vector_table[4].get<SpatialVector>(),
vector_table[5].get<SpatialVector>()
);
break;
case 6:
result = Joint(
vector_table[1].get<SpatialVector>(),
vector_table[2].get<SpatialVector>(),
vector_table[3].get<SpatialVector>(),
vector_table[4].get<SpatialVector>(),
vector_table[5].get<SpatialVector>(),
vector_table[6].get<SpatialVector>()
);
break;
default:
cerr << "Invalid number of DOFs for joint." << endl;
abort();
break;
}
}
stackRestore();
return result;
}
template<>
Body LuaTableNode::getDefault<Body>(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<Vector3d>(com);
inertia = vector_table["inertia"].getDefault<Matrix3d>(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<ConstraintSet>& constraint_sets,
const std::vector<std::string>& constraint_set_names,
bool verbose
);
typedef map<string, unsigned int> 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<ConstraintSet>& constraint_sets,
const std::vector<std::string>& 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<Vector3d>();
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>("");
string parent_name = model_table["frames"][i]["parent"].get<string>();
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>(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<ConstraintSet>& constraint_sets,
const std::vector<std::string>& 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<string>("");
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<string>("").c_str())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["point"].getDefault<Vector3d>(Vector3d::Zero())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["normal"].getDefault<Vector3d>(Vector3d::Zero())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["name"].getDefault<string>("").c_str()
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["normal_acceleration"].getDefault<double>(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<string>("") << endl;
cout << " body point = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["point"].getDefault<Vector3d>(Vector3d::Zero()).transpose()
<< endl;
cout << " world normal = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["normal"].getDefault<Vector3d>(Vector3d::Zero()).transpose()
<< endl;
cout << " constraint name = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["name"].getDefault<string>("") << endl;
cout << " normal acceleration = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["normal_acceleration"].getDefault<double>(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<string>("").c_str())
, model->GetBodyId(model_table["constraint_sets"]
[constraint_set_names[i].c_str()][ci + 1]["successor_body"]
.getDefault<string>("").c_str())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["predecessor_transform"].getDefault<SpatialTransform>(SpatialTransform())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["successor_transform"].getDefault<SpatialTransform>(SpatialTransform())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["axis"].getDefault<SpatialVector>(SpatialVector::Zero())
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["stabilization_coefficient"].getDefault<double>(1.)
, model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1]
["name"].getDefault<string>("").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<string>("") << endl;
cout << " successor body = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["successor_body"].getDefault<string>("") << endl;
cout << " predecessor body transform = " << endl
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["predecessor_transform"]
.getDefault<SpatialTransform>(SpatialTransform()) << endl;
cout << " successor body transform = " << endl
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["successor_transform"]
.getDefault<SpatialTransform>(SpatialTransform()) << endl;
cout << " constraint axis (in predecessor frame) = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["axis"].getDefault<SpatialVector>(SpatialVector::Zero())
.transpose() << endl;
cout << " stabilization coefficient = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["stabilization_coefficient"].getDefault<double>(1.) << endl;
cout << " constraint name = "
<< model_table["constraint_sets"][constraint_set_names[i].c_str()]
[ci + 1]["name"].getDefault<string>("").c_str() << endl;
}
}
else {
cerr << "Invalid constraint type: " << constraintType << endl;
abort();
}
}
}
return true;
}
}
}

309
3rdparty/rbdl/addons/luamodel/luamodel.h vendored Normal file
View File

@ -0,0 +1,309 @@
#ifndef _RBDL_LUAMODEL_H
#define _RBDL_LUAMODEL_H
#include <rbdl/rbdl_config.h>
#include <string>
#include <vector>
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. <a href="http://www.lua.org">Lua</a> 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 = {
* {
* <frame 1 information table>
* },
* {
* <frame 2 information table>
* }
* }
* }
*
* 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<ConstraintSet>& constraint_sets,
const std::vector<std::string>& 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

932
3rdparty/rbdl/addons/luamodel/luatables.cc vendored Normal file
View File

@ -0,0 +1,932 @@
/*
* LuaTables++
* Copyright (c) 2013-2014 Martin Felis <martin@fyxs.org>.
* 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 <assert.h>
#include <iostream>
#include <cstdlib>
#include <vector>
#include <sstream>
#include <cmath>
extern "C"
{
#include <lua.h>
#include <lauxlib.h>
#include <lualib.h>
}
#include <stdio.h> /* defines FILENAME_MAX */
#if defined(WIN32) || defined (_WIN32)
#include <direct.h>
#define get_current_dir _getcwd
#define DIRECTORY_SEPARATOR "\\"
#elif defined(linux) || defined (__linux) || defined(__linux__) || defined(__APPLE__)
#include <unistd.h>
#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<LuaKey> 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<LuaKey> 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<LuaKey> LuaTableNode::getKeyStack() {
std::vector<LuaKey> 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<LuaKey> 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<LuaKey> 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<LuaKey> 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<LuaKey> 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<LuaKey> 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<LuaKey> LuaTableNode::keys() {
std::vector<LuaKey> 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<int>(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<bool>(const bool &default_value) {
bool result = default_value;
if (stackQueryValue()) {
result = lua_toboolean (luaTable->L, -1);
}
stackRestore();
return result;
}
template<> float LuaTableNode::getDefault<float>(const float &default_value) {
float result = default_value;
if (stackQueryValue()) {
result = static_cast<float>(lua_tonumber (luaTable->L, -1));
}
stackRestore();
return result;
}
template<> double LuaTableNode::getDefault<double>(const double &default_value) {
double result = default_value;
if (stackQueryValue()) {
result = lua_tonumber (luaTable->L, -1);
}
stackRestore();
return result;
}
template<> std::string LuaTableNode::getDefault<std::string>(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<bool>(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<float>(const float &value) {
stackCreateValue();
l_push_LuaKey (luaTable->L, key);
lua_pushnumber(luaTable->L, static_cast<double>(value));
// stack: parent, key, value
lua_settable (luaTable->L, -3);
stackRestore();
}
template<> void LuaTableNode::set<double>(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<std::string>(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;
}

261
3rdparty/rbdl/addons/luamodel/luatables.h vendored Normal file
View File

@ -0,0 +1,261 @@
/*
* LuaTables++
* Copyright (c) 2013-2014 Martin Felis <martin@fyxs.org>.
* 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 <iostream>
#include <assert.h>
#include <cstdlib>
#include <string>
#include <vector>
#include <rbdl/rbdl_config.h>
// 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<LuaKey> getKeyStack();
std::string keyStackToString();
bool exists();
void remove();
size_t length();
std::vector<LuaKey> keys();
// Templates for setters and getters. Can be specialized for custom
// types.
template <typename T>
void set (const T &value);
template <typename T>
T getDefault (const T &default_value);
template <typename T>
T get() {
if (!exists()) {
std::cerr << "Error: could not find value " << keyStackToString() << "." << std::endl;
abort();
}
return getDefault (T());
}
// convenience operators (assignment, conversion, comparison)
template <typename T>
void operator=(const T &value) {
set<T>(value);
}
template <typename T>
operator T() {
return get<T>();
}
template <typename T>
bool operator==(T value) {
return value == get<T>();
}
template <typename T>
bool operator!=(T value) {
return value != get<T>();
}
LuaTableNode *parent;
LuaTable *luaTable;
LuaKey key;
int stackTop;
};
template<typename T>
bool operator==(T value, LuaTableNode node) {
return value == (T) node;
}
template<typename T>
bool operator!=(T value, LuaTableNode node) {
return value != (T) node;
}
template<> bool LuaTableNode::getDefault<bool>(const bool &default_value);
template<> double LuaTableNode::getDefault<double>(const double &default_value);
template<> float LuaTableNode::getDefault<float>(const float &default_value);
template<> std::string LuaTableNode::getDefault<std::string>(const std::string &default_value);
template<> void LuaTableNode::set<bool>(const bool &value);
template<> void LuaTableNode::set<float>(const float &value);
template<> void LuaTableNode::set<double>(const double &value);
template<> void LuaTableNode::set<std::string>(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

100
3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc vendored Normal file
View File

@ -0,0 +1,100 @@
#include "rbdl/rbdl.h"
#include "rbdl/rbdl_utils.h"
#include "luamodel.h"
#include <iostream>
#include <iomanip>
#include <sstream>
using namespace std;
using namespace RigidBodyDynamics::Math;
void usage (const char* argv_0) {
cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <model.lua>" << 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;
}

267
3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua vendored Normal file
View File

@ -0,0 +1,267 @@
-- 5b3d.lua
-- Copyright (c) 2016 Davide Corradi <davide.corradi@iwr.uni-heidelberg.de>
-- 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

87
3rdparty/rbdl/addons/luamodel/samplemodel.lua vendored Normal file
View File

@ -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

89
3rdparty/rbdl/addons/muscle/CMakeLists.txt vendored Normal file
View File

@ -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
)

23
3rdparty/rbdl/addons/muscle/LICENSE vendored Normal file
View File

@ -0,0 +1,23 @@
Rigid Body Dynamics Library Muscle Addon -
Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
(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.

202
3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt vendored Normal file
View File

@ -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.

1445
3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1102
3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

927
3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc vendored Normal file
View File

@ -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 <cmath>
#include <cstdio>
#include <iostream>
#include <fstream>
#include <sstream>
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<double>::epsilon()*1e2;
static double INTTOL = (double)std::numeric_limits<double>::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<double>::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<double>::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<double>::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);
}

832
3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h vendored Normal file
View File

@ -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 <cstdio>
#include <iostream>
#include <fstream>
#include <cmath>
/**
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
<B>Computational Cost Details</B>
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.
<B> RBDL Port Notes </B>
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
<B>Conditions:</B>
<B>Computational Costs</B>
\verbatim
~20,500 flops
\endverbatim
<B>Example:</B>
@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.
<br /><br />
Minimum Value: 0
Maximum Value: dydxC < 1
<br /><br />
@param dydxNearC The slope of the force velocity curve as it approaches
the maximum concentric (shortening) contraction velocity.
<br /><br />
Minimum Value: > dydxC
Maximum Value: dydxNearC < 1
<br /><br />
@param dydxIso The slope of the fv curve when dlce(t)/dt = 0.
<br /><br />
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.
<br /><br />
Minimum Value: 0
Maximum Value: dydxC < (fmaxE-1).
<br /><br />
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.
<br /><br />
Minimum Value: > dydxE
Maximum Value: dydxNearE < (fmaxE-1)
<br /><br />
@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
<B>Computational Costs</B>
\verbatim
~8,200 flops
\endverbatim
<B>Example:</B>
@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
<B>Computational Costs</B>
\verbatim
~4,100 flops
\endverbatim
<B>Example:</B>
@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
<B>Computational Costs</B>
\verbatim
~4,100 flops
\endverbatim
<B>Example:</B>
@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
<B>Computational Costs</B>
\verbatim
~4,100 flops
\endverbatim
<B>Example:</B>
@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
<B>Computational Costs</B>
\verbatim
~4,100 flops
\endverbatim
<B>Example:</B>
@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
<B>Computational Costs</B>
\verbatim
~4,100 flops
\endverbatim
<B>Example:</B>
@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_

22
3rdparty/rbdl/addons/muscle/NOTICE vendored Normal file
View File

@ -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

55
3rdparty/rbdl/addons/muscle/README.md vendored Normal file
View File

@ -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 <matthew.millard@iwr.uni-heidelberg.de>
\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.

1434
3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc vendored Normal file
View File

File diff suppressed because it is too large Load Diff

663
3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h vendored Normal file
View File

@ -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 <cstdio>
#include <iostream>
#include <fstream>
#include <cmath>
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.
<b>aborts</b>
-tvAtEccentricOmegaMax < 1.05
-tvAtHalfOmegaMax >= 0.45
-tvAtHalfOmegaMax <= 0.05
<b>References</b>
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.
<b>aborts</b>
-tvAtEccentricOmegaMax < 1.05
-tvAtHalfOmegaMax > 0.45 or tvAtHalfOmegaMax < 0.05
-slopeAtConcentricOmegaMax < 0
-slopeNearEccentricOmegaMax < 0
-slopeAtEccentricOmegaMax < 0
-eccentricCurviness < 0 or eccentricCurviness > 1
<b>References</b>
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.
<b>aborts</b>
- 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.
<b>aborts</b>
- 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.
<b>aborts</b>
- 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.
<b>aborts</b>
- 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.
<b>aborts</b>
- 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.
<b>aborts</b>
- 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_

160
3rdparty/rbdl/addons/muscle/csvtools.cc vendored Normal file
View File

@ -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<std::vector<double > > readMatrixFromFile(
const std::string& filename,
int startingRow)
{
std::ifstream datafile;
datafile.open(filename.c_str());
//SimTK::Matrix data;
std::vector<std::vector<double > > dataMatrix;
std::vector<double > 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<std::vector<double > >& 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[0].size()-1){
datafile << dataMatrix[i][j] << ", ";
}
else{
datafile << dataMatrix[i][j] << "\n";
}
}
}
datafile.close();
}
void printMatrixToFile(
const std::vector<std::vector< int > >& 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[0].size()-1){
datafile << dataMatrix[i][j] << ", ";
}
else{
datafile << dataMatrix[i][j] << "\n";
}
}
}
datafile.close();
}

55
3rdparty/rbdl/addons/muscle/csvtools.h vendored Normal file
View File

@ -0,0 +1,55 @@
/* -------------------------------------------------------------------------- *
* csvtools.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. *
* -------------------------------------------------------------------------- */
#include <iostream>
#include <sstream>
#include <string>
#include <fstream>
#include <vector>
#include <stdlib.h>
/**
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<std::vector<double > >& dataMatrix,
const std::string& header,
const std::string& filename);
void printMatrixToFile( const std::vector<std::vector<int > >& 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<std::vector<double > > readMatrixFromFile(
const std::string& filename,
int startingRow);

12
3rdparty/rbdl/addons/muscle/muscle.h vendored Normal file
View File

@ -0,0 +1,12 @@
//==============================================================================
/*
* RBDL - Rigid Body Dynamics Library: Addon : forceElements
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#ifndef MUSCLE_H_
#define MUSCLE_H_
#include "Millard2016TorqueMuscle.h"
#endif

78
3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt vendored Normal file
View File

@ -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)

725
3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc vendored Normal file
View File

@ -0,0 +1,725 @@
/* *
* TorqueMuscle
* Copyright (c) 2016 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
//==============================================================================
// INCLUDES
//==============================================================================
#include "../Millard2016TorqueMuscle.h"
#include "../csvtools.h"
#include "../../geometry/tests/numericalTestFunctions.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <ostream>
#include <sstream>
#include <stdio.h>
#include <exception>
#include <cassert>
#include <vector>
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)<TOL);
//Total force check
double torque = tq.calcJointTorque(jointAngleAtTauMax,0,activation);
double err = abs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque));
CHECK(abs(torque
- signOfJointTorque*(
tmi.fiberActiveTorque+tmi.fiberPassiveTorque)) < TOL);
//Total active force scales with activation
tq.calcTorqueMuscleInfo( jointAngleAtTauMax,
0.,
activation*0.5,
tmi);
CHECK(abs(tmi.fiberActiveTorque - tauMax*0.5) < TOL);
//Keypoint check - at omega at 50% tau max
//tq.calcTorqueMuscleInfo(jointAngleAtTauMax,
// signOfJointVelocity*omegaAt50TauMax,
// activation,
// tmi);
//CHECK(abs(tmi.jointAngularVelocity
// -signOfJointVelocity*omegaAt50TauMax) < TOL);
//CHECK(abs(tmi.fiberAngularVelocity-omegaAt50TauMax) < TOL);
//This test was updated because Anderson's curve is no longer
//used exactly as it is written in the paper because it
//has very odd behavior: the parameters used for ankle extension
//of the young men is such that the force velocity curve on
//the concentric side never goes to 0.
//CHECK(abs(tmi.fiberActiveTorque - tauMax*0.5) < 0.05*tauMax);
//Keypoint check - power
CHECK(abs(tmi.jointPower-tmi.fiberPower) < TOL);
//CHECK(abs(tmi.jointPower-tmi.fiberTorque*omegaAt50TauMax) < TOL);
//Keypoint check - where passive curve hits tau max.
double jointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
//tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
// signOfJointVelocity*omegaAt50TauMax,
// activation,
// tmi);
//CHECK(abs(tmi.fiberPassiveTorqueAngleMultiplier-1) < TOL);
//Numerically check the active and passive fiber stiffnesses
double h = sqrt(EPSILON);
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75,
0.,
activation,
tmi);
TorqueMuscleInfo tmiL;
TorqueMuscleInfo tmiR;
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75-h,
0.,
activation,
tmiL);
tq.calcTorqueMuscleInfo(jointAngleAtTauMax*0.75+h,
0.,
activation,
tmiR);
double jointK = (tmiR.jointTorque-tmiL.jointTorque)/(2*h);
err = tmi.jointStiffness - jointK;
CHECK(abs(tmi.jointStiffness-jointK) < 1e-5);
double fiberK = signOfJointAngle*(tmiR.fiberTorque-tmiL.fiberTorque)/(2*h);
err = tmi.fiberStiffness - fiberK;
CHECK(abs(tmi.fiberStiffness - fiberK) < 1e-5);
tq.setPassiveTorqueScale(1.5);
CHECK(abs(tq.getPassiveTorqueScale()-1.5)<TOL);
tq.setPassiveTorqueScale(1.0);
TorqueMuscleInfo tmi0;
TorqueMuscleInfo tmi1;
TorqueMuscleInfo tmi2;
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi0);
tq.setPassiveTorqueScale(2.0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi1);
CHECK(abs(tmi0.fiberPassiveTorqueAngleMultiplier -
0.5*tmi1.fiberPassiveTorqueAngleMultiplier) < TOL);
CHECK(abs(tmi0.fiberPassiveTorque -
0.5*tmi1.fiberPassiveTorque) < TOL);
double jtq = tq.calcJointTorque(jointAngleAtPassiveTauMax,
0.,
activation);
err = jtq-tmi1.jointTorque;
CHECK(abs(jtq-tmi1.jointTorque) < TOL );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation-SQRTEPSILON,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation,
tmi1);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
activation+SQRTEPSILON,
tmi2);
double DtqDa = tmi1.DjointTorqueDactivation;
double DtqDa_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDa-DtqDa_NUM);
CHECK(abs(DtqDa-DtqDa_NUM) < abs(DtqDa)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax-SQRTEPSILON,
0.,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+SQRTEPSILON,
0.,
activation,
tmi2);
double DtqDq = tmi1.DjointTorqueDjointAngle;
double DtqDq_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDq-DtqDq_NUM);
CHECK(abs(DtqDq-DtqDq_NUM) < abs(DtqDq)*1e-5 );
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.+SQRTEPSILON,
activation,
tmi2);
double DtqDqdot = tmi1.DjointTorqueDjointAngularVelocity;
double DtqDqdot_NUM = (tmi2.jointTorque-tmi0.jointTorque)/(2*SQRTEPSILON);
err = abs(DtqDqdot-DtqDqdot_NUM);
CHECK(abs(DtqDqdot-DtqDqdot_NUM) < abs(DtqDqdot)*1e-5 );
tq.setPassiveTorqueScale(1.0);
tq.setPassiveCurveAngleOffset(0.0);
jointAngleAtPassiveTauMax = tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.-SQRTEPSILON,
activation,
tmi0);
tq.setPassiveCurveAngleOffset(M_PI/3.0);
double updJointAngleAtPassiveTauMax =
tq.getJointAngleAtOneNormalizedPassiveIsometricTorque() ;
CHECK( abs(updJointAngleAtPassiveTauMax-jointAngleAtPassiveTauMax-M_PI/3.0)
< SQRTEPSILON);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax+M_PI/3.0,
0.-SQRTEPSILON,
activation,
tmi1);
CHECK( abs(tmi0.fiberPassiveTorqueAngleMultiplier
-tmi1.fiberPassiveTorqueAngleMultiplier) < SQRTEPSILON);
//fitPassiveCurveAngleOffset: Extension test
tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveCurveAngleOffset(1.0,
tauMax);
tq.calcTorqueMuscleInfo(1.0,
0.,
0.,
tmi0);
CHECK(abs(tmi0.fiberPassiveTorque - tauMax) < SQRTEPSILON);
//fitPassiveCurveAngleOffset: flexion test
Millard2016TorqueMuscle tqF =
Millard2016TorqueMuscle(DataSet::Anderson2007,
subjectInfo,
Anderson2007::HipFlexion,
jointAngleOffset,
signOfJointAngle,
-1*signOfJointTorque,
"flexion");
tauMax = tqF.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax =
tqF.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tqF.fitPassiveCurveAngleOffset(-1.0, tauMax);
tqF.calcTorqueMuscleInfo(-1.0,
0.,
0.,
tmi0);
CHECK(abs(tmi0.fiberPassiveTorque - tauMax) < SQRTEPSILON);
tauMax = tq.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tq.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tq.fitPassiveTorqueScale(jointAngleAtPassiveTauMax, tauMax*0.5);
tq.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
0.,
tmi0);
CHECK(abs(tmi0.fiberPassiveTorque - tauMax*0.5) < SQRTEPSILON);
//Now for the flexor ...
tauMax = tqF.getMaximumActiveIsometricTorque();
jointAngleAtPassiveTauMax
= tqF.getJointAngleAtOneNormalizedPassiveIsometricTorque();
tqF.fitPassiveTorqueScale(jointAngleAtPassiveTauMax, tauMax*0.5);
tqF.calcTorqueMuscleInfo(jointAngleAtPassiveTauMax,
0.,
0.,
tmi0);
CHECK(abs(tmi0.fiberPassiveTorque - tauMax*0.5) < SQRTEPSILON);
//Test the damping term
double beta = tq.getNormalizedDampingCoefficient();
tq.setNormalizedDampingCoefficient(beta+0.1);
CHECK(abs(beta+0.1-tq.getNormalizedDampingCoefficient())<SQRTEPSILON);
double omegaMax = tq.getMaximumJointAngularVelocity();
double tau = tq.calcJointTorque(0., omegaMax,0);
CHECK(abs(tau) < SQRTEPSILON );
tq.calcTorqueMuscleInfo(0.,omegaMax,0.1,tmi0);
err = abs(tmi0.activation
*tmi0.fiberActiveTorqueAngleMultiplier
*tmi0.fiberActiveTorqueAngularVelocityMultiplier
+tmi0.fiberPassiveTorqueAngleMultiplier
*tq.getPassiveTorqueScale()
+tmi0.fiberNormDampingTorque);
CHECK( err < SQRTEPSILON);
beta = tq.getNormalizedDampingCoefficient();
tauMax = tq.getMaximumActiveIsometricTorque();
tq.calcTorqueMuscleInfo(0.,-omegaMax,0,tmi0);
CHECK( abs(tmi0.fiberDampingTorque - beta*1*tauMax) < SQRTEPSILON );
CHECK( abs(tmi0.fiberNormDampingTorque -beta*1) < SQRTEPSILON );
}
TEST(exampleUsage){
bool printCurves = true;
bool printAllCurves = false;
//int dataSet = DataSetAnderson2007;
//int gender = 0; //male
//int age = 0; //young
double jointAngleOffset = 0;
double signOfJointAngle = 1;
double signOfJointTorque = 1;
std::string name("test");
SubjectInformation subjectInfo;
subjectInfo.gender = GenderSet::Male;
subjectInfo.ageGroup = AgeGroupSet::Young18To25;
subjectInfo.heightInMeters = 1.732;
subjectInfo.massInKg = 69.0;
std::vector< Millard2016TorqueMuscle > 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]
<<Anderson2007::JointTorqueNames[i];
tqMuscle = Millard2016TorqueMuscle(
DataSet::Anderson2007,
subjectInfo,
Anderson2007::JointTorque(i),
0.0,
1.0,
1.0,
tqName.str() );
if(printCurves)
tqMuscle.printJointTorqueProfileToFile("",tqMuscle.getName() ,100);
}
for(int i=0; i < Gymnast::LastJointTorque; ++i){
tqName.str("");
tqName << DataSet.names[1]
<< Gymnast::JointTorqueNames[i];
tqMuscle = Millard2016TorqueMuscle(
DataSet::Gymnast,
subjectInfo,
Gymnast::JointTorque(i),
0.0,
1.0,
1.0,
tqName.str() );
if(printCurves)
tqMuscle.printJointTorqueProfileToFile("",tqMuscle.getName() ,100);
}
tqIdx = -1;
if(printAllCurves){
std::stringstream muscleName;
Millard2016TorqueMuscle muscle;
int genderIdx,ageIdx,tqIdx;
for(int age =0; age < Anderson2007::LastAgeGroup; ++age){
for(int gender=0; gender < Anderson2007::LastGender; ++gender){
for( int tqDir = 0; tqDir < Anderson2007::LastJointTorque; ++tqDir){
//for(int joint=0; joint < 3; ++joint){
// for(int dir = 0; dir < 2; ++dir){
muscleName.str(std::string());
genderIdx = Anderson2007::Gender(gender);
ageIdx = Anderson2007::AgeGroup(age);
tqIdx = Anderson2007::JointTorque(tqDir);
muscleName << "Anderson2007_"
<< AgeGroupSet::names[ageIdx]
<< "_"
<< GenderSet::names[genderIdx]
<< "_"
<< JointTorqueSet::names[tqIdx];
subjectInfo.ageGroup = AgeGroupSet::item(age);
subjectInfo.gender = GenderSet::item(gender);
muscle = Millard2016TorqueMuscle(
DataSet::Anderson2007,
subjectInfo,
Anderson2007::JointTorque(tqDir),
0,
1.0,
1.0,
muscleName.str());
const SmoothSegmentedFunction &tp
= muscle.getPassiveTorqueAngleCurve();
const SmoothSegmentedFunction &ta
= muscle.getActiveTorqueAngleCurve();
const SmoothSegmentedFunction &tv
= muscle.getTorqueAngularVelocityCurve();
RigidBodyDynamics::Math::VectorNd tpDomain
= tp.getCurveDomain();
RigidBodyDynamics::Math::VectorNd tvDomain
= tv.getCurveDomain();
RigidBodyDynamics::Math::VectorNd taDomain
= ta.getCurveDomain();
tp.printCurveToCSVFile(
"",
tp.getName(),
tpDomain[0]-0.1*(tpDomain[1]-tpDomain[0]),
tpDomain[1]+0.1*(tpDomain[1]-tpDomain[0]));
tv.printCurveToCSVFile(
"",
tv.getName(),
tvDomain[0]-0.1*(tvDomain[1]-tvDomain[0]),
tvDomain[1]+0.1*(tvDomain[1]-tvDomain[0]));
ta.printCurveToCSVFile(
"",
ta.getName(),
taDomain[0]-0.1*(taDomain[1]-taDomain[0]),
taDomain[1]+0.1*(taDomain[1]-taDomain[0]));
//}
//}
}
}
}
}
//catch(...){
// exceptionThrown = true;
// }
CHECK(true);
}
int main (int argc, char *argv[])
{
return UnitTest::RunAllTests ();
}

692
3rdparty/rbdl/addons/muscle/tests/testMuscleFunctionFactory.cc vendored Normal file
View File

@ -0,0 +1,692 @@
/* -------------------------------------------------------------------------- *
* 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 MuscleFunctionFactory
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/geometry.h"
#include "../MuscleFunctionFactory.h"
#include "../../geometry/tests/numericalTestFunctions.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
using namespace RigidBodyDynamics::Addons::Geometry;
using namespace RigidBodyDynamics::Addons::Muscle;
using namespace std;
//using namespace OpenSim;
//using namespace SimTK;
/*
static double EPSILON = numeric_limits<double>::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 <<"**************************************************"<<endl;
//cout <<"TENDON CURVE TESTING "<<endl;
double e0 = 0.04;
double kiso = 1.5/e0;
double c = 0.5;//0.75;
double ftoe = 1.0/3.0;
SmoothSegmentedFunction tendonCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::createTendonForceLengthCurve(
e0,kiso,ftoe,c, "test_tendonCurve", tendonCurve);
RigidBodyDynamics::Math::MatrixNd tendonCurveSample
=tendonCurve.calcSampledCurve(6,1.0,1+e0);
//tendonCurve.printMuscleCurveToCSVFile(FILE_PATH);
//0. Test that each curve fulfills its contract at the end points.
//cout << " Keypoint Testing" << endl;
RigidBodyDynamics::Math::VectorNd tendonCurveDomain =
tendonCurve.getCurveDomain();
CHECK(abs(tendonCurve.calcValue(tendonCurveDomain[0]))<TOL_SMALL);
CHECK(abs(tendonCurve.calcValue(tendonCurveDomain[1])-ftoe)<TOL_SMALL);
CHECK(abs(tendonCurve.calcValue(1.0) )< TOL_SMALL);
CHECK(abs(tendonCurve.calcDerivative(1.0,1))< TOL_BIG);
CHECK(abs(tendonCurve.calcDerivative(1.0,2))< TOL_BIG);
CHECK(abs(tendonCurve.calcValue(1+e0) -1.0 ) < TOL_SMALL);
CHECK(abs(tendonCurve.calcDerivative(1+e0,1)-kiso) < TOL_BIG);
CHECK(abs(tendonCurve.calcDerivative(1+e0,2)-0 ) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
tendonCurve,
tendonCurveSample,TOL_DX_BIG);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(tendonCurve, tendonCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(tendonCurve,
tendonCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(tendonCurveSample);
CHECK(curveIsMonotonic);
if(FLAG_PLOT_CURVES){
tendonCurve.printCurveToCSVFile(FILE_PATH,
"tendonCurve",
1.0-(e0/10),
1+e0);
}
//cout << " passed" << endl;
}
TEST(activeForceLengthCurve)
{
//cout << endl;
//cout << endl;
//cout <<"**************************************************"<<endl;
//cout <<"FIBER ACTIVE FORCE LENGTH CURVE TESTING "<<endl;
double lce0 = 0.4;
double lce1 = 0.75;
double lce2 = 1;
double lce3 = 1.6;
double shoulderVal = 0.05;
double plateauSlope = 0.75;//0.75;
double curviness = 0.75;
SmoothSegmentedFunction fiberfalCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberActiveForceLengthCurve(lce0, lce1, lce2, lce3,
shoulderVal, plateauSlope, curviness,
"test_fiberActiveForceLengthCurve", fiberfalCurve);
//fiberfalCurve.printMuscleCurveToCSVFile(FILE_PATH);
RigidBodyDynamics::Math::MatrixNd fiberfalCurveSample
= fiberfalCurve.calcSampledCurve(6,0,lce3);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
CHECK(abs(fiberfalCurve.calcValue(lce0) - shoulderVal) < TOL_SMALL);
CHECK(abs(fiberfalCurve.calcDerivative(lce0,1)) < TOL_BIG);
CHECK(abs(fiberfalCurve.calcDerivative(lce0,2)) < TOL_BIG);
//lce2 isn't the location of the end of a quintic Bezier curve
//so I can't actually do any testing on this point.
//SimTK_TEST_EQ_TOL(fiberfalCurve.calcValue(lce0),shoulderVal,TOL_SMALL);
//SimTK_TEST_EQ_TOL(
//fiberfalCurve.calcDerivative(lce2,1),plateauSlope,TOL_BIG);
//SimTK_TEST_EQ_TOL(fiberfalCurve.calcDerivative(lce2,2),0.0,TOL_BIG);
CHECK(abs(fiberfalCurve.calcValue(lce2) - 1.0) < TOL_SMALL);
CHECK(abs(fiberfalCurve.calcDerivative(lce2,1)) < TOL_BIG);
CHECK(abs(fiberfalCurve.calcDerivative(lce2,2)) < TOL_BIG);
CHECK(abs(fiberfalCurve.calcValue(lce3)-shoulderVal) < TOL_SMALL);
CHECK(abs(fiberfalCurve.calcDerivative(lce3,1)) < TOL_BIG);
CHECK(abs(fiberfalCurve.calcDerivative(lce3,2)) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberfalCurve,
fiberfalCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberfalCurve,fiberfalCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(fiberfalCurve,
fiberfalCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//fiberfalCurve.MuscleCurveToCSVFile("C:/mjhmilla/Stanford/dev");
if(FLAG_PLOT_CURVES){
fiberfalCurve.printCurveToCSVFile(FILE_PATH,
"fiberFalCurve",
0.3,
1.8);
}
}
TEST(ForceVelocityCurve)
{
//cout <<"**************************************************"<<endl;
//cout <<"FIBER FORCE VELOCITY CURVE TESTING "<<endl;
double fmaxE = 1.8;
double dydxC = 0.1;
double dydxNearC = 0.15;
double dydxE = 0.1;
double dydxNearE = 0.1+0.0001;
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_fiberForceVelocityCurve",
fiberFVCurve);
//fiberFVCurve.printMuscleCurveToCSVFile(FILE_PATH);
RigidBodyDynamics::Math::MatrixNd fiberFVCurveSample
= fiberFVCurve.calcSampledCurve(6,-1.0,1.0);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
assert(abs(fiberFVCurve.calcValue(-1) ) < TOL_SMALL);
assert(abs(fiberFVCurve.calcDerivative(-1,1)-dydxC ) < TOL_BIG );
assert(abs(fiberFVCurve.calcDerivative(-1,2) ) < TOL_BIG );
assert(abs(fiberFVCurve.calcValue(0) -1.0 ) < TOL_SMALL);
assert(abs(fiberFVCurve.calcDerivative(0,1)-dydxIso ) < TOL_BIG );
assert(abs(fiberFVCurve.calcDerivative(0,2) ) < TOL_BIG );
assert(abs(fiberFVCurve.calcValue(1) -fmaxE ) < TOL_SMALL);
assert(abs(fiberFVCurve.calcDerivative(1,1)-dydxE ) < TOL_BIG );
assert(abs(fiberFVCurve.calcDerivative(1,2) ) < TOL_BIG );
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberFVCurve,
fiberFVCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberFVCurve,fiberFVCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(
fiberFVCurve,
fiberFVCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
isCurveMontonic(fiberFVCurveSample);
CHECK(curveIsContinuous);
if(FLAG_PLOT_CURVES){
fiberFVCurve.printCurveToCSVFile(FILE_PATH,
"fiberFvCurve",
-1.1,
1.1);
}
//cout << " passed" << endl;
}
TEST(ForceVelocityInverseCurve)
{
//cout <<"**************************************************"<<endl;
//cout <<"FIBER FORCE VELOCITY INVERSE CURVE TESTING "<<endl;
double fmaxE = 1.8;
double dydxC = 0.1;
double dydxNearC = 0.15;
double dydxE = 0.1;
double dydxNearE = 0.1+0.0001;
double dydxIso = 5;
double concCurviness= 0.1;
double eccCurviness = 0.75;
SmoothSegmentedFunction fiberFVInvCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberForceVelocityInverseCurve(
fmaxE,
dydxC,
dydxNearC,
dydxIso,
dydxE,
dydxNearE,
concCurviness,
eccCurviness,
"test_fiberForceVelocityInverseCurve",
fiberFVInvCurve);
SmoothSegmentedFunction fiberFVCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberForceVelocityCurve(
fmaxE,
dydxC,
dydxNearC,
dydxIso,
dydxE,
dydxNearE,
concCurviness,
eccCurviness,
"test_fiberForceVelocityCurve",
fiberFVCurve);
//fiberFVInvCurve.printMuscleCurveToFile(FILE_PATH);
RigidBodyDynamics::Math::MatrixNd fiberFVCurveSample
= fiberFVCurve.calcSampledCurve(6, -1.0, 1.0);
RigidBodyDynamics::Math::MatrixNd fiberFVInvCurveSample
= fiberFVInvCurve.calcSampledCurve(6,0,fmaxE);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
CHECK(abs( fiberFVInvCurve.calcValue(0) +1 ) < TOL_SMALL);
CHECK(abs( fiberFVInvCurve.calcDerivative(0,1)-1/dydxC ) < TOL_BIG);
CHECK(abs( fiberFVInvCurve.calcDerivative(0,2) ) < TOL_BIG);
CHECK(abs( fiberFVInvCurve.calcValue(1) ) < TOL_SMALL);
CHECK(abs( fiberFVInvCurve.calcDerivative(1,1)-1/dydxIso ) < TOL_BIG);
CHECK(abs( fiberFVInvCurve.calcDerivative(1,2) ) < TOL_BIG);
CHECK(abs( fiberFVInvCurve.calcValue(fmaxE) -1 ) < TOL_SMALL);
CHECK(abs( fiberFVInvCurve.calcDerivative(fmaxE,1)-1/dydxE) < TOL_BIG);
CHECK(abs( fiberFVInvCurve.calcDerivative(fmaxE,2) ) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberFVInvCurve,
fiberFVInvCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberFVInvCurve,fiberFVInvCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(
fiberFVInvCurve,
fiberFVInvCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(fiberFVInvCurveSample);
CHECK(curveIsMonotonic);
//5. Testing the inverse of the curve - is it really an inverse?
//cout << endl;
//cout << " TEST: Inverse correctness:fv(fvinv(fv)) = fv" << endl;
double fv = 0;
double dlce = 0;
double fvCalc = 0;
double fvErr = 0;
double fvErrMax = 0;
for(int i = 0; i < fiberFVInvCurveSample.rows(); i++){
fv = fiberFVCurveSample(i,0);
dlce = fiberFVInvCurve.calcValue(fv);
fvCalc = fiberFVCurve.calcValue(dlce);
fvErr = abs(fv-fvCalc);
if(fvErrMax < fvErr)
fvErrMax = fvErr;
CHECK( fvErr < TOL_BIG);
}
if(FLAG_PLOT_CURVES){
fiberFVInvCurve.printCurveToCSVFile(FILE_PATH,
"fiberFvInvCurve",
-0.1,
fmaxE+0.1);
}
//printf(" passed with a maximum error of %fe-12",fvErrMax*1e12);
}
TEST(passiveForceLengthCurve)
{
double e0f = 0.6;
double kisof = 8.389863790885878;
double cf = 0.65;
double klow = 0.5*(1.0/e0f);
SmoothSegmentedFunction fiberFLCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberForceLengthCurve(
0.0, e0f,klow,kisof,cf,
"test_fiberForceLength",
fiberFLCurve);
RigidBodyDynamics::Math::MatrixNd fiberFLCurveSample
= fiberFLCurve.calcSampledCurve(6,1.0,1.0+e0f);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
RigidBodyDynamics::Math::VectorNd fiberFLCurveDomain
= fiberFLCurve.getCurveDomain();
CHECK(abs(fiberFLCurveDomain[0] -1) < TOL_SMALL);
CHECK(abs(fiberFLCurveDomain[1] - (1+e0f)) < TOL_SMALL);
CHECK(abs(fiberFLCurve.calcValue(1.0) ) < TOL_SMALL);
CHECK(abs(fiberFLCurve.calcDerivative(1.0,1)) < TOL_BIG);
CHECK(abs(fiberFLCurve.calcDerivative(1.0,2)) < TOL_BIG);
CHECK(abs(fiberFLCurve.calcValue(1+e0f) -1.0) < TOL_SMALL);
CHECK(abs(fiberFLCurve.calcDerivative(1+e0f,1)-kisof) < TOL_BIG);
CHECK(abs(fiberFLCurve.calcDerivative(1+e0f,2)) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberFLCurve,
fiberFLCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberFLCurve,fiberFLCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(
fiberFLCurve,
fiberFLCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(fiberFLCurveSample);
CHECK(curveIsMonotonic);
if(FLAG_PLOT_CURVES){
fiberFLCurve.printCurveToCSVFile(FILE_PATH,
"fiberFLCurve",
1.0-0.1,
1.0+e0f+0.1);
}
}
TEST(compressiveForceLengthCurve)
{
///////////////////////////////////////
//FIBER COMPRESSIVE FORCE LENGTH
///////////////////////////////////////
//cout <<"**************************************************"<<endl;
//cout <<"FIBER COMPRESSIVE FORCE LENGTH CURVE TESTING "<<endl;
double lmax = 0.6;
double kce = -8.389863790885878;
double cce = 0.5;//0.0;
SmoothSegmentedFunction fiberCECurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberCompressiveForceLengthCurve(
lmax,
kce,
cce,
"test_fiberCompressiveForceLengthCurve",
fiberCECurve);
//fiberCECurve.printMuscleCurveToFile("C:/mjhmilla/Stanford/dev"
// "/OpenSim_LOCALPROJECTS/MuscleLibrary_Bench_20120210/build");
RigidBodyDynamics::Math::MatrixNd fiberCECurveSample
= fiberCECurve.calcSampledCurve(6,0,lmax);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
RigidBodyDynamics::Math::VectorNd fiberCECurveDomain
= fiberCECurve.getCurveDomain();
CHECK(abs(fiberCECurveDomain[0]) < TOL_SMALL);
CHECK(abs(fiberCECurveDomain[1]- lmax) < TOL_SMALL);
CHECK(abs(fiberCECurve.calcValue(lmax)) < TOL_SMALL);
CHECK(abs(fiberCECurve.calcDerivative(lmax,1)) < TOL_BIG);
CHECK(abs(fiberCECurve.calcDerivative(lmax,2)) < TOL_BIG);
CHECK(abs(fiberCECurve.calcValue(0) - 1.0) < TOL_SMALL);
CHECK(abs(fiberCECurve.calcDerivative(0,1)-kce) < TOL_BIG);
CHECK(abs(fiberCECurve.calcDerivative(0,2)) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberCECurve,
fiberCECurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberCECurve,fiberCECurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(
fiberCECurve,
fiberCECurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(fiberCECurveSample);
CHECK(curveIsMonotonic);
//5. Testing Exceptions
//cout << endl;
//cout << " Exception Testing" << endl;
if(FLAG_PLOT_CURVES){
fiberCECurve.printCurveToCSVFile(FILE_PATH,
"fiberCECurve",
-0.1,
lmax + 0.1);
}
//cout << " passed" << endl;
}
TEST(compressivePhiCurve)
{
//cout <<"**************************************************"<<endl;
//cout <<"FIBER COMPRESSIVE FORCE PHI CURVE TESTING "<<endl;
double phi0 = (M_PI/2)*(1.0/2.0);
double phi1 = M_PI/2;
double kphi = 8.389863790885878;
double cphi = 0.0;
SmoothSegmentedFunction fiberCEPhiCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberCompressiveForcePennationCurve(phi0,kphi,cphi,
"test_fiberCompressiveForcePennationCurve", fiberCEPhiCurve);
RigidBodyDynamics::Math::MatrixNd fiberCEPhiCurveSample
= fiberCEPhiCurve.calcSampledCurve(6,phi0,phi1);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
CHECK(abs(fiberCEPhiCurve.calcValue(phi0)) < TOL_SMALL);
CHECK(abs(fiberCEPhiCurve.calcDerivative(phi0,1)) < TOL_BIG);
CHECK(abs(fiberCEPhiCurve.calcDerivative(phi0,2)) < TOL_BIG);
CHECK(abs(fiberCEPhiCurve.calcValue(phi1)) -1 < TOL_SMALL);
CHECK(abs(fiberCEPhiCurve.calcDerivative(phi1,1)-kphi) < TOL_BIG);
CHECK(abs(fiberCEPhiCurve.calcDerivative(phi1,2)) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberCEPhiCurve,
fiberCEPhiCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberCEPhiCurve,fiberCEPhiCurveSample);
//3. Test numerically to see if the curve is C2 continuous
//cout << "Attention: Removed test for the Cos Phi Compressive Curve"<<endl;
bool curveIsContinuous = isCurveC2Continuous(
fiberCEPhiCurve,
fiberCEPhiCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(fiberCEPhiCurveSample);
CHECK(curveIsMonotonic);
if(FLAG_PLOT_CURVES){
fiberCEPhiCurve.printCurveToCSVFile(FILE_PATH,
"fiberCEPhiCurve",
phi0-0.1,
phi1+0.1);
}
//cout << " passed" << endl;
}
TEST(compressiveCosPhiCurve)
{
//cout <<"**************************************************"<<endl;
//cout <<"FIBER COMPRESSIVE FORCE COSPHI CURVE TESTING "<<endl;
double cosPhi0 = cos( (80.0/90.0)*M_PI/2);
double kcosPhi = -1.2/(cosPhi0);
double ccosPhi = 0.5;
SmoothSegmentedFunction fiberCECosPhiCurve = SmoothSegmentedFunction();
MuscleFunctionFactory::
createFiberCompressiveForceCosPennationCurve(
cosPhi0,kcosPhi,ccosPhi,
"test_fiberCompressiveForceCosPennationCurve",
fiberCECosPhiCurve);
RigidBodyDynamics::Math::MatrixNd fiberCECosPhiCurveSample
= fiberCECosPhiCurve.calcSampledCurve(6,0,cosPhi0);
//0. Test that each curve fulfills its contract.
//cout << " Keypoint Testing" << endl;
CHECK(abs(fiberCECosPhiCurve.calcValue(cosPhi0) ) < TOL_SMALL);
CHECK(abs(fiberCECosPhiCurve.calcDerivative(cosPhi0,1) ) < TOL_BIG);
CHECK(abs(fiberCECosPhiCurve.calcDerivative(cosPhi0,2) ) < TOL_BIG);
CHECK(abs(fiberCECosPhiCurve.calcValue(0) - 1.0 ) < TOL_SMALL);
CHECK(abs(fiberCECosPhiCurve.calcDerivative(0,1) -kcosPhi) < TOL_BIG);
CHECK(abs(fiberCECosPhiCurve.calcDerivative(0,2) ) < TOL_BIG);
//cout << " passed" << endl;
//cout << endl;
//1. Test each derivative sample for correctness against a numerically
// computed version
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
fiberCECosPhiCurve,
fiberCECosPhiCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
//testMuscleCurveIntegral(fiberCECosPhiCurve,fiberCECosPhiCurveSample);
//3. Test numerically to see if the curve is C2 continuous
bool curveIsContinuous = isCurveC2Continuous(
fiberCECosPhiCurve,
fiberCECosPhiCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
bool curveIsMonotonic = isCurveMontonic(fiberCECosPhiCurveSample);
CHECK(curveIsMonotonic);
//cout << " passed" << endl;
if(FLAG_PLOT_CURVES){
fiberCECosPhiCurve.printCurveToCSVFile(FILE_PATH,
"fiberCECosPhiCurve",
-0.1,
cosPhi0+0.1);
}
}

779
3rdparty/rbdl/addons/muscle/tests/testTorqueMuscleFunctionFactory.cc vendored Normal file
View File

@ -0,0 +1,779 @@
/* -------------------------------------------------------------------------- *
* 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 "../TorqueMuscleFunctionFactory.h"
#include "../../geometry/geometry.h"
#include "../../geometry/tests/numericalTestFunctions.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
using namespace RigidBodyDynamics::Addons::Geometry;
using namespace RigidBodyDynamics::Addons::Muscle;
using namespace std;
/*
static double EPSILON = numeric_limits<double>::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 <<endl;
//cout <<endl;
//cout <<"**************************************************"<<endl;
//cout <<"ANDERSON 2007 ACTIVE TORQUE ANGLE CURVE TESTING "<<endl;
SmoothSegmentedFunction andersonTaCurve = SmoothSegmentedFunction();
TorqueMuscleFunctionFactory::
createAnderson2007ActiveTorqueAngleCurve(c2,c3,
"test_Anderson2007TorqueAngleCurve",
andersonTaCurve);
double angleRange = (M_PI/c4);
double angleActiveMin = c3 - angleRange*0.75;
double angleActiveMax = c3 + angleRange*0.75;
RigidBodyDynamics::Math::MatrixNd andersonTaCurveSample
= andersonTaCurve.calcSampledCurve( 6,
angleActiveMin,
angleActiveMax);
//cout << " Keypoint Testing" << endl;
CHECK(abs(andersonTaCurve.calcValue(c3) - 1.0) < TOL_SMALL);
CHECK(abs(andersonTaCurve.calcDerivative(c3,1)) < TOL_BIG);
CHECK(abs(andersonTaCurve.calcDerivative(c3,2)) < TOL_BIG);
RigidBodyDynamics::Math::VectorNd curveDomain
= andersonTaCurve.getCurveDomain();
CHECK(abs(andersonTaCurve.calcValue(curveDomain[0])) < TOL_SMALL);
CHECK(abs(andersonTaCurve.calcDerivative(curveDomain[0],1)) < TOL_BIG);
CHECK(abs(andersonTaCurve.calcValue(curveDomain[1])) < TOL_SMALL);
CHECK(abs(andersonTaCurve.calcDerivative(curveDomain[1],1)) < TOL_BIG);
//cout << " passed " << endl;
//cout << " Continuity and Smoothness Testing" << endl;
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
andersonTaCurve,
andersonTaCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
bool curveIsContinuous = isCurveC2Continuous( andersonTaCurve,
andersonTaCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
if(FLAG_PLOT_CURVES){
andersonTaCurve.printCurveToCSVFile(
FILE_PATH,
"anderson2007ActiveTorqueAngleCurve",
angleActiveMin,
angleActiveMax);
}
}
TEST(Anderson2007PassiveTorqueAngleCurve)
{
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 <<endl;
//cout <<endl;
//cout <<"**************************************************"<<endl;
//cout <<"ANDERSON 2007 PASSIVE TORQUE ANGLE CURVE TESTING "<<endl;
double curveSign = 1.0;
for(int z = 0; z<2; ++z){
if(z == 0){
curveSign = 1.0;
//cout <<" TESTING SIDE 1"<<endl;
}else{
curveSign = -1.0;
//cout <<" TESTING SIDE 2"<<endl;
}
SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction();
TorqueMuscleFunctionFactory::
createAnderson2007PassiveTorqueAngleCurve(
scale,
c1,
b1*curveSign,
k1,
b2*curveSign,
k2,
"test_passiveTorqueAngleCurve",
andersonTpCurve);
RigidBodyDynamics::Math::VectorNd curveDomain
= andersonTpCurve.getCurveDomain();
double angleMin = curveDomain[0];
double angleMax = curveDomain[1];
RigidBodyDynamics::Math::MatrixNd andersonTpCurveSample
= andersonTpCurve.calcSampledCurve( 6,
angleMin-0.1,
angleMax+0.1);
//cout << " Keypoint Testing" << endl;
double tauMin = andersonTpCurve.calcValue(angleMin);
double tauMax = andersonTpCurve.calcValue(angleMax);
double tauMinAngle = angleMin;
if(tauMin > 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 <<endl;
//cout <<endl;
//cout <<"**************************************************"<<endl;
//cout <<"ANDERSON 2007 ACTIVE TORQUE VELOCITY CURVE TESTING"<<endl;
double minEccentricMultiplier = 1.1;
double maxEccentricMultiplier = 1.4;
SmoothSegmentedFunction andersonTvCurve = SmoothSegmentedFunction();
TorqueMuscleFunctionFactory::
createAnderson2007ActiveTorqueVelocityCurve(
c4,c5,c6,minEccentricMultiplier,maxEccentricMultiplier,
"test_anderson2007ActiveTorqueVelocityCurve",
andersonTvCurve);
RigidBodyDynamics::Math::VectorNd curveDomain
= andersonTvCurve.getCurveDomain();
double angularVelocityMin = curveDomain[0];
double angularVelocityMax = curveDomain[1];
RigidBodyDynamics::Math::MatrixNd andersonTvCurveSample
= andersonTvCurve.calcSampledCurve( 6,
angularVelocityMin-0.1,
angularVelocityMax+0.1);
//cout << " Keypoint Testing" << endl;
CHECK(abs(andersonTvCurve.calcValue(0) - 1.0) < TOL_SMALL);
//CHECK(abs(andersonTvCurve.calcValue(c4) - 0.75) < TOL_BIG);
//CHECK(abs(andersonTvCurve.calcValue(c5) - 0.5) < TOL_BIG);
double val = abs(andersonTvCurve.calcValue(
angularVelocityMax/angularVelocityMax));
CHECK(val < TOL_BIG);
double maxTv = andersonTvCurve.calcValue(
angularVelocityMin/angularVelocityMax);
CHECK(maxTv >= minEccentricMultiplier);
CHECK(maxTv <= maxEccentricMultiplier);
CHECK(abs(andersonTvCurve.calcDerivative
(angularVelocityMax/angularVelocityMax,1))<TOL_SMALL);
CHECK(abs(andersonTvCurve.calcDerivative
(angularVelocityMin/angularVelocityMax,1))<TOL_SMALL);
//cout << " passed " << endl;
//cout << " Continuity and Smoothness Testing" << endl;
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
andersonTvCurve,
andersonTvCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
bool curveIsContinuous = isCurveC2Continuous( andersonTvCurve,
andersonTvCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
bool curveIsMonotonic = isCurveMontonic(andersonTvCurveSample);
CHECK(curveIsMonotonic);
if(FLAG_PLOT_CURVES){
andersonTvCurve.printCurveToCSVFile(
FILE_PATH,
"anderson2007ActiveTorqueVelocityCurve",
angularVelocityMin,
angularVelocityMax);
}
}
//==============================================================================
TEST(TorqueAngularVelocityCurve)
{
SmoothSegmentedFunction tv = SmoothSegmentedFunction();
SmoothSegmentedFunction tvX = SmoothSegmentedFunction();
double tvAtEccentricOmegaMax = 1.3;
double tvAtHalfConcentricOmegaMax = 0.3;
std::string curveName0("tvTest0");
std::string curveName1("tvTest1");
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
tvAtEccentricOmegaMax,
tvAtHalfConcentricOmegaMax,
curveName0,
tv);
double slopeAtConcentricOmegaMax = -0.1;
double slopeAtEccentricOmegaMax = -0.1;
double slopeNearEccentricOmegaMax = -0.01;
double eccentricCurviness = 0.75;
TorqueMuscleFunctionFactory::createTorqueVelocityCurve(
tvAtEccentricOmegaMax,
tvAtHalfConcentricOmegaMax,
slopeAtConcentricOmegaMax,
slopeNearEccentricOmegaMax,
slopeAtEccentricOmegaMax,
eccentricCurviness,
curveName1,
tvX);
double wmin = -1.1;
double wmax = 1.1;
int npts = 100;
double delta = (wmax-wmin)/((double)npts-1.0);
//Get the parameters for the Hill type curve
double wmaxC = 1.0;
double wmaxE = -1.0;
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 tvHill = 0;
double errHill = 0;
for(int i=0; i<npts; ++i){
w = wmin + i*delta;
if(w > 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);
}
}

88
3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake vendored Normal file
View File

@ -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}
)

369
3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake vendored Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
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)

92
3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake vendored Normal file
View File

@ -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()

28
3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake vendored Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
# 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)

116
3rdparty/rbdl/addons/urdfreader/CMakeLists.txt vendored Normal file
View File

@ -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
)

50
3rdparty/rbdl/addons/urdfreader/README.md vendored Normal file
View File

@ -0,0 +1,50 @@
urdfreader - load models from (URDF Unified Robot Description Format) files
Copyright (c) 2012 Martin Felis <martin@fysx.org>
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 <martin@fysx.org>
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.

69
3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc vendored Normal file
View File

@ -0,0 +1,69 @@
#include <rbdl/rbdl.h>
#include <rbdl/rbdl_utils.h>
#include "urdfreader.h"
#include <iostream>
using namespace std;
bool verbose = false;
bool floatbase = false;
string filename = "";
void usage (const char* argv_0) {
cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <robot.urdf>" << 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;
}

5
3rdparty/rbdl/addons/urdfreader/thirdparty/README.md vendored Normal file
View File

@ -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/

299
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt vendored Normal file
View File

@ -0,0 +1,299 @@
Changes in version 1.0.1:
- Fixed comment tags which were outputing as '<?--' instead of
the correct '<!--'.
- Implemented the Next and Prev methods of the TiXmlAttribute class.
- Renamed 'LastAttribtute' to 'LastAttribute'
- Fixed bad pointer to 'isspace' that could occur while parsing text.
- Errors finding beginning and end of tags no longer throw it into an
infinite loop. (Hopefully.)
Changes in version 1.0.2
- Minor documentation fixes.
Changes in version 1.0.3
- After nodes are added to a document, they return a pointer
to the new node instead of a bool for success.
- Elements can be constructed with a value, which is the
element name. Every element must have a value or it will be
invalid, but the code changes to enforce this are not fully
in place.
Changes in version 1.1.0
- Added the TiXmlAttributeSet class to pull the attributes into
a seperate container.
- Moved the doubly liked list out of XmlBase. Now XmlBase only
requires the Print() function and defines some utility functions.
- Moved errors into a seperate file. (With the idea of internationalization
to the other latin-1 languages.)
- Added the "NodeType"
- Fixed white space parsing in text to conform with the standard.
Basically, all white space becomes just one space.
- Added the TiXmlDeclaration class to read xml declarations.
Changes in version 1.2.0
- Removed the factory. The factory was not really in the spirit
of small and simple, confused the code, and was of limited value.
- Added FirstChildElement and NextSiblingElement, because they
are such common functions.
- Re-wrote the example to test and demonstrate more functionality.
Changes in version 1.2.1
- Fixed a bug where comments couldn't be inside elements.
- Loading now clears out existing XML rather than appending.
- Added the "Clear" method on a node to delete all its children.
Changes in version 1.2.2
- Fixed TiXmlAttribute::Previous actually returning "next." Thanks
to Rickard Troedsson for the bug fix.
Changes in version 1.2.3
- Added the TIXML prefix to the error strings to resolve conflicts
with #defines in OS headers. Thanks to Steve Lhomme.
- Fixed a delete buf that should be a delete [] buf.
Thanks to Ephi Sinowitz.
Changes in version 1.2.4
- ReplaceChild() was almost guarenteed to fail. Should be fixed,
thanks to Joe Smith. Joe also pointed out that the Print() functions
should take stream references: I agree, and would like to overload
the Print() method to take either format, but I don't want to do
this in a dot release.
- Some compilers seem to need an extra <ctype.h> 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 <ctype.h> 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 <istream> and <ostream> 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 "<?--" instead of "<!--" (thanks ion_pulse)
- Added the TiXmlHandle. An easy, safe way to browse XML DOMs with less code.
- Added QueryAttribute calls which have better error messaging. (Proposed by Fletcher Dunn)
- Nodes and attributes can now print themselves to strings. (Yves suggestion)
- Fixed bug where entities with one character would confuse parser. (Thanks Roman)
2.2.1
- Additional testing (no more bugs found to be fixed in this release)
- Significant performance improvement to the cursor code.
2.3.0
- User Data are now defined in TiXmlBase instead of TiXmlNode
- Character Entities are now UCS-2
- Character Entities can be decimal or hexadecimal
- UTF-8 conversion.
- Fixed many, many bugs.
2.3.1
- Fixed bug in handling nulls embedded in the input.
- Make UTF-8 parser tolerant of bad text encoding.
- Added encoding detection.
- Many fixes and input from John-Philip Leonard Johansson (JP) and Ellers,
including UTF-8 feedback, bug reports, and patches. Thanks!
- Added version # constants - a suggestion from JP and Ellers.
- [ 979180 ] Missing ; in entity reference, fix from Rob Laveaux.
- Copy constructors and assignment have been a long time coming. Thanks to
Fokke and JP.
2.3.2
- Made the IsAlpha and IsAlphaNum much more tolerant of non-UTF-8 encodings. Thanks
Volker Boerchers for finding the issue.
- Ran the program though the magnificent Valgrind - http://valgrind.kde.org - to check
for memory errors. Fixed some minor issues.
2.3.3
- Fixed crash when test program was run from incorrect directory.
- Fixed bug 1070717 - empty document not returned correctly - thanks Katsuhisa Yuasa.
- Bug 1079301 resolved - deprecated stdlib calls. Thanks Adrian Boeing.
- Bug 1035218 fixed - documentation errors. Xunji Luo
- Other bug fixes have accumulated and been fixed on the way as well; my apologies to
authors not credited!
- Big fix / addition is to correctly return const values. TinyXml could basically
remove const in a method like this: TiXmlElement* Foo() const, where the returned element
was a pointer to internal data. That is now: const TiXmlElement* Foo() const and
TiXmlElement* Foo().
2.3.4
- Fixed additional const errors, thanks Kent Gibson.
- Correctly re-enable warnings after tinyxml header. Thanks Cory Nelson.
- Variety of type cleanup and warning fixes. Thanks Warren Stevens.
- Cleaned up unneeded constructor calls in TinyString - thanks to Geoff Carlton and
the discussion group on sourceforge.
2.4.0
- Improved string class, thanks Tyge Lovset (whose name gets mangled in English - sorry)
- Type cast compiler warning, thanks Rob van den Bogaard
- Added GetText() convenience function. Thanks Ilya Parniuk & Andrew Ellers for input.
- Many thanks to marlonism for finding an infinite loop in bad xml.
- A patch to cleanup warnings from Robert Gebis.
- Added ValueStr() to get the value of a node as a string.
- TiXmlText can now parse and output as CDATA
- Additional string improvement from James (z2895)
- Removed extraneous 'const', thanks David Aldrich
- First pass at switching to the "safe" stdlib functions. Many people have suggested and
pushed on this, but Warren Stevens put together the first proposal.
- TinyXml now will do EOL normalization before parsing, consistent with the W3C XML spec.
- Documents loaded with the UTF-8 BOM will now save with the UTF-8 BOM. Good suggestion
from 'instructor_'
- Ellers submitted his very popular tutorials, which have been added to the distribution.
2.4.1
- Fixed CDATA output formatting
- Fixed memory allocators in TinyString to work with overloaded new/delete
2.4.2
- solosnake pointed out that TIXML_LOG causes problems on an XBOX. The definition in the header
was superflous and was moved inside of DEBUG_PARSING
2.4.3
- Fixed a test bug that caused a crash in 'xmltest'. TinyXML was fine, but it isn't good
to ship with a broken test suite.
- Started converting some functions to not cast between std::string and const char*
quite as often.
- Added FILE* versions of the document loads - good suggestion from Wade Brainerd
- Empty documents might not always return the errors they should. [1398915] Thanks to igor v.
- Added some asserts for multiply adding a node, regardng bug [1391937] suggested by Paco Arjonilla.
2.4.4
- Bug find thanks to andre-gross found a memory leak that occured when a document failed to load.
- Bug find (and good analysis) by VirtualJim who found a case where attribute parsing
should be throwing an error and wasn't.
- Steve Hyatt suggested the QueryValueAttribute method, which is now implemented.
- DavidA identified a chunk of dead code.
- Andrew Baxter sent in some compiler warnings that were good clean up points.
2.5
- Added the Visit() API. Many thanks to both Andrew Ellerton and John-Philip for all their
work, code, suggestion, and just general pushing that it should be done.
- Removed existing streaming code and use TiXmlPrinter instead.
- [ tinyxml-Bugs-1527079 ] Compile error in tinystr.cpp fixed, thanks to Paul Suggs
- [ tinyxml-Bugs-1522890 ] SaveFile has no error checks fixed, thanks to Ivan Dobrokotov
- Ivan Dobrokotov also reported redundant memory allocation in the Attribute() method, which
upon investigation was a mess. The attribute should now be fixed for both const char* and
std::string, and the return types match the input parameters.
- Feature [ 1511105 ] Make TiXmlComment constructor accept a string / char*, implemented.
Thanks to Karl Itschen for the feedback.
- [ 1480108 ] Stream parsing fails when CDATA contains tags was found by Tobias Grimm, who also
submitted a test case and patch. A significant bug in CDATA streaming (operator>>) 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.)

530
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt vendored Normal file
View File

@ -0,0 +1,530 @@
/** @mainpage
<h1> TinyXML </h1>
TinyXML is a simple, small, C++ XML parser that can be easily
integrated into other programs.
<h2> What it does. </h2>
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 <a href="http://www.w3.org/TR/2004/REC-xml-20040204/">
http://www.w3.org/TR/2004/REC-xml-20040204/</a>. An intro to XML
(that I really like) can be found at
<a href="http://skew.org/xml/tutorial/">http://skew.org/xml/tutorial</a>.
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.
<h2> What it doesn't do. </h2>
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
<!DOCTYPE Archiv [
<!ELEMENT Comment (#PCDATA)>
]>
@endverbatim
because TinyXML sees this as a !DOCTYPE node with an illegally
embedded !ELEMENT node. This may be addressed in the future.
<h2> Tutorials. </h2>
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
<h2> Code Status. </h2>
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.
<h2> Related Projects </h2>
TinyXML projects you may find useful! (Descriptions provided by the projects.)
<ul>
<li> <b>TinyXPath</b> (http://tinyxpath.sourceforge.net). TinyXPath is a small footprint
XPath syntax decoder, written in C++.</li>
<li> <b>TinyXML++</b> (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.</li>
</ul>
<h2> Features </h2>
<h3> Using STL </h3>
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.
<h3> UTF-8 </h3>
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:
<ol>
<li> 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. </li>
<li> If the declaration tag is read, and it has an encoding="UTF-8", then
TinyXML will read it as UTF-8. </li>
<li> If the declaration tag is read, and it has no encoding specified, then TinyXML will
read it as UTF-8. </li>
<li> 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.</li>
<li> Until one of the above criteria is met, TinyXML runs in Legacy Mode.</li>
</ol>
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 <a href="http://skew.org/xml/tutorial/">Skew.org link</a> 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.
<h3> Entities </h3>
TinyXML recognizes the pre-defined "character entities", meaning special
characters. Namely:
@verbatim
&amp; &
&lt; <
&gt; >
&quot; "
&apos; '
@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 &amp; 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 "&#xA0;" or "&#160;" are both to the non-breaking space characher.
<h3> Printing </h3>
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.
<h3> Streams </h3>
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.
<h3> White space </h3>
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.
<h3> Handles </h3>
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.
<h3> Row and Column tracking </h3>
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().
<h2> Using and Installing </h2>
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.
<h3>Windows project file for VC6</h3>
<ul>
<li>tinyxml: tinyxml library, non-STL </li>
<li>tinyxmlSTL: tinyxml library, STL </li>
<li>tinyXmlTest: test app, non-STL </li>
<li>tinyXmlTestSTL: test app, STL </li>
</ul>
<h3>Makefile</h3>
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.
<h3>To Use in an Application:</h3>
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.
<h2> How TinyXML works. </h2>
An example is probably the best way to go. Take:
@verbatim
<?xml version="1.0" standalone=no>
<!-- Our to do list data -->
<ToDo>
<Item priority="1"> Go to the <bold>Toy store!</bold></Item>
<Item priority="2"> Do bills</Item>
</ToDo>
@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
<?xml version="1.0" standalone=no>
@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
<!-- Our to do list data -->
@endverbatim
A comment. Will become a TiXmlComment object.
@verbatim
<ToDo>
@endverbatim
The "ToDo" tag defines a TiXmlElement object. This one does not have
any attributes, but does contain 2 other elements.
@verbatim
<Item priority="1">
@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
<bold>
@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
<h2> Documentation </h2>
The documentation is build with Doxygen, using the 'dox'
configuration file.
<h2> License </h2>
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.
<h2> References </h2>
The World Wide Web Consortium is the definitive standard body for
XML, and their web pages contain huge amounts of information.
The definitive spec: <a href="http://www.w3.org/TR/2004/REC-xml-20040204/">
http://www.w3.org/TR/2004/REC-xml-20040204/</a>
I also recommend "XML Pocket Reference" by Robert Eckstein and published by
OReilly...the book that got the whole thing started.
<h2> Contributors, Contacts, and a Brief History </h2>
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
*/

111
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp vendored Normal file
View File

@ -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<TiXmlString::size_type>( 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<TiXmlString::size_type>( strlen(a) );
tmp.reserve(a_len + b.length());
tmp.append(a, a_len);
tmp += b;
return tmp;
}
#endif // TIXML_USE_STL

305
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h vendored Normal file
View File

@ -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 <assert.h>
#include <string.h>
/* 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<size_type>( 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<size_type>( 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<Rep*>(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<Rep*>( 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<int*>( 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

1886
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1805
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

38
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.sln vendored Normal file
View File

@ -0,0 +1,38 @@

Microsoft Visual Studio Solution File, Format Version 11.00
# Visual C++ Express 2010
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "tinyXmlTest", "tinyXmlTest.vcxproj", "{34719950-09E8-457E-BE23-8F1CE3A1F1F6}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "tinyXmlTestSTL", "tinyXmlTestSTL.vcxproj", "{53ED5965-5BCA-47B5-9EB0-EDD20882F22F}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "tinyxml", "tinyxml_lib.vcxproj", "{C406DAEC-0886-4771-8DEA-9D7329B46CC1}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "tinyxmlSTL", "tinyxmlSTL.vcxproj", "{A3A84737-5017-4577-B8A2-79429A25B8B6}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Win32 = Debug|Win32
Release|Win32 = Release|Win32
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{34719950-09E8-457E-BE23-8F1CE3A1F1F6}.Debug|Win32.ActiveCfg = Debug|Win32
{34719950-09E8-457E-BE23-8F1CE3A1F1F6}.Debug|Win32.Build.0 = Debug|Win32
{34719950-09E8-457E-BE23-8F1CE3A1F1F6}.Release|Win32.ActiveCfg = Release|Win32
{34719950-09E8-457E-BE23-8F1CE3A1F1F6}.Release|Win32.Build.0 = Release|Win32
{53ED5965-5BCA-47B5-9EB0-EDD20882F22F}.Debug|Win32.ActiveCfg = Debug|Win32
{53ED5965-5BCA-47B5-9EB0-EDD20882F22F}.Debug|Win32.Build.0 = Debug|Win32
{53ED5965-5BCA-47B5-9EB0-EDD20882F22F}.Release|Win32.ActiveCfg = Release|Win32
{53ED5965-5BCA-47B5-9EB0-EDD20882F22F}.Release|Win32.Build.0 = Release|Win32
{C406DAEC-0886-4771-8DEA-9D7329B46CC1}.Debug|Win32.ActiveCfg = Debug|Win32
{C406DAEC-0886-4771-8DEA-9D7329B46CC1}.Debug|Win32.Build.0 = Debug|Win32
{C406DAEC-0886-4771-8DEA-9D7329B46CC1}.Release|Win32.ActiveCfg = Release|Win32
{C406DAEC-0886-4771-8DEA-9D7329B46CC1}.Release|Win32.Build.0 = Release|Win32
{A3A84737-5017-4577-B8A2-79429A25B8B6}.Debug|Win32.ActiveCfg = Debug|Win32
{A3A84737-5017-4577-B8A2-79429A25B8B6}.Debug|Win32.Build.0 = Debug|Win32
{A3A84737-5017-4577-B8A2-79429A25B8B6}.Release|Win32.ActiveCfg = Release|Win32
{A3A84737-5017-4577-B8A2-79429A25B8B6}.Release|Win32.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
EndGlobal

52
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxmlerror.cpp vendored Normal file
View File

@ -0,0 +1,52 @@
/*
www.sourceforge.net/projects/tinyxml
Original code (2.0 and earlier )copyright (c) 2000-2006 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 "tinyxml.h"
// The goal of the seperate error file is to make the first
// step towards localization. tinyxml (currently) only supports
// english error messages, but the could now be translated.
//
// It also cleans up the code a bit.
//
const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
{
"No error",
"Error",
"Failed to open file",
"Error parsing Element.",
"Failed to read Element name",
"Error reading Element value.",
"Error reading Attributes.",
"Error: empty tag.",
"Error reading end tag.",
"Error parsing Unknown.",
"Error parsing Comment.",
"Error parsing Declaration.",
"Error document empty.",
"Error null (0) or unexpected EOF found in input stream.",
"Error parsing CDATA.",
"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
};

1638
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxmlparser.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

11
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8test.xml vendored Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8"?>
<document>
<English name="name" value="value">The world has many languages</English>
<Russian name="название(имя)" value="ценность">Мир имеет много языков</Russian>
<Spanish name="el nombre" value="el valor">el mundo tiene muchos idiomas</Spanish>
<SimplifiedChinese name="名字" value="价值">世界有很多语言</SimplifiedChinese>
<Русский название="name" ценность="value">&lt;имеет&gt;</Русский>
<汉语 名字="name" 价值="value">世界有很多语言</汉语>
<Heavy>"M&#x0eB;t&#230;l!"</Heavy>
<ä>Umlaut Element</ä>
</document>

11
3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml vendored Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<document>
<English name="name" value="value">The world has many languages</English>
<Russian name="название(имя)" value="ценность">Мир имеет много языков</Russian>
<Spanish name="el nombre" value="el valor">el mundo tiene muchos idiomas</Spanish>
<SimplifiedChinese name="名字" value="价值">世界有很多语言</SimplifiedChinese>
<Русский название="name" ценность="value">&lt;имеет&gt;</Русский>
<汉语 名字="name" 价值="value">世界有很多语言</汉语>
<Heavy>&quot;Mëtæl!&quot;</Heavy>
<ä>Umlaut Element</ä>
</document>

27
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h vendored Normal file
View File

@ -0,0 +1,27 @@
#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H
#define BOOST_REPLACEMENT_LEXICAL_CAST_H
#include <stdlib.h>
namespace boost
{
template <typename T> 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

28
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp vendored Normal file
View File

@ -0,0 +1,28 @@
#include "printf_console.h"
#include <stdio.h>
#include <rbdl/Logging.h>
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;
}

13
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h vendored Normal file
View File

@ -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

210
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h vendored Normal file
View File

@ -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 <nicola@fluidinteractive.com>
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 <assert.h>
#ifdef _WIN32
#include <windows.h>
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 <pthread.h>
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<typename T>
class my_shared_ptr
{
public:
my_shared_ptr(): m_ptr(NULL), m_count(NULL) { }
my_shared_ptr(my_shared_ptr<T> const& other):
m_ptr(other.m_ptr),
m_count(other.m_count)
{
if(other.m_count != NULL) other.m_count->increment();
}
template<typename U>
my_shared_ptr(my_shared_ptr<U> 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<T*>(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<T*>(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<T> const& rhs) const { return m_ptr < rhs.m_ptr; }
my_shared_ptr<T>& operator=(my_shared_ptr<T> 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<typename U>
my_shared_ptr<T>& operator=(my_shared_ptr<U>& 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<typename U> 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

253
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp vendored Normal file
View File

@ -0,0 +1,253 @@
#include <assert.h>
//#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "string_split.h"
namespace boost
{
void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> 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<numSubStr;i++)
pieces.push_back(std::string(strArray[i]));
str_array_free(strArray);
}
}
std::vector<std::string> is_any_of(const char* seps)
{
std::vector<std::string> strArray;
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
char sep2[2] = {0,0};
sep2[0] = seps[i];
strArray.push_back(sep2);
}
return strArray;
}
};
/* Append an item to a dynamically allocated array of strings. On failure,
return NULL, in which case the original array is intact. The item
string is dynamically copied. If the array is NULL, allocate a new
array. Otherwise, extend the array. Make sure the array is always
NULL-terminated. Input string might not be '\0'-terminated. */
char **str_array_append(char **array, size_t nitems, const char *item,
size_t itemlen)
{
/* Make a dynamic copy of the item. */
char *copy;
if (item == NULL)
copy = NULL;
else {
copy = (char*)malloc(itemlen + 1);
if (copy == NULL)
return NULL;
memcpy(copy, item, itemlen);
copy[itemlen] = '\0';
}
/* Extend array with one element. Except extend it by two elements,
in case it did not yet exist. This might mean it is a teeny bit
too big, but we don't care. */
array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
if (array == NULL) {
free(copy);
return NULL;
}
/* Add copy of item to array, and return it. */
array[nitems] = copy;
array[nitems+1] = NULL;
return array;
}
/* Free a dynamic array of dynamic strings. */
void str_array_free(char **array)
{
if (array == NULL)
return;
for (size_t i = 0; array[i] != NULL; ++i)
free(array[i]);
free(array);
}
/* 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)
{
size_t nitems = 0;
char **array = NULL;
const char *start = input;
const char *next = strstr(start, sep);
size_t seplen = strlen(sep);
const char *item;
size_t itemlen;
for (;;) {
next = strstr(start, sep);
if (next == NULL) {
/* Add the remaining string (or empty string, if input ends with
separator. */
char **newstr = str_array_append(array, nitems, start, strlen(start));
if (newstr == NULL) {
str_array_free(array);
return NULL;
}
array = newstr;
++nitems;
break;
} else if (next == input) {
/* Input starts with separator. */
item = "";
itemlen = 0;
} else {
item = start;
itemlen = next - item;
}
char **newstr = str_array_append(array, nitems, item, itemlen);
if (newstr == NULL) {
str_array_free(array);
return NULL;
}
array = newstr;
++nitems;
start = next + seplen;
}
if (nitems == 0) {
/* Input does not contain separator at all. */
assert(array == NULL);
array = str_array_append(array, nitems, input, strlen(input));
}
return array;
}
/* Return length of a NULL-delimited array of strings. */
size_t str_array_len(char **array)
{
size_t len;
for (len = 0; array[len] != NULL; ++len)
continue;
return len;
}
#ifdef UNIT_TEST_STRING
#define MAX_OUTPUT 20
int main(void)
{
struct {
const char *input;
const char *sep;
char *output[MAX_OUTPUT];
} tab[] = {
/* Input is empty string. Output should be a list with an empty
string. */
{
"",
"and",
{
"",
NULL,
},
},
/* Input is exactly the separator. Output should be two empty
strings. */
{
"and",
"and",
{
"",
"",
NULL,
},
},
/* Input is non-empty, but does not have separator. Output should
be the same string. */
{
"foo",
"and",
{
"foo",
NULL,
},
},
/* Input is non-empty, and does have separator. */
{
"foo bar 1 and foo bar 2",
" and ",
{
"foo bar 1",
"foo bar 2",
NULL,
},
},
};
const int tab_len = sizeof(tab) / sizeof(tab[0]);
bool errors;
errors = false;
for (int i = 0; i < tab_len; ++i) {
printf("test %d\n", i);
char **output = str_split(tab[i].input, tab[i].sep);
if (output == NULL) {
fprintf(stderr, "output is NULL\n");
errors = true;
break;
}
size_t num_output = str_array_len(output);
printf("num_output %lu\n", (unsigned long) num_output);
size_t num_correct = str_array_len(tab[i].output);
if (num_output != num_correct) {
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
(unsigned long) num_output, (unsigned long) num_correct);
errors = true;
} else {
for (size_t j = 0; j < num_output; ++j) {
if (strcmp(tab[i].output[j], output[j]) != 0) {
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
(unsigned long) j, output[j], tab[i].output[j]);
errors = true;
break;
}
}
}
str_array_free(output);
printf("\n");
}
if (errors)
return EXIT_FAILURE;
return 0;
}
#endif//

31
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.h vendored Normal file
View File

@ -0,0 +1,31 @@
#ifndef STRING_SPLIT_H
#define STRING_SPLIT_H
#include <cstring>
#include <vector>
#include <string>
namespace boost
{
void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> separators);
std::vector<std::string> 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

15
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE vendored Normal file
View File

@ -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.

7
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt vendored Normal file
View File

@ -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

63
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h vendored Normal file
View File

@ -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 <string>
#include <map>
#include "tinyxml/tinyxml.h"
//#include <boost/function.hpp>
#ifndef M_PI
#define M_PI 3.1415925438
#endif //M_PI
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
namespace urdf{
my_shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
}
#endif

137
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp vendored Normal file
View File

@ -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 <iostream>
#include <fstream>
using namespace urdf;
void printTree(my_shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild
printTree(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
}
#define MSTRINGIFY(A) #A
const char* urdf_char = MSTRINGIFY(
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
</joint>
</robot>);
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<ModelInterface> 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<const Link> 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;
}

579
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp vendored Normal file
View File

@ -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 <sstream>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h>
#ifdef URDF_USE_BOOST
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/lexical_cast.h>
#endif
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#else
#include "urdf/boost_replacement/printf_console.h"
#endif
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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;
}
}

505
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp vendored Normal file
View File

@ -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 <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
//#include <fstream>
//#include <sstream>
#ifdef URDF_USE_BOOST
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/lexical_cast.h>
#endif
#include <algorithm>
#include <tinyxml/tinyxml.h>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#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<double>(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<double>(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<double>(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<Geometry> parseGeometry(TiXmlElement *g)
{
my_shared_ptr<Geometry> 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<Geometry>();
}
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<double>(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<double>(inertia_xml->Attribute("ixx"));
i.ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
i.ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
i.iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
i.iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
i.izz = boost::lexical_cast<double>(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<Visual> 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<Collision> 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;
}
}

240
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp vendored Normal file
View File

@ -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 <boost/algorithm/string.hpp>
#include <vector>
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#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<ModelInterface> parseURDF(const std::string &xml_string)
{
my_shared_ptr<ModelInterface> 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;
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;
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;
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<std::string, std::string> 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;
}
}

91
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp vendored Normal file
View File

@ -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 <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
#include <fstream>
#include <sstream>
//#include <boost/lexical_cast.hpp>
#include <algorithm>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#else
#include "urdf/boost_replacement/printf_console.h"
#endif
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
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;
}
}

85
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp vendored Normal file
View File

@ -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 <urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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;
}
}

154
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp vendored Normal file
View File

@ -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 <urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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<double>(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<JointState> 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<std::string> 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<double>(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<std::string> 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<double>(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<std::string> 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<double>(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);
}
};
}

364
3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp vendored Normal file
View File

@ -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 <urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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<unsigned int>(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<unsigned int>(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<double>(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<double>(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<double>(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 <image> 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<unsigned int>(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<double>(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<double>(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<double>(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<unsigned int>(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<double>(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<double>(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<double>(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<VisualSensor> parseVisualSensor(TiXmlElement *g)
{
boost::shared_ptr<VisualSensor> 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 <sensor> 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;
}
}

Some files were not shown because too many files have changed in this diff Show More