diff --git a/.gitignore b/.gitignore index c9a4c7e4..bd0f64ff 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,135 @@ -debug -build +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# dotenv +.env + +# virtualenv +.venv +venv/ +ENV/ + +# Spyder project settings +.spyderproject +.spyproject +Pipfile.lock +.idea + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ + +# Visual code +.vscode + +# Visual studio +.vs + +# CMake +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +CMakeLists.txt.user + +# Specific project files and folders +build* +debug* +install* +*.user* +includes/numpy +S2MSource +pyomeca +release* +*.DS_Store + doc/html **/build -CMakeLists.txt.user diff --git a/CMake/FindCasadi.cmake b/CMake/FindCasadi.cmake new file mode 100644 index 00000000..5adddb47 --- /dev/null +++ b/CMake/FindCasadi.cmake @@ -0,0 +1,29 @@ +# - Find Casadi +# Find the native Casadi includes and libraries +# +# Casadi_INCLUDE_DIR - where to find casadi.hpp, etc. +# Casadi_LIBRARIES - List of libraries when using Casadi. +# Casadi_FOUND - True if Casadi is found. + +if (Casadi_INCLUDE_DIR) + # Already in cache, be silent + set (Casadi_FIND_QUIETLY TRUE) +endif (Casadi_INCLUDE_DIR) + +find_path (Casadi_INCLUDE_DIR "casadi.hpp" + PATHS ${CMAKE_INSTALL_PREFIX}/include/casadi ${CMAKE_INSTALL_PREFIX}/Library/include/casadi +) +find_library (Casadi_LIBRARY NAMES casadi + PATHS ${CMAKE_INSTALL_PREFIX}/lib ${CMAKE_INSTALL_PREFIX}/Library/lib +) + +# handle the QUIETLY and REQUIRED arguments and set Casadi_FOUND to TRUE if +# all listed variables are TRUE +include (FindPackageHandleStandardArgs) +find_package_handle_standard_args (Casadi DEFAULT_MSG + Casadi_LIBRARY + Casadi_INCLUDE_DIR +) + + + diff --git a/CMakeLists.txt b/CMakeLists.txt index a0508d8b..93850105 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.10) SET ( RBDL_VERSION_MAJOR 3 ) -SET ( RBDL_VERSION_MINOR 0 ) -SET ( RBDL_VERSION_PATCH 0 ) +SET ( RBDL_VERSION_MINOR 1 ) +SET ( RBDL_VERSION_PATCH 2 ) SET ( RBDL_VERSION ${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH} ) @@ -16,7 +16,6 @@ LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) INCLUDE_DIRECTORIES ( ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_CURRENT_BINARY_DIR}/include ) @@ -36,15 +35,6 @@ IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 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) - MESSAGE (WARNING "Could not find Eigen3 on your system. Please install it!") -ENDIF (NOT EIGEN3_FOUND) - -INCLUDE_DIRECTORIES (${EIGEN3_INCLUDE_DIR}) - IF (MSVC) SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /permissive- /std:c++17") ENDIF (MSVC) @@ -61,6 +51,9 @@ ELSE( ${CMAKE_VERSION} VERSION_LESS 3.12.0 ) ENDIF( ${CMAKE_VERSION} VERSION_LESS 3.12.0 ) +SET (RBDL_BUILD_COMPILER_ID ${CMAKE_CXX_COMPILER_ID}) +SET (RBDL_BUILD_COMPILER_VERSION ${CMAKE_CXX_COMPILER_VERSION}) + # Options SET (RBDL_BUILD_STATIC_DEFAULT OFF) OPTION (RBDL_BUILD_STATIC "Build statically linked library (otherwise dynamiclly linked)" ${RBDL_BUILD_STATIC_DEFAULT}) @@ -76,10 +69,17 @@ OPTION (RBDL_BUILD_ADDON_MUSCLE "Build the muscle library" OFF) OPTION (RBDL_BUILD_ADDON_MUSCLE_FITTING "Build muscle library fitting functions (requires Ipopt)" OFF) OPTION (RBDL_BUILD_EXECUTABLES "Build addon executables, disable this if you only want to build the libraries." ON) OPTION (RBDL_USE_PYTHON_2 "Use python 2 instead of python 3" OFF) +OPTION (RBDL_USE_CASADI_MATH "Use the CasADi backend" OFF) -SET (RBDL_BUILD_COMPILER_ID ${CMAKE_CXX_COMPILER_ID}) -SET (RBDL_BUILD_COMPILER_VERSION ${CMAKE_CXX_COMPILER_VERSION}) +# Find and use the system's Eigen3 library +FIND_PACKAGE (Eigen3 3.0.0) + +IF (NOT EIGEN3_FOUND) + MESSAGE (WARNING "Could not find Eigen3 on your system. Please install it!") +ENDIF (NOT EIGEN3_FOUND) + +INCLUDE_DIRECTORIES (${EIGEN3_INCLUDE_DIR}) # Addons IF (RBDL_BUILD_ADDON_URDFREADER) @@ -90,7 +90,9 @@ IF (RBDL_BUILD_ADDON_BENCHMARK) ADD_SUBDIRECTORY ( addons/benchmark ) ENDIF (RBDL_BUILD_ADDON_BENCHMARK) - +IF (RBDL_USE_CASADI_MATH) + ADD_SUBDIRECTORY( casadi ) +ENDIF (RBDL_USE_CASADI_MATH) IF(RBDL_BUILD_ADDON_MUSCLE) SET(RBDL_BUILD_ADDON_GEOMETRY ON CACHE BOOL "Build the geometry library" FORCE) @@ -138,16 +140,13 @@ SET ( RBDL_SOURCES 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) + TARGET_INCLUDE_DIRECTORIES(rbdl-static PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/include) + IF (NOT MSVC) SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib") - ENDIF (NOT WIN32) + ENDIF (NOT MSVC) SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES OUTPUT_NAME "rbdl") IF (RBDL_BUILD_ADDON_LUAMODEL) @@ -160,6 +159,11 @@ IF (RBDL_BUILD_STATIC) rbdl_urdfreader-static ) ENDIF (RBDL_BUILD_ADDON_URDFREADER) + IF (RBDL_USE_CASADI_MATH) + TARGET_LINK_LIBRARIES ( rbdl-static + ${Casadi_LIBRARY} + ) + ENDIF (RBDL_USE_CASADI_MATH) INSTALL (TARGETS rbdl-static LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} @@ -168,11 +172,18 @@ IF (RBDL_BUILD_STATIC) ) ELSE (RBDL_BUILD_STATIC) ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} ) + TARGET_INCLUDE_DIRECTORIES(rbdl PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/include) SET_TARGET_PROPERTIES ( rbdl PROPERTIES VERSION ${RBDL_VERSION} SOVERSION ${RBDL_SO_VERSION} ) + IF (RBDL_USE_CASADI_MATH) + TARGET_LINK_LIBRARIES ( rbdl + ${Casadi_LIBRARY} + ) + ENDIF (RBDL_USE_CASADI_MATH) + INSTALL (TARGETS rbdl LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} @@ -217,7 +228,7 @@ INSTALL ( FILES ${headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl ) # pkg-config CONFIGURE_FILE ( - ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pc.cmake + ${CMAKE_CURRENT_SOURCE_DIR}/share/rbdl.pc.cmake ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pc @ONLY ) INSTALL ( @@ -225,6 +236,23 @@ INSTALL ( DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig ) +# Prepare share +if(WIN32 AND NOT CYGWIN) + set(CMAKE_SHARE_DIR cmake) +else() + set(CMAKE_SHARE_DIR lib/cmake/RBDL) +endif() +include(CMakePackageConfigHelpers) +configure_package_config_file( + share/RBDLConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/RBDLConfig.cmake + INSTALL_DESTINATION ${CMAKE_SHARE_DIR} +) +INSTALL ( + FILES ${CMAKE_CURRENT_BINARY_DIR}/RBDLConfig.cmake + DESTINATION ${CMAKE_SHARE_DIR} + ) + # Packaging SET(CPACK_GENERATOR "DEB") SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Martin Felis ") diff --git a/Doxyfile b/Doxyfile index 844f467c..cb7948d0 100644 --- a/Doxyfile +++ b/Doxyfile @@ -676,7 +676,8 @@ INPUT = ./src \ addons/muscle/MuscleFunctionFactory.h \ addons/muscle/TorqueMuscleFunctionFactory.h \ addons/muscle/TorqueMuscleFittingToolkit.h \ - ./include/rbdl + ./include/rbdl \ + casadi/info.h # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is @@ -772,6 +773,7 @@ EXAMPLE_PATH = doc \ examples/luamodel \ examples/simple \ examples/python \ + examples # If the value of the EXAMPLE_PATH tag contains directories, you can use the diff --git a/README.md b/README.md index e3ec3a54..89df167d 100644 --- a/README.md +++ b/README.md @@ -206,10 +206,10 @@ at which point you will see full list of build options for RBDL. We recommend th - Install Ipopt. One of the easier ways to do this is to follow these instructions from [Ipopt's online documentation](https://www.coin-or.org/Ipopt/documentation/node12.html#SECTION00042300000000000000) which guides you through the process. Instructions to build the code appear in the README located in the Ipopt folder - Configure RBDL's cmake file with these flags set to 'On' ``` - RBDL_BUILD_ADDON_GEOMETRY ON - RBDL_BUILD_ADDON_LUAMODEL ON - RBDL_BUILD_ADDON_MUSCLE ON - RBDL_BUILD_ADDON_MUSCLE_FITTING ON + RBDL_BUILD_ADDON_GEOMETRY ON + RBDL_BUILD_ADDON_LUAMODEL ON + RBDL_BUILD_ADDON_MUSCLE ON + RBDL_BUILD_ADDON_MUSCLE_FITTING ON ``` - Set the CUSTOM_IPOPT_PATH to the main Ipopt directory. diff --git a/addons/geometry/CMakeLists.txt b/addons/geometry/CMakeLists.txt index 92004294..49e10942 100644 --- a/addons/geometry/CMakeLists.txt +++ b/addons/geometry/CMakeLists.txt @@ -75,5 +75,5 @@ FILE ( GLOB headers INSTALL ( FILES ${GEOMETRY_HEADERS} DESTINATION - ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/geometry + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/geometry ) diff --git a/addons/luamodel/CMakeLists.txt b/addons/luamodel/CMakeLists.txt index 6ad660f2..f300b513 100644 --- a/addons/luamodel/CMakeLists.txt +++ b/addons/luamodel/CMakeLists.txt @@ -119,7 +119,7 @@ ELSE (RBDL_BUILD_STATIC) RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) + ) ENDIF (RBDL_BUILD_STATIC) FILE ( GLOB headers @@ -128,5 +128,5 @@ FILE ( GLOB headers INSTALL ( FILES ${headers} DESTINATION - ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/luamodel + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/luamodel ) diff --git a/addons/luamodel/luatypes.h b/addons/luamodel/luatypes.h index 7a6d64d1..dbc4609f 100644 --- a/addons/luamodel/luatypes.h +++ b/addons/luamodel/luatypes.h @@ -168,9 +168,9 @@ LuaTableNode::getDefault( LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); result.r = vector_table["r"].getDefault( - RigidBodyDynamics::Math::Vector3d::Zero(3)); + RigidBodyDynamics::Math::Vector3d::Zero()); result.E = vector_table["E"].getDefault( - RigidBodyDynamics::Math::Matrix3d::Identity (3,3)); + RigidBodyDynamics::Math::Matrix3d::Identity ()); } stackRestore(); @@ -310,9 +310,9 @@ LuaTableNode::getDefault( double mass = 0.; RigidBodyDynamics::Math::Vector3d com( - RigidBodyDynamics::Math::Vector3d::Zero(3)); + RigidBodyDynamics::Math::Vector3d::Zero()); RigidBodyDynamics::Math::Matrix3d inertia( - RigidBodyDynamics::Math::Matrix3d::Identity(3,3)); + RigidBodyDynamics::Math::Matrix3d::Identity()); mass = vector_table["mass"]; com = vector_table["com"] diff --git a/addons/urdfreader/CMakeLists.txt b/addons/urdfreader/CMakeLists.txt index 2a34a020..098200e2 100644 --- a/addons/urdfreader/CMakeLists.txt +++ b/addons/urdfreader/CMakeLists.txt @@ -121,5 +121,5 @@ FILE ( GLOB headers INSTALL ( FILES ${headers} DESTINATION - ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/urdfreader + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/urdfreader ) diff --git a/casadi/CMakeLists.txt b/casadi/CMakeLists.txt new file mode 100644 index 00000000..fbef5643 --- /dev/null +++ b/casadi/CMakeLists.txt @@ -0,0 +1,127 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +FIND_PACKAGE (Casadi REQUIRED) + +IF (RBDL_STORE_VERSION) + # Set versioning information that can be queried during runtime + EXECUTE_PROCESS(COMMAND "git" "rev-parse" "HEAD" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + OUTPUT_VARIABLE RBDL_BUILD_COMMIT) + STRING(REGEX REPLACE "\n$" "" RBDL_BUILD_COMMIT "${RBDL_BUILD_COMMIT}") + EXECUTE_PROCESS(COMMAND "git" "describe" "--all" "--dirty" "--long" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + OUTPUT_VARIABLE RBDL_BUILD_BRANCH) + STRING(REGEX REPLACE "\n$" "" RBDL_BUILD_BRANCH "${RBDL_BUILD_BRANCH}") + SET (RBDL_BUILD_TYPE ${CMAKE_BUILD_TYPE}) +ELSE (RBDL_STORE_VERSION) + SET (RBDL_BUILD_COMMIT "unknown") + SET (RBDL_BUILD_BRANCH "unknown") + SET (RBDL_BUILD_TYPE "unknown") +ENDIF (RBDL_STORE_VERSION) + +CONFIGURE_FILE ( + "${CMAKE_SOURCE_DIR}/include/rbdl/rbdl_casadi_config.h.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h" + ) + +SET ( RBDL_CASADI_SOURCES + ${CMAKE_SOURCE_DIR}/src/rbdl_version.cc + ${CMAKE_SOURCE_DIR}/src/rbdl_mathutils.cc + ${CMAKE_SOURCE_DIR}/src/rbdl_utils.cc + ${CMAKE_SOURCE_DIR}/src/rbdl_errors.cc + ${CMAKE_SOURCE_DIR}/src/Constraints.cc + ${CMAKE_SOURCE_DIR}/src/Constraint_Contact.cc + ${CMAKE_SOURCE_DIR}/src/Constraint_Loop.cc + ${CMAKE_SOURCE_DIR}/src/Dynamics.cc + ${CMAKE_SOURCE_DIR}/src/Logging.cc + ${CMAKE_SOURCE_DIR}/src/Joint.cc + ${CMAKE_SOURCE_DIR}/src/Model.cc + ${CMAKE_SOURCE_DIR}/src/Kinematics.cc +) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl-casadi-static STATIC ${RBDL_CASADI_SOURCES} ) + + TARGET_INCLUDE_DIRECTORIES (rbdl-casadi-static PUBLIC BEFORE + ${Casadi_INCLUDE_DIR} + ${Casadi_INCLUDE_DIR}/.. + ${CMAKE_SOURCE_DIR}/include + ${CMAKE_CURRENT_BINARY_DIR}/include + ) + + IF (NOT MSVC) + SET_TARGET_PROPERTIES ( rbdl-casadi-static PROPERTIES PREFIX "lib") + ENDIF (NOT MSVC) + SET_TARGET_PROPERTIES ( rbdl-casadi-static PROPERTIES OUTPUT_NAME "rbdl-casadi") + + TARGET_LINK_LIBRARIES ( rbdl-casadi-static + ${Casadi_LIBRARY} + ) + + INSTALL (TARGETS rbdl-casadi-static + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl-casadi SHARED ${RBDL_CASADI_SOURCES} ) + TARGET_INCLUDE_DIRECTORIES (rbdl-casadi PUBLIC BEFORE + ${Casadi_INCLUDE_DIR} + ${Casadi_INCLUDE_DIR}/.. + ${CMAKE_SOURCE_DIR}/include + ${CMAKE_CURRENT_BINARY_DIR}/include + ) + + SET_TARGET_PROPERTIES ( rbdl-casadi PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES ( rbdl-casadi + ${Casadi_LIBRARY} + ) + + INSTALL (TARGETS rbdl-casadi + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +# Setup of CasADi install settings +FILE ( GLOB headers + ${CMAKE_SOURCE_DIR}/include/rbdl/*.h + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h + ) + +INSTALL ( FILES ${headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl-casadi/rbdl ) +INSTALL ( DIRECTORY "${CMAKE_SOURCE_DIR}/include/rbdl/CasadiMath" + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl-casadi/rbdl +) + +# pkg-config +CONFIGURE_FILE ( + ${CMAKE_SOURCE_DIR}/share/rbdl-casadi.pc.cmake + ${CMAKE_CURRENT_BINARY_DIR}/rbdl-casadi.pc @ONLY + ) +INSTALL ( + FILES ${CMAKE_CURRENT_BINARY_DIR}/rbdl-casadi.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig + ) + +# Prepare share +if(WIN32 AND NOT CYGWIN) + set(CMAKE_SHARE_DIR cmake) +else() + set(CMAKE_SHARE_DIR lib/cmake/RBDLCasadi) +endif() +include(CMakePackageConfigHelpers) +configure_package_config_file( + ${CMAKE_SOURCE_DIR}/share/RBDLCasadiConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/RBDLCasadiConfig.cmake + INSTALL_DESTINATION ${CMAKE_SHARE_DIR} +) +INSTALL ( + FILES ${CMAKE_CURRENT_BINARY_DIR}/RBDLCasadiConfig.cmake + DESTINATION ${CMAKE_SHARE_DIR} + ) diff --git a/casadi/info.h b/casadi/info.h new file mode 100644 index 00000000..dd74b0ce --- /dev/null +++ b/casadi/info.h @@ -0,0 +1,18 @@ +/** @page Casadi + * + * RBDL comes in two flavors which only differ in what mathematical backend they use. + * Casadi is a very interesting backend, because it has some unique features such as alogrithmic + * differentiation. But due to its complexity it is not the default. + * + * This page documents how to use the casadi version of RBDL. If you are unsure if you need casadi + * then you probably don't and should stick to the standart RBDL that uses Eigen as it's backend. + * For more information about casadi take a look at their website . + * + * The casadi_simple example shows how to build a simple application that uses casadi: + * + * CMakeLists.txt: note that the FindCasadi.cmake file is also required to find the correct include paths for casadi + * \include casadi_simple/CMakeLists.txt + * + * A simple rbdl example source file that works with casadi: + * \include casadi_simple/example.cc + */ diff --git a/doc/Mainpage.h b/doc/Mainpage.h index 6fc67cb8..d813b1c1 100644 --- a/doc/Mainpage.h +++ b/doc/Mainpage.h @@ -34,10 +34,18 @@ * * All development takes place on GitHub and you can follow RBDL's * development and obtain code here:
- * https://github.com/rbdl/rbdl + * https://github.com/orb-hd/rbdl-orb * * \section recent_changes Recent Changes *
    + *
  • 13 August 2021: New release 3.1.0: + *
      + *
    • Added support for rbdl-casadi, as an additional version of the rbdl library. + * It may be installed on the system in parallel to rbdl.
      • + *
    • Added system install of cmake config files, so that projects building on rbdl do not + * need to provide their own FindRBDL.cmake files anymore.
      • + *
    + *
    • *
  • 02 May 2018: New release 2.6.0: *
      *
    • Added support for closed-loop models by replacing Contacts API by a new diff --git a/doc/api_changes.txt b/doc/api_changes.txt index 596b1c40..51a02fd6 100644 --- a/doc/api_changes.txt +++ b/doc/api_changes.txt @@ -1,3 +1,7 @@ +3.0.0 -> 3.1.0 (13. August 2020) +- introducing the casadi math backend, rbdl-casadi is build as a seperated library that can be installed parallel to rbdl +- added install of cmake config files, so projects building on rbdl do not need to write their own FindRBDL.cmake files anymore + 2.6.0 -> 3.0.0 (24. September 2019) - Replaced aborting behaviour with actual error handling by providing an error diff --git a/examples/casadi_simple/CMakeLists.txt b/examples/casadi_simple/CMakeLists.txt new file mode 100644 index 00000000..6b4c343b --- /dev/null +++ b/examples/casadi_simple/CMakeLists.txt @@ -0,0 +1,29 @@ +PROJECT (RBDLCasadiExample CXX) + +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +# We need to add the project source path to the CMake module path so that +# the FindRBDL.cmake script can be found. +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} ) + + +SET(CUSTOM_RBDL_PATH "" CACHE PATH "Path to specific RBDL Installation") +# Search for the RBDL include directory and library +FIND_PACKAGE (Casadi REQUIRED) +FIND_PACKAGE (RBDLCasadi REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( + ${RBDLCasadi_INCLUDE_DIR} + ${Casadi_INCLUDE_DIR} + ${Casadi_INCLUDE_DIR}/.. +) + +# Create an executable +ADD_EXECUTABLE (casadi_example example.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES (casadi_example + ${Casadi_LIBRARY} + ${RBDLCasadi_LIBRARY} + ) diff --git a/examples/casadi_simple/FindCasadi.cmake b/examples/casadi_simple/FindCasadi.cmake new file mode 100644 index 00000000..5adddb47 --- /dev/null +++ b/examples/casadi_simple/FindCasadi.cmake @@ -0,0 +1,29 @@ +# - Find Casadi +# Find the native Casadi includes and libraries +# +# Casadi_INCLUDE_DIR - where to find casadi.hpp, etc. +# Casadi_LIBRARIES - List of libraries when using Casadi. +# Casadi_FOUND - True if Casadi is found. + +if (Casadi_INCLUDE_DIR) + # Already in cache, be silent + set (Casadi_FIND_QUIETLY TRUE) +endif (Casadi_INCLUDE_DIR) + +find_path (Casadi_INCLUDE_DIR "casadi.hpp" + PATHS ${CMAKE_INSTALL_PREFIX}/include/casadi ${CMAKE_INSTALL_PREFIX}/Library/include/casadi +) +find_library (Casadi_LIBRARY NAMES casadi + PATHS ${CMAKE_INSTALL_PREFIX}/lib ${CMAKE_INSTALL_PREFIX}/Library/lib +) + +# handle the QUIETLY and REQUIRED arguments and set Casadi_FOUND to TRUE if +# all listed variables are TRUE +include (FindPackageHandleStandardArgs) +find_package_handle_standard_args (Casadi DEFAULT_MSG + Casadi_LIBRARY + Casadi_INCLUDE_DIR +) + + + diff --git a/examples/casadi_simple/example.cc b/examples/casadi_simple/example.cc new file mode 100644 index 00000000..cc95bc47 --- /dev/null +++ b/examples/casadi_simple/example.cc @@ -0,0 +1,74 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +casadi::MX fd(Model& model, const VectorNd& Q, const VectorNd& Qdot, const VectorNd& Tau){ + VectorNd Qddot(model.dof_count); + ForwardDynamics (model, Q, Qdot, Tau, Qddot); + return Qddot; +} + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = NULL; + + unsigned int body_a_id, body_b_id, body_c_id; + Body body_a, body_b, body_c; + Joint joint_a, joint_b, joint_c; + + model = new Model(); + + model->gravity = Vector3d (0., -9.81, 0.); + + body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.)); + joint_a = Joint( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.)); + joint_c = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + auto Q_sym = VectorNd::sym ("Q", model->dof_count); + auto QDot_sym = VectorNd::sym ("QDot", model->dof_count); + auto Tau_sym = VectorNd::sym ("Tau", model->dof_count); + casadi::Function fd_fun = casadi::Function("fd_fun", {Q_sym, QDot_sym, Tau_sym}, {fd (*model, Q_sym, QDot_sym, Tau_sym)}, {"Q", "QDot", "Tau"}, {"QDDot"}).expand(); + + auto Q = casadi::DM::zeros (model->dof_count); + auto QDot = casadi::DM::zeros (model->dof_count); + auto Tau = casadi::DM::zeros (model->dof_count); + casadi::DM QDDot = fd_fun(casadi::DMDict{ {"Q", Q}, {"QDot", QDot}, {"Tau", Tau} }).at("QDDot"); + + std::cout << QDDot << std::endl; + + delete model; + + return 0; +} + diff --git a/include/rbdl/Body.h b/include/rbdl/Body.h index f7e1cb73..2607435f 100644 --- a/include/rbdl/Body.h +++ b/include/rbdl/Body.h @@ -29,7 +29,7 @@ struct RBDL_DLLAPI Body { Body() : mMass (0.), mCenterOfMass (0., 0., 0.), - mInertia (Math::Matrix3d::Zero(3,3)), + mInertia (Math::Matrix3d::Zero()), mIsVirtual (false) { }; Body(const Body &body) : @@ -61,9 +61,9 @@ struct RBDL_DLLAPI Body { * \param com the position of the center of mass in the bodies coordinates * \param gyration_radii the radii of gyration at the center of mass of the body */ - Body(const double &mass, - const Math::Vector3d &com, - const Math::Vector3d &gyration_radii) : + Body(const Math::Scalar &mass, + const Math::Vector3d &com, + const Math::Vector3d &gyration_radii) : mMass (mass), mCenterOfMass(com), mIsVirtual (false) @@ -86,9 +86,9 @@ struct RBDL_DLLAPI Body { * \param com the position of the center of mass in the bodies coordinates * \param inertia_C the inertia at the center of mass */ - Body(const double &mass, - const Math::Vector3d &com, - const Math::Matrix3d &inertia_C) : + Body(const Math::Scalar &mass, + const Math::Vector3d &com, + const Math::Matrix3d &inertia_C) : mMass (mass), mCenterOfMass(com), mInertia (inertia_C), @@ -110,17 +110,21 @@ struct RBDL_DLLAPI Body { */ void Join (const Math::SpatialTransform &transform, const Body &other_body) { +#ifndef RBDL_USE_CASADI_MATH // nothing to do if we join a massles body to the current. if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) { return; } +#endif - double other_mass = other_body.mMass; - double new_mass = mMass + other_mass; + Math::Scalar other_mass = other_body.mMass; + Math::Scalar new_mass = mMass + other_mass; +#ifndef RBDL_USE_CASADI_MATH if (new_mass == 0.) { throw Errors::RBDLError("Error: cannot join bodies as both have zero mass!\n"); } +#endif Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r; @@ -189,7 +193,7 @@ struct RBDL_DLLAPI Body { ~Body() {}; /// \brief The mass of the body - double mMass; + Math::Scalar mMass; /// \brief The position of the center of mass in body coordinates Math::Vector3d mCenterOfMass; /// \brief Inertia matrix at the center of mass @@ -206,7 +210,7 @@ struct RBDL_DLLAPI Body { */ struct RBDL_DLLAPI FixedBody { /// \brief The mass of the body - double mMass; + Math::Scalar mMass; /// \brief The position of the center of mass in body coordinates Math::Vector3d mCenterOfMass; /// \brief The spatial inertia that contains both mass and inertia information diff --git a/include/rbdl/CasadiMath/MX_Xd_dynamic.h b/include/rbdl/CasadiMath/MX_Xd_dynamic.h new file mode 100644 index 00000000..b076e477 --- /dev/null +++ b/include/rbdl/CasadiMath/MX_Xd_dynamic.h @@ -0,0 +1,247 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef MX_XD_DYNAMICS_H +#define MX_XD_DYNAMICS_H + +#include +#include +#include + +#include +#include "MX_Xd_scalar.h" +#include "MX_Xd_subMatrix.h" + +namespace RBDLCasadiMath { + +class MX_Xd_dynamic : public casadi::MX{ +public: + MX_Xd_dynamic( + unsigned int nrows = 1, + unsigned int ncols = 1) : casadi::MX(nrows, ncols){ + } + + virtual ~MX_Xd_dynamic(){ + + } + + + MX_Xd_dynamic(const casadi::MX& m) : + casadi::MX(m){ + } + + void conservativeResize (unsigned int nrows, unsigned int ncols = 1) { + MX_Xd_dynamic result = casadi::MX::zeros(nrows, ncols); + + unsigned int arows = std::min (nrows, rows()); + unsigned int acols = std::min (ncols, cols()); + + for (unsigned int i = 0; i < arows; i++) { + for (unsigned int j = 0; j < acols; j++) { + result(i,j) = (*this)(i,j); + } + } + + *this = result; + } + + static MX_Xd_dynamic Zero(unsigned int nrows, unsigned int ncols = 1){ + return casadi::MX::zeros(nrows, ncols); + } + MX_Xd_dynamic& setZero(){ + *this = casadi::MX::zeros(this->rows(), this->cols()); + return *this; + } + + static MX_Xd_dynamic One(unsigned int nrows, unsigned int ncols = 1){ + return casadi::MX::ones(nrows, ncols); + } + MX_Xd_dynamic& setOnes(){ + *this = casadi::MX::ones(this->rows(), this->cols()); + return *this; + } + + static MX_Xd_dynamic Identity(unsigned int size, unsigned int ignoredSize = 0){ + return casadi::MX::eye(size); + } + MX_Xd_dynamic& setIdentity(){ + *this = casadi::MX::eye(cols()); + return *this; + } + + MX_Xd_SubMatrix operator[](unsigned int i) { + return (*this)(i, 0); + } + MX_Xd_scalar operator[](unsigned int i) const { + return (*this)(i, 0); + } + MX_Xd_SubMatrix operator()(unsigned int i, unsigned int j=0) { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(i), static_cast(i+1)), + casadi::Slice(static_cast(j), static_cast(j+1))); + } + MX_Xd_scalar operator()(unsigned int i, unsigned int j=0) const { + return this->casadi::MX::operator()(i, j); + } + + void resize(unsigned int newI, unsigned int newJ = 1){ + *this = casadi::MX(newI, newJ); + } + + unsigned int rows() const { + return static_cast(this->casadi::MX::rows()); + } + + unsigned int cols() const { + return static_cast(this->casadi::MX::columns()); + } + + unsigned int size() const { + return rows() * cols(); + } + + template + MX_Xd_SubMatrix block( + unsigned int row_start, + unsigned int col_start) + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + template + MX_Xd_dynamic block( + unsigned int row_start, + unsigned int col_start) const + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + + MX_Xd_SubMatrix block( + unsigned int row_start, + unsigned int col_start, + unsigned int row_count, + unsigned int col_count) + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + MX_Xd_dynamic block( + unsigned int row_start, + unsigned int col_start, + unsigned int row_count, + unsigned int col_count) const + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + + MX_Xd_SubMatrix topRows( + unsigned int numberRows) + { + return (*this).block(0, 0, numberRows, cols()); + } + MX_Xd_dynamic topRows( + unsigned int numberRows) const + { + return (*this).block(0, 0, numberRows, cols()); + } + + + MX_Xd_dynamic transpose() const { + return T(); + } + + MX_Xd_dynamic inverse() const { + return inv(*this); + } + + MX_Xd_scalar dot(const MX_Xd_dynamic &other_vector) const { + return casadi::MX::dot(*this, other_vector); + } + + MX_Xd_dynamic norm() const { + return casadi::MX::norm_2(*this); + } + + void normalize() { + *this /= norm(); + } + + MX_Xd_dynamic squaredNorm() const { + return norm() * norm(); + } + + void operator+=( + const MX_Xd_dynamic& other) { + this->casadi::MX::operator+=(other); + } + MX_Xd_dynamic operator+( + const MX_Xd_dynamic& other) const { + MX_Xd_dynamic out(*this); + return out.casadi::MX::operator+=(other); + } + void operator-=( + const MX_Xd_dynamic& other) { + this->casadi::MX::operator-=(other); + } + MX_Xd_dynamic operator-( + const MX_Xd_dynamic& other) const { + MX_Xd_dynamic out(*this); + return out.casadi::MX::operator-=(other); + } + void operator*=( + const MX_Xd_dynamic& m2) { + *this = casadi::MX::mtimes(*this, m2); + } + MX_Xd_dynamic operator*(const MX_Xd_dynamic& other) const { + return casadi::MX::mtimes(*this, other); + } + MX_Xd_dynamic operator*(const MX_Xd_scalar& other) const { + return casadi::MX::mtimes(*this, other); + } + MX_Xd_dynamic operator*(const MX_Xd_SubMatrix& other) const { + return casadi::MX::mtimes(*this, other); + } + MX_Xd_dynamic operator*(const double& other) const { + return casadi::MX::mtimes(*this, other); + } + + void operator/=(const MX_Xd_scalar &scalar) { + for (unsigned int i = 0; i < rows() * cols(); i++) + this->casadi::MX::operator/=(scalar(0, 0)); + } + MX_Xd_dynamic operator/(const MX_Xd_scalar &scalar) const { + MX_Xd_dynamic result (*this); + for (unsigned int i = 0; i < rows(); ++i){ + for (unsigned int j = 0; j < cols(); ++j){ + result(i, j) /= scalar; + } + } + return result; + } + MX_Xd_dynamic operator/( + const MX_Xd_SubMatrix& scalar) const { + MX_Xd_dynamic result (*this); + for (unsigned int i = 0; i < rows(); ++i){ + for (unsigned int j = 0; j < cols(); ++j){ + result(i, j) /= scalar; + } + } + return result; + } +}; + +} + +/* MX_XD_DYNAMICS_H */ +#endif + diff --git a/include/rbdl/CasadiMath/MX_Xd_scalar.h b/include/rbdl/CasadiMath/MX_Xd_scalar.h new file mode 100644 index 00000000..0bf054ab --- /dev/null +++ b/include/rbdl/CasadiMath/MX_Xd_scalar.h @@ -0,0 +1,110 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef MX_XD_SCALAR_H +#define MX_XD_SCALAR_H + +#include +#include +#include + +#include + +namespace RBDLCasadiMath { + +class MX_Xd_scalar : public casadi::MX{ +public: + MX_Xd_scalar() : casadi::MX(1, 1){ + + } + + virtual ~MX_Xd_scalar(){ + + } + + + MX_Xd_scalar(const double val) : casadi::MX(1, 1){ + (*this)(0, 0) = val; + } + + MX_Xd_scalar(const casadi::MX& m) : casadi::MX(m){ + + } + + MX_Xd_scalar( + const MX_Xd_scalar& v0) : + casadi::MX(1, 1) + { + (*this)(0) = v0(0); + } + + unsigned int rows() const { + return 1; + } + + unsigned int cols() const { + return 1; + } + + unsigned int size() const { + return 1; + } + + MX_Xd_scalar operator[](unsigned int i) const{ + return (*this)(i); + } + + void operator+=( + const MX_Xd_scalar& other) { + this->casadi::MX::operator+=(other); + } + MX_Xd_scalar operator+( + const MX_Xd_scalar& other) const { + MX_Xd_scalar out(*this); + return out.casadi::MX::operator+=(other); + } + + void operator-=( + const MX_Xd_scalar& other) { + this->casadi::MX::operator-=(other); + } + MX_Xd_scalar operator-( + const MX_Xd_scalar& other) const { + MX_Xd_scalar out(*this); + return out.casadi::MX::operator-=(other); + } + MX_Xd_scalar operator-( + const double& other) const { + MX_Xd_scalar out(*this); + return out.casadi::MX::operator-=(other); + } + + MX_Xd_scalar operator*(const MX_Xd_scalar& other) const{ + return casadi::MX::times(*this, other); + } + MX_Xd_scalar operator*( + double other) { + return casadi::MX::times(*this, other); + } + + void operator/=( + const MX_Xd_scalar& other + ){ + this->casadi::MX::operator/=(other); + } + MX_Xd_scalar operator/( + const MX_Xd_scalar& other) const { + MX_Xd_scalar out(*this); + return out.casadi::MX::operator/=(other); + } +}; + +} + +/* MX_XD_SCALAR_H */ +#endif + diff --git a/include/rbdl/CasadiMath/MX_Xd_static.h b/include/rbdl/CasadiMath/MX_Xd_static.h new file mode 100644 index 00000000..786c7135 --- /dev/null +++ b/include/rbdl/CasadiMath/MX_Xd_static.h @@ -0,0 +1,457 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef MX_XD_STATIC_H +#define MX_XD_STATIC_H + +#include +#include +#include + +#include +#include "MX_Xd_scalar.h" +#include "MX_Xd_dynamic.h" + + +namespace RBDLCasadiMath { + +template +class MX_Xd_static : public casadi::MX{ +public: + MX_Xd_static() : casadi::MX(nrows, ncols){ + + } + + virtual ~MX_Xd_static(){ + + } + + MX_Xd_static(const double val) : casadi::MX(1, 1) + { + (*this)(0, 0) = val; + } + + MX_Xd_static(const casadi::MX& m) : casadi::MX(m){ + } + + MX_Xd_static( + const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1) : + casadi::MX(2, 1) + { + (*this)(0) = v0(0); + (*this)(1) = v1(0); + } + MX_Xd_static( + const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2) : + casadi::MX(3, 1) + { + this->casadi::MX::operator ()(0) = v0(0); + (*this)(1) = v1(0); + (*this)(2) = v2(0); + + } + MX_Xd_static(const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2, + const MX_Xd_scalar& v3) : + casadi::MX(4, 1) + { + (*this)(0) = v0(0); + (*this)(1) = v1(0); + (*this)(2) = v2(0); + (*this)(3) = v3(0); + } + MX_Xd_static(const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2, + const MX_Xd_scalar& v3, + const MX_Xd_scalar& v4, + const MX_Xd_scalar& v5) : + casadi::MX(6, 1) + { + (*this)(0) = v0(0); + (*this)(1) = v1(0); + (*this)(2) = v2(0); + (*this)(3) = v3(0); + (*this)(4) = v4(0); + (*this)(5) = v5(0); + } + + MX_Xd_static(const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2, + const MX_Xd_scalar& v3, + const MX_Xd_scalar& v4, + const MX_Xd_scalar& v5, + const MX_Xd_scalar& v6, + const MX_Xd_scalar& v7) : + casadi::MX(8, 1) + { + (*this)(0) = v0(0); + (*this)(1) = v1(0); + (*this)(2) = v2(0); + (*this)(3) = v3(0); + (*this)(4) = v4(0); + (*this)(5) = v5(0); + (*this)(6) = v6(0); + (*this)(7) = v7(0); + + } + MX_Xd_static(const MX_Xd_scalar& v00, const MX_Xd_scalar& v01, const MX_Xd_scalar& v02, + const MX_Xd_scalar& v10, const MX_Xd_scalar& v11, const MX_Xd_scalar& v12, + const MX_Xd_scalar& v20, const MX_Xd_scalar& v21, const MX_Xd_scalar& v22) : + casadi::MX(3, 3) + { + (*this)(0,0) = v00(0, 0); + (*this)(0,1) = v01(0, 0); + (*this)(0,2) = v02(0, 0); + (*this)(1,0) = v10(0, 0); + (*this)(1,1) = v11(0, 0); + (*this)(1,2) = v12(0, 0); + (*this)(2,0) = v20(0, 0); + (*this)(2,1) = v21(0, 0); + (*this)(2,2) = v22(0, 0); + } + + MX_Xd_static(const MX_Xd_scalar& v00, const MX_Xd_scalar& v01, const MX_Xd_scalar& v02, const MX_Xd_scalar& v03, + const MX_Xd_scalar& v10, const MX_Xd_scalar& v11, const MX_Xd_scalar& v12, const MX_Xd_scalar& v13, + const MX_Xd_scalar& v20, const MX_Xd_scalar& v21, const MX_Xd_scalar& v22, const MX_Xd_scalar& v23, + const MX_Xd_scalar& v30, const MX_Xd_scalar& v31, const MX_Xd_scalar& v32, const MX_Xd_scalar& v33) : + casadi::MX(4, 4) + { + (*this)(0,0) = v00(0, 0); + (*this)(0,1) = v01(0, 0); + (*this)(0,2) = v02(0, 0); + (*this)(0,3) = v03(0, 0); + (*this)(1,0) = v10(0, 0); + (*this)(1,1) = v11(0, 0); + (*this)(1,2) = v12(0, 0); + (*this)(1,3) = v13(0, 0); + (*this)(2,0) = v20(0, 0); + (*this)(2,1) = v21(0, 0); + (*this)(2,2) = v22(0, 0); + (*this)(2,3) = v23(0, 0); + (*this)(3,0) = v30(0, 0); + (*this)(3,1) = v31(0, 0); + (*this)(3,2) = v32(0, 0); + (*this)(3,3) = v33(0, 0); + } + + MX_Xd_static(const MX_Xd_scalar& v00, const MX_Xd_scalar& v01, const MX_Xd_scalar& v02, const MX_Xd_scalar& v03, const MX_Xd_scalar& v04, const MX_Xd_scalar& v05, + const MX_Xd_scalar& v10, const MX_Xd_scalar& v11, const MX_Xd_scalar& v12, const MX_Xd_scalar& v13, const MX_Xd_scalar& v14, const MX_Xd_scalar& v15, + const MX_Xd_scalar& v20, const MX_Xd_scalar& v21, const MX_Xd_scalar& v22, const MX_Xd_scalar& v23, const MX_Xd_scalar& v24, const MX_Xd_scalar& v25, + const MX_Xd_scalar& v30, const MX_Xd_scalar& v31, const MX_Xd_scalar& v32, const MX_Xd_scalar& v33, const MX_Xd_scalar& v34, const MX_Xd_scalar& v35, + const MX_Xd_scalar& v40, const MX_Xd_scalar& v41, const MX_Xd_scalar& v42, const MX_Xd_scalar& v43, const MX_Xd_scalar& v44, const MX_Xd_scalar& v45, + const MX_Xd_scalar& v50, const MX_Xd_scalar& v51, const MX_Xd_scalar& v52, const MX_Xd_scalar& v53, const MX_Xd_scalar& v54, const MX_Xd_scalar& v55) : + casadi::MX(6, 6) + { + (*this)(0,0) = v00(0, 0); + (*this)(0,1) = v01(0, 0); + (*this)(0,2) = v02(0, 0); + (*this)(0,3) = v03(0, 0); + (*this)(0,4) = v04(0, 0); + (*this)(0,5) = v05(0, 0); + + (*this)(1,0) = v10(0, 0); + (*this)(1,1) = v11(0, 0); + (*this)(1,2) = v12(0, 0); + (*this)(1,3) = v13(0, 0); + (*this)(1,4) = v14(0, 0); + (*this)(1,5) = v15(0, 0); + + (*this)(2,0) = v20(0, 0); + (*this)(2,1) = v21(0, 0); + (*this)(2,2) = v22(0, 0); + (*this)(2,3) = v23(0, 0); + (*this)(2,4) = v24(0, 0); + (*this)(2,5) = v25(0, 0); + + (*this)(3,0) = v30(0, 0); + (*this)(3,1) = v31(0, 0); + (*this)(3,2) = v32(0, 0); + (*this)(3,3) = v33(0, 0); + (*this)(3,4) = v34(0, 0); + (*this)(3,5) = v35(0, 0); + + (*this)(4,0) = v40(0, 0); + (*this)(4,1) = v41(0, 0); + (*this)(4,2) = v42(0, 0); + (*this)(4,3) = v43(0, 0); + (*this)(4,4) = v44(0, 0); + (*this)(4,5) = v45(0, 0); + + (*this)(5,0) = v50(0, 0); + (*this)(5,1) = v51(0, 0); + (*this)(5,2) = v52(0, 0); + (*this)(5,3) = v53(0, 0); + (*this)(5,4) = v54(0, 0); + (*this)(5,5) = v55(0, 0); + } + + /// + /// \brief set For 3d Vector + /// \param v0 X + /// \param v1 Y + /// \param v2 Z + /// + void set( + const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2){ + (*this)(0) = v0; + (*this)(1) = v1; + (*this)(2) = v2; + } + /// + /// \brief set For Quaternion + /// \param v0 X + /// \param v1 Y + /// \param v2 Z + /// \param v3 W + /// + void set( + const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2, + const MX_Xd_scalar& v3){ + (*this)(0) = v0; + (*this)(1) = v1; + (*this)(2) = v2; + (*this)(3) = v3; + } + /// + /// \brief set For SpatialVector + /// \param v0 + /// \param v1 + /// \param v2 + /// \param v3 + /// \param v4 + /// \param v5 + /// + void set( + const MX_Xd_scalar& v0, + const MX_Xd_scalar& v1, + const MX_Xd_scalar& v2, + const MX_Xd_scalar& v3, + const MX_Xd_scalar& v4, + const MX_Xd_scalar& v5){ + (*this)(0) = v0; + (*this)(1) = v1; + (*this)(2) = v2; + (*this)(3) = v3; + (*this)(4) = v4; + (*this)(5) = v5; + } + + static MX_Xd_static Identity(){ + return casadi::MX::eye(ncols); + } + MX_Xd_static& setIdentity(){ + *this = casadi::MX::eye(ncols); + return *this; + } + + static MX_Xd_static Zero(){ + return MX_Xd_static::zeros(nrows, ncols); + } + MX_Xd_static& setZero(){ + *this = casadi::MX::zeros(this->rows(), this->cols()); + return *this; + } + + static MX_Xd_static One(){ + return MX_Xd_static::ones(nrows, ncols); + } + MX_Xd_static& setOnes(){ + *this = casadi::MX::ones(this->rows(), this->cols()); + return *this; + } + + + unsigned int rows() const { + return static_cast(this->casadi::MX::rows()); + } + + unsigned int cols() const { + return static_cast(this->casadi::MX::columns()); + } + + unsigned int size() const { + return rows() * cols(); + } + + template + MX_Xd_SubMatrix block ( + unsigned int row_start, + unsigned int col_start) + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + template + MX_Xd_static block( + unsigned int row_start, + unsigned int col_start) const + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + MX_Xd_SubMatrix block( + unsigned int row_start, + unsigned int col_start, + unsigned int row_count, + unsigned int col_count) + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + MX_Xd_dynamic block( + unsigned int row_start, + unsigned int col_start, + unsigned int row_count, + unsigned int col_count) const + { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(row_start), static_cast(row_start+row_count)), + casadi::Slice(static_cast(col_start), static_cast(col_start+col_count))); + } + + MX_Xd_SubMatrix operator[](unsigned int i) { + return (*this)(i); + } + MX_Xd_SubMatrix operator()(unsigned int i, unsigned int j=0) { + return this->casadi::MX::operator()( + casadi::Slice(static_cast(i), static_cast(i+1)), + casadi::Slice(static_cast(j), static_cast(j+1))); + } + MX_Xd_scalar operator[](unsigned int i) const { + return (*this)(i); + } + MX_Xd_scalar operator()(unsigned int i, unsigned int j=0) const { + return this->casadi::MX::operator()(i, j); + } + + + MX_Xd_scalar dot(const MX_Xd_static &other_vector) const { + return casadi::MX::dot(*this, other_vector); + } + + MX_Xd_static<3, 1> cross(const MX_Xd_static<3, 1> &other_vector) const { + MX_Xd_static<3, 1> result; + result[0] = (*this)[1] * other_vector[2] - (*this)[2] * other_vector[1]; + result[1] = (*this)[2] * other_vector[0] - (*this)[0] * other_vector[2]; + result[2] = (*this)[0] * other_vector[1] - (*this)[1] * other_vector[0]; + + return result; + } + + MX_Xd_static transpose() const { + return T(); + } + MX_Xd_static inverse() const { + if (ncols == 3 && nrows == 3){ + // computes the inverse of a matrix m + MX_Xd_scalar det = (*this)(0, 0) * ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) - + (*this)(0, 1) * ((*this)(1, 0) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 0)) + + (*this)(0, 2) * ((*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0)); + + MX_Xd_scalar invdet = 1 / det; + + MX_Xd_static<3, 3> minv; // inverse of matrix m + minv(0, 0) = ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) * invdet; + minv(0, 1) = ((*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2)) * invdet; + minv(0, 2) = ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1)) * invdet; + minv(1, 0) = ((*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2)) * invdet; + minv(1, 1) = ((*this)(0, 0) * (*this)(2, 2) - (*this)(0, 2) * (*this)(2, 0)) * invdet; + minv(1, 2) = ((*this)(1, 0) * (*this)(0, 2) - (*this)(0, 0) * (*this)(1, 2)) * invdet; + minv(2, 0) = ((*this)(1, 0) * (*this)(2, 1) - (*this)(2, 0) * (*this)(1, 1)) * invdet; + minv(2, 1) = ((*this)(2, 0) * (*this)(0, 1) - (*this)(0, 0) * (*this)(2, 1)) * invdet; + minv(2, 2) = ((*this)(0, 0) * (*this)(1, 1) - (*this)(1, 0) * (*this)(0, 1)) * invdet; + return minv; + } else { + return inv(*this); + } + } + + MX_Xd_scalar norm() const{ + return casadi::MX::norm_2(*this); + } + + MX_Xd_scalar squaredNorm() const{ + return norm() * norm(); + } + + void normalize() { + *this /= norm(); + } + + + void operator+=( + const MX_Xd_static& other) { + this->casadi::MX::operator+=(other); + } + MX_Xd_static operator+( + const MX_Xd_static& other) const { + MX_Xd_static out(*this); + return out.casadi::MX::operator+=(other); + } + void operator-=( + const MX_Xd_static& other) { + this->casadi::MX::operator-=(other); + } + MX_Xd_static operator-( + const MX_Xd_static& other) const { + MX_Xd_static out(*this); + return out.casadi::MX::operator-=(other); + } + template + void operator*=( + const MX_Xd_static& m2) { + *this = casadi::MX::mtimes(*this, m2); + } + template + MX_Xd_static operator*(const MX_Xd_static& other) const { + return casadi::MX::mtimes(*this, other); + } + MX_Xd_static operator*( + const MX_Xd_scalar& other) const { + return casadi::MX::mtimes(*this, other); + } + void operator*=( + const MX_Xd_scalar& other) { + *this = casadi::MX::mtimes(*this, other); + } + + void operator/=(const MX_Xd_scalar &scalar) { + this->casadi::MX::operator/=(scalar(0, 0)); + } + MX_Xd_static operator/(const MX_Xd_scalar &scalar) const { + MX_Xd_static result (*this); + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] /= scalar; + return result; + } + MX_Xd_static operator/( + const MX_Xd_SubMatrix& scalar) const { + MX_Xd_static result (*this); + for (unsigned int i = 0; i < nrows * ncols; i++) + result[i] /= scalar; + return result; + } +}; + +} + +/* MX_XD_STATIC_H */ +#endif + diff --git a/include/rbdl/CasadiMath/MX_Xd_subMatrix.h b/include/rbdl/CasadiMath/MX_Xd_subMatrix.h new file mode 100644 index 00000000..bc7db6f6 --- /dev/null +++ b/include/rbdl/CasadiMath/MX_Xd_subMatrix.h @@ -0,0 +1,67 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef MX_XD_SUBMATRIX_H +#define MX_XD_SUBMATRIX_H + +#include +#include +#include + +#include + +namespace RBDLCasadiMath { + +class MX_Xd_SubMatrix : public casadi::SubMatrix{ +public: + MX_Xd_SubMatrix(casadi::MX& mat, const casadi::Slice& i, const casadi::Slice& j) : + casadi::SubMatrix(mat, i, j) + { + + } + MX_Xd_SubMatrix(MX_Xd_SubMatrix& mat, const casadi::Slice& i, const casadi::Slice& j) : + casadi::SubMatrix(mat, i, j) + { + + } + MX_Xd_SubMatrix(const casadi::SubMatrix& me) : + casadi::SubMatrix(me) + { + + } + + virtual ~MX_Xd_SubMatrix(){ + + } + + MX_Xd_scalar norm() const { + return casadi::SubMatrix::norm_2(*this); + } + + void normalize() { + *this /= norm(); + } + + void operator=(const casadi::SubMatrix& submat){ + this->casadi::SubMatrix::operator=(submat); + } + void operator=(const casadi::MX& mat){ + this->casadi::SubMatrix::operator =(mat); + } + void operator=(const MX_Xd_scalar& mat){ + this->casadi::SubMatrix::operator =(mat); + } + void operator=(double mat){ + this->casadi::SubMatrix::operator =(mat); + } +}; + +} + +/* MX_XD_SUBMATRIX_H */ +#endif + diff --git a/include/rbdl/CasadiMath/MX_Xd_utils.h b/include/rbdl/CasadiMath/MX_Xd_utils.h new file mode 100644 index 00000000..a61eaa55 --- /dev/null +++ b/include/rbdl/CasadiMath/MX_Xd_utils.h @@ -0,0 +1,397 @@ +#ifndef MX_XD_UTILS_H +#define MX_XD_UTILS_H + +#include "rbdl/CasadiMath/MX_Xd_scalar.h" +#include "rbdl/CasadiMath/MX_Xd_static.h" +#include "rbdl/CasadiMath/MX_Xd_dynamic.h" +#include "rbdl/CasadiMath/MX_Xd_subMatrix.h" + + +namespace RBDLCasadiMath { + +template +MX_Xd_static operator*( + const MX_Xd_static& me, + const MX_Xd_scalar& other) { + return casadi::MX::mtimes(me, other); +} +template +MX_Xd_static operator*( + const MX_Xd_static& me, + const double& other) { + return casadi::MX::mtimes(me, other); +} +template +MX_Xd_static operator*( + const MX_Xd_scalar& other, + const MX_Xd_static& me + ) { + return casadi::MX::mtimes(me, other); +} +template +MX_Xd_static operator*( + const double& other, + const MX_Xd_static& me + ) { + return casadi::MX::mtimes(me, other); +} + +template +MX_Xd_static operator+( + const MX_Xd_static& me, + const MX_Xd_dynamic& other) { + MX_Xd_static out(me); + return out.casadi::MX::operator+=(other); +} +template +MX_Xd_static operator+( + const MX_Xd_static& me, + const MX_Xd_SubMatrix& other) { + MX_Xd_static out(me); + return out.casadi::MX::operator+=(other); +} +inline MX_Xd_scalar operator+( + MX_Xd_scalar me, + const MX_Xd_SubMatrix& other) { + return me.casadi::MX::operator+=(other); +} +inline MX_Xd_scalar operator+( + MX_Xd_scalar me, + const double& other) { + return me.casadi::MX::operator+=(other); +} +template +MX_Xd_static operator+( + const MX_Xd_SubMatrix& me, + const MX_Xd_static& other) { + MX_Xd_static out(other); + return out.casadi::MX::operator+=(me); +} + +template +MX_Xd_static operator-( + const MX_Xd_static& other) { + return other.casadi::MX::operator-(); +} +template +MX_Xd_static operator-( + const MX_Xd_static& me, + const MX_Xd_dynamic& other) { + MX_Xd_static out(me); + return out.casadi::MX::operator-=(other); +} +template +MX_Xd_static operator-( + const MX_Xd_static& me, + const MX_Xd_SubMatrix& other) { + MX_Xd_static out(me); + return out.casadi::MX::operator-=(other); +} +template +MX_Xd_static operator-( + const MX_Xd_SubMatrix& me, + const MX_Xd_static& other) { + MX_Xd_static out(other); + return out.casadi::MX::operator-=(me); +} + +inline MX_Xd_dynamic operator*( + const MX_Xd_SubMatrix& me, + const MX_Xd_scalar& other){ + return MX_Xd_SubMatrix::times(me, other); +} +inline MX_Xd_dynamic operator*( + const MX_Xd_SubMatrix& me, + const MX_Xd_SubMatrix& other){ + return MX_Xd_SubMatrix::mtimes(me, other); +} + +inline MX_Xd_SubMatrix operator/( + const MX_Xd_SubMatrix& me, + const MX_Xd_SubMatrix& scalar) { + MX_Xd_SubMatrix result (me); + result.MX_Xd_SubMatrix::operator/=(scalar); + return result; +} +inline MX_Xd_SubMatrix operator/( + const MX_Xd_SubMatrix& me, + const double& scalar) { + MX_Xd_SubMatrix result (me); + result.MX_Xd_SubMatrix::operator/=(scalar); + return result; +} + +inline MX_Xd_dynamic operator*( + const MX_Xd_scalar& m1, + const MX_Xd_dynamic m2){ + return casadi::MX::mtimes(m1, m2); +} +inline MX_Xd_dynamic operator*( + const double& m1, + const MX_Xd_dynamic m2){ + return casadi::MX::mtimes(m1, m2); +} +inline MX_Xd_dynamic operator*( + const double& m1, + const MX_Xd_SubMatrix m2){ + return casadi::MX::mtimes(m1, m2); +} +template +MX_Xd_dynamic operator*( + const MX_Xd_static& m1, + const MX_Xd_dynamic m2){ + return casadi::MX::mtimes(m1, m2); +} +template +MX_Xd_dynamic operator*( + const MX_Xd_dynamic m1, + const MX_Xd_static& m2){ + return casadi::MX::mtimes(m1, m2); +} +template +MX_Xd_dynamic operator*( + const MX_Xd_SubMatrix& m1, + const MX_Xd_static& m2){ + return casadi::MX::mtimes(m1, m2); +} +template +MX_Xd_dynamic operator*( + const MX_Xd_static& m1, + const MX_Xd_SubMatrix& m2){ + return casadi::MX::mtimes(m1, m2); +} + +inline MX_Xd_dynamic operator/(double scalar, MX_Xd_SubMatrix mat){ + for (unsigned int i=0; i +inline MX_Xd_dynamic operator/(double scalar, MX_Xd_static mat){ + for (unsigned int i=0; i +MX_Xd_static operator+( + const MX_Xd_dynamic& me, + const MX_Xd_static& other + ) { + MX_Xd_static out(me); + return out.casadi::MX::operator+=(other); +} +inline MX_Xd_dynamic operator-( + const MX_Xd_dynamic& me) { + return me.casadi::MX::operator-(); +} +inline MX_Xd_dynamic operator-( + const MX_Xd_SubMatrix& me) { + return me.casadi::MX::operator-(); +} +inline MX_Xd_dynamic operator-( + MX_Xd_SubMatrix me, + const MX_Xd_scalar& scalar){ + return me.casadi::SubMatrix::operator-=(scalar); +} +inline MX_Xd_scalar operator-( + MX_Xd_scalar me, + const MX_Xd_SubMatrix& other) { + return me.casadi::MX::operator-=(other); +} +inline MX_Xd_scalar operator+( + const MX_Xd_dynamic& me, + const MX_Xd_scalar& other + ) { + MX_Xd_scalar out(me); + return out.casadi::MX::operator+=(other); +} +inline MX_Xd_scalar operator+( + const MX_Xd_scalar& me, + const MX_Xd_dynamic& other + ) { + MX_Xd_scalar out(me); + return out.casadi::MX::operator+=(other); +} +inline MX_Xd_dynamic operator+( + const MX_Xd_SubMatrix& me, + const MX_Xd_dynamic& other + ) { + MX_Xd_dynamic out(other); + return out.casadi::MX::operator+=(me); +} +inline MX_Xd_dynamic operator+( + const MX_Xd_dynamic& me, + const MX_Xd_SubMatrix& other + ) { + MX_Xd_dynamic out(me); + return out.casadi::MX::operator+=(other); +} + +inline MX_Xd_dynamic operator+( + MX_Xd_SubMatrix me, + const MX_Xd_SubMatrix& other){ + return me.casadi::SubMatrix::operator+=(other); +} + +template +inline MX_Xd_static operator-( + const MX_Xd_dynamic& me, + const MX_Xd_static& other + ) { + MX_Xd_static out(me); + return out.casadi::MX::operator-=(other); +} +inline MX_Xd_scalar operator-( + const MX_Xd_dynamic& me, + const MX_Xd_scalar& other + ) { + MX_Xd_scalar out(me); + return out.casadi::MX::operator-=(other); +} +inline MX_Xd_scalar operator-( + const MX_Xd_scalar& me, + const MX_Xd_dynamic& other + ) { + MX_Xd_scalar out(me); + return out.casadi::MX::operator-=(other); +} +inline MX_Xd_dynamic operator-( + const MX_Xd_SubMatrix& me, + const MX_Xd_dynamic& other + ) { + MX_Xd_dynamic out(-other); + return out.casadi::MX::operator+=(me); +} +inline MX_Xd_dynamic operator-( + const MX_Xd_dynamic& me, + const MX_Xd_SubMatrix& other + ) { + MX_Xd_dynamic out(me); + return out.casadi::MX::operator-=(other); +} +inline MX_Xd_dynamic operator-( + const MX_Xd_SubMatrix& me, + const MX_Xd_SubMatrix& other + ) { + MX_Xd_dynamic out(me); + return out.casadi::MX::operator-=(other); +} + + +inline MX_Xd_scalar operator*( + double other, + const MX_Xd_scalar& me + ) { + return casadi::MX::times(me, other); +} + +inline MX_Xd_scalar operator+( + double other, + const MX_Xd_scalar& me) { + MX_Xd_scalar out(me); + return out.casadi::MX::operator+=(other); +} + +inline MX_Xd_scalar operator-( + const MX_Xd_scalar& other) { + return other.casadi::MX::operator-(); +} +inline MX_Xd_scalar operator-( + double other, + const MX_Xd_scalar& me) { + MX_Xd_scalar out(-me); + return out.casadi::MX::operator+=(other); +} + +} + + +inline RBDLCasadiMath::MX_Xd_scalar exp(const RBDLCasadiMath::MX_Xd_scalar& x){ + return casadi::MX::exp(x); +} + +namespace std { +using namespace RBDLCasadiMath; + +inline MX_Xd_scalar sqrt(const MX_Xd_scalar& x){ + return casadi::MX::sqrt(x); +} +inline MX_Xd_scalar sin(const MX_Xd_scalar& x){ + return casadi::MX::sin(x); +} +inline MX_Xd_scalar asin(const MX_Xd_scalar& x){ + return casadi::MX::asin(x); +} +inline MX_Xd_scalar cos(const MX_Xd_scalar& x){ + return casadi::MX::cos(x); +} +inline MX_Xd_scalar acos(const MX_Xd_scalar& x){ + return casadi::MX::acos(x); +} +inline MX_Xd_scalar tan(const MX_Xd_scalar& x){ + return casadi::MX::tan(x); +} +inline MX_Xd_scalar atan2(const MX_Xd_scalar& x, const MX_Xd_scalar& y){ + return casadi::MX::atan2(x,y); +} +inline bool isnan(const casadi::MX& x){ + return !x.is_regular(); +} + +template +inline MX_Xd_dynamic fabs(const MX_Xd_dynamic& m){ + return casadi::MX::abs(m); +} +template +inline MX_Xd_scalar fabs(const MX_Xd_static& m){ + return casadi::MX::abs(m); +} +inline MX_Xd_scalar fabs(const MX_Xd_scalar& m){ + return casadi::MX::abs(m); +} + +template +inline MX_Xd_static pow(const MX_Xd_static& m, int exponent){ + return casadi::MX::mpower(m, exponent); +} +template +inline MX_Xd_static pow(const MX_Xd_static& m, unsigned int exponent){ + return casadi::MX::mpower(m, exponent); +} +template +inline MX_Xd_static pow(const MX_Xd_static& m, double exponent){ + return casadi::MX::mpower(m, exponent); +} +inline MX_Xd_scalar pow(const MX_Xd_scalar& m, int exponent){ + return casadi::MX::mpower(m, exponent); +} +inline MX_Xd_scalar pow(const MX_Xd_scalar& m, unsigned int exponent){ + return casadi::MX::mpower(m, exponent); +} +inline MX_Xd_scalar pow(const MX_Xd_scalar& m, double exponent){ + return casadi::MX::mpower(m, exponent); +} + +} + +#endif diff --git a/include/rbdl/Constraints.h b/include/rbdl/Constraints.h index 5536922c..b5d8e445 100644 --- a/include/rbdl/Constraints.h +++ b/include/rbdl/Constraints.h @@ -942,9 +942,11 @@ struct RBDL_DLLAPI ConstraintSet { Math::VectorNd py; Math::VectorNd pz; +#ifndef RBDL_USE_CASADI_MATH /// Workspace for the QR decomposition of the null-space method Eigen::HouseholderQR GT_qr; Eigen::FullPivHouseholderQR GPT_full_qr; +#endif Math::MatrixNd GT_qr_Q; Math::MatrixNd GPT; @@ -1087,6 +1089,7 @@ void CalcConstrainedSystemVariables ( std::vector *f_ext = NULL ); +#ifndef RBDL_USE_CASADI_MATH /** \brief Computes a feasible initial value of the generalized joint positions. * * \param model the model @@ -1113,6 +1116,7 @@ bool CalcAssemblyQ( double tolerance = 1e-12, unsigned int max_iter = 100 ); +#endif /** \brief Computes a feasible initial value of the generalized joint velocities. * @@ -1212,6 +1216,7 @@ void ForwardDynamicsConstraintsRangeSpaceSparse ( std::vector *f_ext = NULL ); +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace ( Model &model, @@ -1223,6 +1228,7 @@ void ForwardDynamicsConstraintsNullSpace ( bool update_kinematics = true, std::vector *f_ext = NULL ); +#endif /** \brief Computes forward dynamics that accounts for active contacts in * ConstraintSet. @@ -1300,7 +1306,7 @@ void ForwardDynamicsContactsKokkevis ( ); - +#ifndef RBDL_USE_CASADI_MATH /** @brief A relaxed inverse-dynamics operator that can be applied to under-actuated or fully-actuated constrained multibody systems. @@ -1557,7 +1563,7 @@ void InverseDynamicsConstraintsRelaxed( Math::VectorNd &TauOutput, bool update_kinematics=true, std::vector *f_ext = NULL); - +#endif /** @brief An inverse-dynamics operator that can be applied to fully-actuated constrained systems. @@ -1724,6 +1730,7 @@ void InverseDynamicsConstraints( bool update_kinematics=true, std::vector *f_ext = NULL); +#ifndef RBDL_USE_CASADI_MATH /** \brief A method to evaluate if the constrained system is fully actuated. @@ -1755,6 +1762,7 @@ bool isConstrainedSystemFullyActuated( ConstraintSet &CS, bool update_kinematics=true, std::vector *f_ext = NULL); +#endif /** \brief Computes contact gain by constructing and solving the full lagrangian * equation @@ -1829,6 +1837,7 @@ void ComputeConstraintImpulsesRangeSpaceSparse ( Math::VectorNd &QDotPlusOutput ); +#ifndef RBDL_USE_CASADI_MATH /** \brief Resolves contact gain using SolveContactSystemNullSpace() * \param model rigid body model * \param Q state vector of the internal joints @@ -1844,6 +1853,7 @@ void ComputeConstraintImpulsesNullSpace ( ConstraintSet &CS, Math::VectorNd &QDotPlusOutput ); +#endif /** \brief Solves the full contact system directly, i.e. simultaneously for * contact forces and joint accelerations. diff --git a/include/rbdl/Joint.h b/include/rbdl/Joint.h index 97dbad5a..4694c413 100644 --- a/include/rbdl/Joint.h +++ b/include/rbdl/Joint.h @@ -440,7 +440,9 @@ struct RBDL_DLLAPI Joint { } else if (joint_type == JointTypePrismatic) { // make sure we have a unit axis +#ifndef RBDL_USE_CASADI_MATH assert (joint_axis.squaredNorm() == 1.); +#endif mJointAxes[0].set ( 0., 0., 0., @@ -465,15 +467,31 @@ struct RBDL_DLLAPI Joint { mDoFCount = 1; mJointAxes = new Math::SpatialVector[mDoFCount]; mJointAxes[0] = Math::SpatialVector (axis_0); + + // TODO this has to be properly determined AND test case. Try Matt's dot product idea +#ifdef RBDL_USE_CASADI_MATH + if (!axis_0[0].is_zero()) { +#else if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.)) { +#endif mJointType = JointTypeRevoluteX; +#ifdef RBDL_USE_CASADI_MATH + } else if (!axis_0[1].is_zero()) { +#else } else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.)) { +#endif mJointType = JointTypeRevoluteY; +#ifdef RBDL_USE_CASADI_MATH + } else if (!axis_0[2].is_zero()) { +#else } else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.)) { +#endif mJointType = JointTypeRevoluteZ; - } else if (axis_0[0] == 0 && - axis_0[1] == 0 && - axis_0[2] == 0) { +#ifdef RBDL_USE_CASADI_MATH + } else if (axis_0[0].is_zero() && axis_0[1].is_zero() && axis_0[2].is_zero()) { +#else + } else if (axis_0[0] == 0. && axis_0[1] == 0. && axis_0[2] == 0.) { +#endif mJointType = JointTypePrismatic; } else { mJointType = JointTypeHelical; @@ -662,7 +680,10 @@ struct RBDL_DLLAPI Joint { */ bool validate_spatial_axis (Math::SpatialVector &axis) { - +#ifdef RBDL_USE_CASADI_MATH + // If using casadi, the axes won't be validated + return true; +#else bool axis_rotational = false; bool axis_translational = false; @@ -686,6 +707,7 @@ struct RBDL_DLLAPI Joint { } return axis_rotational != axis_translational; +#endif } /// \brief The spatial axes of the joint diff --git a/include/rbdl/Kinematics.h b/include/rbdl/Kinematics.h index cf849f9b..7d1462b1 100644 --- a/include/rbdl/Kinematics.h +++ b/include/rbdl/Kinematics.h @@ -311,6 +311,7 @@ RBDL_DLLAPI bool update_kinematics = true ); +#ifndef RBDL_USE_CASADI_MATH /** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) * * \param model rigid body model @@ -416,6 +417,7 @@ RBDL_DLLAPI bool InverseKinematics ( InverseKinematicsConstraintSet &CS, Math::VectorNd &Qres ); +#endif /** @} */ diff --git a/include/rbdl/Model.h b/include/rbdl/Model.h index 5f005edc..9acd12f2 100644 --- a/include/rbdl/Model.h +++ b/include/rbdl/Model.h @@ -25,9 +25,11 @@ // as members need to have a special allocater. This can be achieved with // the following macro. +#ifndef RBDL_USE_CASADI_MATH EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody) +#endif /** \brief Namespace for all structures of the RigidBodyDynamics library */ diff --git a/include/rbdl/Quaternion.h b/include/rbdl/Quaternion.h index 324a5549..9ad39fa9 100644 --- a/include/rbdl/Quaternion.h +++ b/include/rbdl/Quaternion.h @@ -9,6 +9,7 @@ #define RBDL_QUATERNION_H #include +#include namespace RigidBodyDynamics { @@ -26,7 +27,7 @@ class Quaternion : public Vector4d { Quaternion (const Vector4d &vec4) : Vector4d (vec4) {} - Quaternion (double x, double y, double z, double w): + Quaternion (Scalar x, Scalar y, Scalar z, Scalar w): Vector4d (x, y, z, w) {} Quaternion operator* (const double &s) const { @@ -68,30 +69,41 @@ class Quaternion : public Vector4d { Quaternion slerp (double alpha, const Quaternion &quat) const { // check whether one of the two has 0 length - double s = std::sqrt (squaredNorm() * quat.squaredNorm()); + Scalar s = std::sqrt (squaredNorm() * quat.squaredNorm()); - // division by 0.f is unhealthy! - assert (s != 0.); + // division by 0.f is unhealthy + #ifndef RBDL_USE_CASADI_MATH + assert (s != 0.); + #endif - double angle = acos (dot(quat) / s); + Scalar angle = acos (dot(quat) / s); +#ifndef RBDL_USE_CASADI_MATH if (angle == 0. || std::isnan(angle)) { return *this; } assert(!std::isnan(angle)); +#endif - double d = 1. / std::sin (angle); - double p0 = std::sin ((1. - alpha) * angle); - double p1 = std::sin (alpha * angle); + Scalar d = 1. / std::sin (angle); + Scalar p0 = std::sin ((1. - alpha) * angle); + Scalar p1 = std::sin (alpha * angle); +#ifdef RBDL_USE_CASADI_MATH + return Quaternion (casadi::MX::if_else(casadi::MX::lt(dot (quat), 0.), + Quaternion( ((*this) * p0 - quat * p1) * d), + Quaternion( ((*this) * p0 + quat * p1) * d)) ); +#else if (dot (quat) < 0.) { - return Quaternion( ((*this) * p0 - quat * p1) * d); + return Quaternion( ((*this) * p0 - quat * p1) * d); + } else { + return Quaternion( ((*this) * p0 + quat * p1) * d); } - return Quaternion( ((*this) * p0 + quat * p1) * d); +#endif } - static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) { - double d = axis.norm(); - double s2 = std::sin (angle_rad * 0.5) / d; + static Quaternion fromAxisAngle (const Vector3d &axis, Scalar angle_rad) { + Scalar d = axis.norm(); + Scalar s2 = std::sin (angle_rad * 0.5) / d; return Quaternion ( axis[0] * s2, axis[1] * s2, @@ -101,7 +113,7 @@ class Quaternion : public Vector4d { } static Quaternion fromMatrix (const Matrix3d &mat) { - double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; + Scalar w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; return Quaternion ( (mat(1,2) - mat(2,1)) / (w * 4.), (mat(2,0) - mat(0,2)) / (w * 4.), @@ -128,10 +140,10 @@ class Quaternion : public Vector4d { } Matrix3d toMatrix() const { - double x = (*this)[0]; - double y = (*this)[1]; - double z = (*this)[2]; - double w = (*this)[3]; + Scalar x = (*this)[0]; + Scalar y = (*this)[1]; + Scalar z = (*this)[2]; + Scalar w = (*this)[3]; return Matrix3d ( 1 - 2*y*y - 2*z*z, 2*x*y + 2*w*z, @@ -170,7 +182,7 @@ class Quaternion : public Vector4d { } Quaternion timeStep (const Vector3d &omega, double dt) { - double omega_norm = omega.norm(); + Scalar omega_norm = omega.norm(); return Quaternion::fromAxisAngle (omega / omega_norm, dt * omega_norm) * (*this); } diff --git a/include/rbdl/SpatialAlgebraOperators.h b/include/rbdl/SpatialAlgebraOperators.h index 5da48651..e8208036 100644 --- a/include/rbdl/SpatialAlgebraOperators.h +++ b/include/rbdl/SpatialAlgebraOperators.h @@ -27,20 +27,20 @@ inline Matrix3d VectorCrossMatrix (const Vector3d &vector) { struct RBDL_DLLAPI SpatialRigidBodyInertia { SpatialRigidBodyInertia() : m (0.), - h (Vector3d::Zero(3,1)), + h (Vector3d::Zero()), Ixx (0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.) {} SpatialRigidBodyInertia ( - double mass, const Vector3d &com_mass, const Matrix3d &inertia) : + Scalar mass, const Vector3d &com_mass, const Matrix3d &inertia) : m (mass), h (com_mass), Ixx (inertia(0,0)), Iyx (inertia(1,0)), Iyy(inertia(1,1)), Izx (inertia(2,0)), Izy(inertia(2,1)), Izz(inertia(2,2)) { } - SpatialRigidBodyInertia (double m, const Vector3d &h, - const double &Ixx, - const double &Iyx, const double &Iyy, - const double &Izx, const double &Izy, const double &Izz + SpatialRigidBodyInertia (Scalar m, const Vector3d &h, + const Scalar &Ixx, + const Scalar &Iyx, const Scalar &Iyy, + const Scalar &Izx, const Scalar &Izy, const Scalar &Izz ) : m (m), h (h), Ixx (Ixx), @@ -90,7 +90,7 @@ struct RBDL_DLLAPI SpatialRigidBodyInertia { result.block<3,3>(0,3) = VectorCrossMatrix(h); result.block<3,3>(3,0) = - VectorCrossMatrix(h); - result.block<3,3>(3,3) = Matrix3d::Identity(3,3) * m; + result.block<3,3>(3,3) = Matrix3d::Identity() * m; return result; } @@ -113,7 +113,7 @@ struct RBDL_DLLAPI SpatialRigidBodyInertia { mat(5,3) = 0.; mat(5,4) = 0.; mat(5,5) = m; } - static SpatialRigidBodyInertia createFromMassComInertiaC (double mass, const Vector3d &com, const Matrix3d &inertia_C) { + static SpatialRigidBodyInertia createFromMassComInertiaC (Scalar mass, const Vector3d &com, const Matrix3d &inertia_C) { SpatialRigidBodyInertia result; result.m = mass; result.h = com * mass; @@ -128,11 +128,11 @@ struct RBDL_DLLAPI SpatialRigidBodyInertia { } /// Mass - double m; + Scalar m; /// Coordinates of the center of mass Vector3d h; /// Inertia expressed at the origin - double Ixx, Iyx, Iyy, Izx, Izy, Izz; + Scalar Ixx, Iyx, Iyy, Izx, Izy, Izz; }; /** \brief Compact representation of spatial transformations. @@ -144,8 +144,8 @@ struct RBDL_DLLAPI SpatialRigidBodyInertia { */ struct RBDL_DLLAPI SpatialTransform { SpatialTransform() : - E (Matrix3d::Identity(3,3)), - r (Vector3d::Zero(3,1)) + E (Matrix3d::Identity()), + r (Vector3d::Zero()) {} SpatialTransform (const Matrix3d &rotation, const Vector3d &translation) : E (rotation), @@ -253,7 +253,7 @@ struct RBDL_DLLAPI SpatialTransform { ); SpatialMatrix result; result.block<3,3>(0,0) = E; - result.block<3,3>(0,3) = Matrix3d::Zero(3,3); + result.block<3,3>(0,3) = Matrix3d::Zero(); result.block<3,3>(3,0) = -_Erx; result.block<3,3>(3,3) = E; @@ -270,7 +270,7 @@ struct RBDL_DLLAPI SpatialTransform { SpatialMatrix result; result.block<3,3>(0,0) = E; result.block<3,3>(0,3) = -_Erx; - result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,0) = Matrix3d::Zero(); result.block<3,3>(3,3) = E; return result; @@ -286,7 +286,7 @@ struct RBDL_DLLAPI SpatialTransform { SpatialMatrix result; result.block<3,3>(0,0) = E.transpose(); result.block<3,3>(0,3) = -_Erx.transpose(); - result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,0) = Matrix3d::Zero(); result.block<3,3>(3,3) = E.transpose(); return result; @@ -327,8 +327,8 @@ inline std::ostream& operator<<(std::ostream& output, const SpatialTransform &X) return output; } -inline SpatialTransform Xrot (double angle_rad, const Vector3d &axis) { - double s, c; +inline SpatialTransform Xrot (Scalar angle_rad, const Vector3d &axis) { + Scalar s, c; s = sin(angle_rad); c = cos(angle_rad); @@ -351,8 +351,8 @@ inline SpatialTransform Xrot (double angle_rad, const Vector3d &axis) { ); } -inline SpatialTransform Xrotx (const double &xrot) { - double s, c; +inline SpatialTransform Xrotx (const Scalar &xrot) { + Scalar s, c; s = sin (xrot); c = cos (xrot); return SpatialTransform ( @@ -365,8 +365,8 @@ inline SpatialTransform Xrotx (const double &xrot) { ); } -inline SpatialTransform Xroty (const double &yrot) { - double s, c; +inline SpatialTransform Xroty (const Scalar &yrot) { + Scalar s, c; s = sin (yrot); c = cos (yrot); return SpatialTransform ( @@ -379,8 +379,8 @@ inline SpatialTransform Xroty (const double &yrot) { ); } -inline SpatialTransform Xrotz (const double &zrot) { - double s, c; +inline SpatialTransform Xrotz (const Scalar &zrot) { + Scalar s, c; s = sin (zrot); c = cos (zrot); return SpatialTransform ( @@ -395,7 +395,7 @@ inline SpatialTransform Xrotz (const double &zrot) { inline SpatialTransform Xtrans (const Vector3d &r) { return SpatialTransform ( - Matrix3d::Identity(3,3), + Matrix3d::Identity(), r ); } @@ -439,7 +439,7 @@ inline SpatialVector crossf (const SpatialVector &v1, const SpatialVector &v2) { v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5], -v1[1] * v2[0] + v1[0] * v2[1] - v1[4] * v2[3] + v1[3] * v2[4], - v1[2] * v2[4] + v1[1] * v2[5], - + v1[2] * v2[3] - v1[0] * v2[5], + v1[2] * v2[3] - v1[0] * v2[5], - v1[1] * v2[3] + v1[0] * v2[4] ); } diff --git a/include/rbdl/rbdl_casadi_config.h.cmake b/include/rbdl/rbdl_casadi_config.h.cmake new file mode 100644 index 00000000..71bc7620 --- /dev/null +++ b/include/rbdl/rbdl_casadi_config.h.cmake @@ -0,0 +1,96 @@ +/* +* RBDL - Rigid Body Dynamics Library +* Copyright (c) 2011-2018 Martin Felis +* +* Licensed under the zlib license. See LICENSE for more details. +*/ + +#ifndef RBDL_CONFIG_H +#define RBDL_CONFIG_H + +#define RBDL_API_VERSION (@RBDL_VERSION_MAJOR@ << 16) + (@RBDL_VERSION_MINOR@ << 8) + @RBDL_VERSION_PATCH@ + +#cmakedefine RBDL_ENABLE_LOGGING +#cmakedefine RBDL_BUILD_COMMIT "@RBDL_BUILD_COMMIT@" +#cmakedefine RBDL_BUILD_TYPE "@RBDL_BUILD_TYPE@" +#cmakedefine RBDL_BUILD_BRANCH "@RBDL_BUILD_BRANCH@" +#cmakedefine RBDL_BUILD_ADDON_MUSCLE_FITTING +#cmakedefine RBDL_BUILD_ADDON_MUSCLE +#cmakedefine RBDL_BUILD_COMPILER_ID "@RBDL_BUILD_COMPILER_ID@" +#cmakedefine RBDL_BUILD_COMPILER_VERSION "@RBDL_BUILD_COMPILER_VERSION@" +#cmakedefine RBDL_BUILD_ADDON_LUAMODEL +#cmakedefine RBDL_BUILD_ADDON_URDFREADER +#cmakedefine RBDL_BUILD_STATIC +#cmakedefine RBDL_USE_ROS_URDF_LIBRARY +#cmakedefine RBDL_USE_CASADI_MATH + + +/* compatibility defines */ +#ifdef _WIN32 +#define __func__ __FUNCTION__ +#define M_PI 3.1415926535897932384 +#pragma warning(disable:4251) /*no DLL interface for type of member of exported class*/ +#pragma warning(disable:4275) /*no DLL interface for base class of exported class*/ +#endif + +// Handle portable symbol export. +// Defining manually which symbol should be exported is required +// under Windows whether MinGW or MSVC is used. +// +// The headers then have to be able to work in two different modes: +// - dllexport when one is building the library, +// - dllimport for clients using the library. +// +// On Linux, set the visibility accordingly. If C++ symbol visibility +// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility +# if defined _WIN32 || defined __CYGWIN__ +// On Microsoft Windows, use dllimport and dllexport to tag symbols. +# define RBDL_DLLIMPORT __declspec(dllimport) +# define RBDL_DLLEXPORT __declspec(dllexport) +# define RBDL_DLLLOCAL +# else +// On Linux, for GCC >= 4, tag symbols using GCC extension. +# if __GNUC__ >= 4 +# define RBDL_DLLIMPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLEXPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLLOCAL __attribute__ ((visibility("hidden"))) +# else +// Otherwise (GCC < 4 or another compiler is used), export everything. +# define RBDL_DLLIMPORT +# define RBDL_DLLEXPORT +# define RBDL_DLLLOCAL +# endif // __GNUC__ >= 4 +# endif // defined _WIN32 || defined __CYGWIN__ + +# ifdef RBDL_BUILD_STATIC +// If one is using the library statically, get rid of +// extra information. +# define RBDL_DLLAPI +# define RBDL_LOCAL +# else +// Depending on whether one is building or using the +// library define DLLAPI to import or export. +# ifdef rbdl_EXPORTS +# define RBDL_DLLAPI RBDL_DLLEXPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLIMPORT +# elif rbdl_casadi_EXPORTS +# define RBDL_DLLAPI RBDL_DLLEXPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLIMPORT +# elif rbdl_urdfreader_EXPORTS +# define RBDL_DLLAPI RBDL_DLLIMPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLEXPORT +# elif rbdl_geometry_EXPORTS +# define RBDL_DLLAPI RBDL_DLLIMPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLEXPORT +# elif rbdl_luamodel_EXPORTS +# define RBDL_DLLAPI RBDL_DLLIMPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLEXPORT +# else +# define RBDL_DLLAPI RBDL_DLLIMPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLIMPORT +# endif // RBDL_EXPORTS +# define RBDL_LOCAL RBDL_DLLLOCAL +# endif // RBDL_BUILD_STATIC + + +#endif diff --git a/include/rbdl/rbdl_config.h.cmake b/include/rbdl/rbdl_config.h.cmake index 2e874e79..e90a0447 100644 --- a/include/rbdl/rbdl_config.h.cmake +++ b/include/rbdl/rbdl_config.h.cmake @@ -72,6 +72,9 @@ # ifdef rbdl_EXPORTS # define RBDL_DLLAPI RBDL_DLLEXPORT # define RBDL_ADDON_DLLAPI RBDL_DLLIMPORT +# elif rbdl_casadi_EXPORTS +# define RBDL_DLLAPI RBDL_DLLEXPORT +# define RBDL_ADDON_DLLAPI RBDL_DLLIMPORT # elif rbdl_urdfreader_EXPORTS # define RBDL_DLLAPI RBDL_DLLIMPORT # define RBDL_ADDON_DLLAPI RBDL_DLLEXPORT diff --git a/include/rbdl/rbdl_eigenmath.h b/include/rbdl/rbdl_eigenmath.h index f70f277e..917eb410 100644 --- a/include/rbdl/rbdl_eigenmath.h +++ b/include/rbdl/rbdl_eigenmath.h @@ -208,6 +208,61 @@ class RBDL_TEMPLATE_DLLAPI SpatialVector_t : public Eigen::Matrix } }; +class RBDL_TEMPLATE_DLLAPI Matrix4_t : public Eigen::Matrix +{ + public: + typedef Eigen::Matrix Base; + + template + Matrix4_t(const Eigen::MatrixBase& other) + : Eigen::Matrix(other) + {} + + template + Matrix4_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Matrix4_t() + {} + + EIGEN_STRONG_INLINE Matrix4_t( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03 + , m10, m11, m12, m13 + , m20, m21, m22, m23 + , m30, m31, m32, m33 + ; + } + + void set( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03 + , m10, m11, m12, m13 + , m20, m21, m22, m23 + , m30, m31, m32, m33 + ; + } +}; + class RBDL_TEMPLATE_DLLAPI SpatialMatrix_t : public Eigen::Matrix { public: diff --git a/include/rbdl/rbdl_math.h b/include/rbdl/rbdl_math.h index c80fd4c2..58cbf018 100644 --- a/include/rbdl/rbdl_math.h +++ b/include/rbdl/rbdl_math.h @@ -10,6 +10,28 @@ #include "rbdl/rbdl_config.h" +#ifdef RBDL_USE_CASADI_MATH +#include "CasadiMath/MX_Xd_utils.h" + +typedef RBDLCasadiMath::MX_Xd_scalar Vector1_t; +typedef RBDLCasadiMath::MX_Xd_static<2,1> Vector2_t; +typedef RBDLCasadiMath::MX_Xd_static<3,1> Vector3_t; +typedef RBDLCasadiMath::MX_Xd_static<2,2> Matrix2_t; +typedef RBDLCasadiMath::MX_Xd_static<3,3> Matrix3_t; +typedef RBDLCasadiMath::MX_Xd_static<4,1> Vector4_t; + +typedef RBDLCasadiMath::MX_Xd_static<6,1> SpatialVector_t; +typedef RBDLCasadiMath::MX_Xd_static<6,6> SpatialMatrix_t; + +typedef RBDLCasadiMath::MX_Xd_static<6,3> Matrix63_t; +typedef RBDLCasadiMath::MX_Xd_static<4,3> Matrix43_t; +typedef RBDLCasadiMath::MX_Xd_static<4,4> Matrix4_t; + +typedef RBDLCasadiMath::MX_Xd_dynamic MatrixN_t; +typedef RBDLCasadiMath::MX_Xd_dynamic VectorN_t; + + +#else #include #include #include @@ -17,26 +39,33 @@ #include "rbdl/rbdl_eigenmath.h" +typedef double Vector1_t; +typedef Eigen::Matrix Matrix2_t; typedef Eigen::Matrix Matrix63_t; typedef Eigen::Matrix Matrix43_t; typedef Eigen::VectorXd VectorN_t; typedef Eigen::MatrixXd MatrixN_t; +#endif namespace RigidBodyDynamics { /** \brief Math types such as vectors and matrices and utility functions. */ namespace Math { +typedef Vector1_t Scalar; typedef Vector2_t Vector2d; typedef Vector3_t Vector3d; typedef Vector4_t Vector4d; +typedef Matrix2_t Matrix2d; typedef Matrix3_t Matrix3d; typedef SpatialVector_t SpatialVector; typedef SpatialMatrix_t SpatialMatrix; typedef Matrix63_t Matrix63; typedef Matrix43_t Matrix43; +typedef Matrix4_t Matrix4d; typedef VectorN_t VectorNd; typedef MatrixN_t MatrixNd; + } /* Math */ } /* RigidBodyDynamics */ @@ -46,12 +75,14 @@ typedef MatrixN_t MatrixNd; // If we use Eigen3 we have to create specializations of the STL // std::vector such that the alignment is done properly. +#ifndef RBDL_USE_CASADI_MATH EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix63) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix43) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia) +#endif /* RBDL_MATH_H_H */ #endif diff --git a/include/rbdl/rbdl_mathutils.h b/include/rbdl/rbdl_mathutils.h index 8a791784..33f4ba0d 100644 --- a/include/rbdl/rbdl_mathutils.h +++ b/include/rbdl/rbdl_mathutils.h @@ -37,7 +37,7 @@ extern RBDL_DLLAPI Vector3d Vector3dZero; extern RBDL_DLLAPI Matrix3d Matrix3dIdentity; extern RBDL_DLLAPI Matrix3d Matrix3dZero; -RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) { +RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, Scalar *ptr) { // TODO: use memory mapping operators for Eigen VectorNd result (n); @@ -48,7 +48,7 @@ RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) { return result; } -RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major = true) { +RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, Scalar *ptr, bool row_major = true) { MatrixNd result (rows, cols); if (row_major) { @@ -68,19 +68,23 @@ RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, return result; } +#ifndef RBDL_USE_CASADI_MATH /// \brief Solves a linear system using gaussian elimination with pivoting RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x); +#endif // \todo write test RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix); +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a, - const SpatialMatrix &matrix_b, double epsilon); + const SpatialMatrix &matrix_b, Scalar epsilon); RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a, - const SpatialVector &vector_b, double epsilon); + const SpatialVector &vector_b, Scalar epsilon); +#endif /** \brief Translates the inertia matrix to a new center. */ -RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com); +RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, Scalar mass, const Vector3d &com); /** \brief Creates a transformation of a linear displacement * @@ -101,7 +105,7 @@ RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement); * * \param zrot Rotation angle in radians. */ -RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot); +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const Scalar &zrot); /** \brief Creates a rotational transformation around the Y-axis * @@ -110,7 +114,7 @@ RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot); * * \param yrot Rotation angle in radians. */ -RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot); +RBDL_DLLAPI SpatialMatrix Xroty_mat (const Scalar &yrot); /** \brief Creates a rotational transformation around the X-axis * @@ -119,7 +123,7 @@ RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot); * * \param xrot Rotation angle in radians. */ -RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot); +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const Scalar &xrot); /** \brief Creates a spatial transformation for given parameters * @@ -132,8 +136,8 @@ RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot); */ RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler); -RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) { - double s, c; +RBDL_DLLAPI inline Matrix3d rotx (const Scalar &xrot) { + Scalar s, c; s = sin (xrot); c = cos (xrot); return Matrix3d ( @@ -143,8 +147,8 @@ RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) { ); } -RBDL_DLLAPI inline Matrix3d roty (const double &yrot) { - double s, c; +RBDL_DLLAPI inline Matrix3d roty (const Scalar &yrot) { + Scalar s, c; s = sin (yrot); c = cos (yrot); return Matrix3d ( @@ -154,8 +158,8 @@ RBDL_DLLAPI inline Matrix3d roty (const double &yrot) { ); } -RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) { - double s, c; +RBDL_DLLAPI inline Matrix3d rotz (const Scalar &zrot) { + Scalar s, c; s = sin (zrot); c = cos (zrot); return Matrix3d ( @@ -165,8 +169,8 @@ RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) { ); } -RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) { - double s, c; +RBDL_DLLAPI inline Matrix3d rotxdot (const Scalar &x, const Scalar &xdot) { + Scalar s, c; s = sin (x); c = cos (x); return Matrix3d ( @@ -176,8 +180,8 @@ RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) { ); } -RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) { - double s, c; +RBDL_DLLAPI inline Matrix3d rotydot (const Scalar &y, const Scalar &ydot) { + Scalar s, c; s = sin (y); c = cos (y); return Matrix3d ( @@ -187,8 +191,8 @@ RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) { ); } -RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) { - double s, c; +RBDL_DLLAPI inline Matrix3d rotzdot (const Scalar &z, const Scalar &zdot) { + Scalar s, c; s = sin (z); c = cos (z); return Matrix3d ( @@ -199,10 +203,10 @@ RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) { } RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) { - double sy = sin(zyx_angles[1]); - double cy = cos(zyx_angles[1]); - double sx = sin(zyx_angles[2]); - double cx = cos(zyx_angles[2]); + Scalar sy = sin(zyx_angles[1]); + Scalar cy = cos(zyx_angles[1]); + Scalar sx = sin(zyx_angles[2]); + Scalar cx = cos(zyx_angles[2]); return Vector3d ( zyx_angle_rates[2] - sy * zyx_angle_rates[0], @@ -223,16 +227,16 @@ RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d & } RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) { - double sy = sin(zyx_angles[1]); - double cy = cos(zyx_angles[1]); - double sx = sin(zyx_angles[2]); - double cx = cos(zyx_angles[2]); - double xdot = zyx_angle_rates[2]; - double ydot = zyx_angle_rates[1]; - double zdot = zyx_angle_rates[0]; - double xddot = zyx_angle_rates_dot[2]; - double yddot = zyx_angle_rates_dot[1]; - double zddot = zyx_angle_rates_dot[0]; + Scalar sy = sin(zyx_angles[1]); + Scalar cy = cos(zyx_angles[1]); + Scalar sx = sin(zyx_angles[2]); + Scalar cx = cos(zyx_angles[2]); + Scalar xdot = zyx_angle_rates[2]; + Scalar ydot = zyx_angle_rates[1]; + Scalar zdot = zyx_angle_rates[0]; + Scalar xddot = zyx_angle_rates_dot[2]; + Scalar yddot = zyx_angle_rates_dot[1]; + Scalar zddot = zyx_angle_rates_dot[0]; return Vector3d ( xddot - (cy * ydot * zdot + sy * zddot), diff --git a/include/rbdl/rbdl_utils.h b/include/rbdl/rbdl_utils.h index dfeb291e..2cbd3c44 100644 --- a/include/rbdl/rbdl_utils.h +++ b/include/rbdl/rbdl_utils.h @@ -18,10 +18,14 @@ struct Model; /** \brief Namespace that contains optional helper functions */ namespace Utils { +#ifndef RBDL_USE_CASADI_MATH /** \brief Creates a human readable overview of the model. */ RBDL_DLLAPI std::string GetModelHierarchy (const Model &model); +#endif +#ifndef RBDL_USE_CASADI_MATH /** \brief Creates a human readable overview of the Degrees of Freedom. */ RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model); +#endif /** \brief Creates a human readable overview of the locations of all bodies that have names. */ RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model); @@ -51,7 +55,7 @@ RBDL_DLLAPI void CalcCenterOfMass ( const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd *qddot, - double &mass, + Math::Scalar &mass, Math::Vector3d &com, Math::Vector3d *com_velocity = NULL, Math::Vector3d *com_acceleration = NULL, @@ -81,10 +85,10 @@ RBDL_DLLAPI void CalcZeroMomentPoint ( ); /** \brief Computes the potential energy of the full model. */ -RBDL_DLLAPI double CalcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics = true); +RBDL_DLLAPI Math::Scalar CalcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics = true); /** \brief Computes the kinetic energy of the full model. */ -RBDL_DLLAPI double CalcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics = true); +RBDL_DLLAPI Math::Scalar CalcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics = true); } } diff --git a/python/rbdl_ptr_functions.h b/python/rbdl_ptr_functions.h index c8c21f35..e05189cc 100644 --- a/python/rbdl_ptr_functions.h +++ b/python/rbdl_ptr_functions.h @@ -1622,9 +1622,9 @@ RBDL_DLLAPI void CalcCenterOfMass ( } } - SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3)); - SpatialVector htot (SpatialVector::Zero(6)); - SpatialVector hdot_tot (SpatialVector::Zero(6)); + SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero()); + SpatialVector htot (SpatialVector::Zero()); + SpatialVector hdot_tot (SpatialVector::Zero()); for (size_t i = model.mBodies.size() - 1; i > 0; i--) { unsigned int lambda = model.lambda[i]; diff --git a/share/FindRBDL.cmake b/share/FindRBDL.cmake new file mode 100644 index 00000000..b52874ae --- /dev/null +++ b/share/FindRBDL.cmake @@ -0,0 +1,255 @@ +SET (RBDL_FOUND FALSE) +SET (RBDL_LUAMODEL_FOUND FALSE) +SET (RBDL_URDFREADER_FOUND FALSE) +SET (RBDL_GEOMETRY_FOUND FALSE) +SET (RBDL_MUSCLE_FOUND FALSE) + + +UNSET( RBDL_INCLUDE_DIR CACHE) +UNSET( RBDL_LIBRARY CACHE) +UNSET( RBDL_LUAMODEL_INCLUDE_DIR CACHE) +UNSET( RBDL_LUAMODEL_LIBRARY CACHE) +UNSET( RBDL_URDFREADER_INCLUDE_DIR CACHE) +UNSET( RBDL_URDFREADER_LIBRARY CACHE) +UNSET( RBDL_MUSCLE_INCLUDE_DIR CACHE) +UNSET( RBDL_MUSCLE_LIBRARY CACHE) +UNSET( RBDL_GEOMETRY_INCLUDE_DIR CACHE) +UNSET( RBDL_GEOMETRY_LIBRARY CACHE) + +IF(CUSTOM_RBDL_PATH) + + FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_LIBRARY rbdl + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + + FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + +ELSE(CUSTOM_RBDL_PATH) + FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_LIBRARY rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + + FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) +ENDIF(CUSTOM_RBDL_PATH) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ELSE(RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + IF(RBDL_FIND_REQUIRED) + MESSAGE (SEND_ERROR " Could not find RBDL.") + MESSAGE (SEND_ERROR " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ELSE(RBDL_FIND_REQUIRED) + MESSAGE (STATUS " Could not find RBDL.") + MESSAGE (STATUS " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ENDIF(RBDL_FIND_REQUIRED) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + + +IF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR) + SET (RBDL_LUAMODEL_FOUND TRUE) +ENDIF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR) + +IF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR) + SET (RBDL_URDFREADER_FOUND TRUE) +ENDIF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR) + +IF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR) + SET (RBDL_MUSCLE_FOUND TRUE) +ENDIF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR) + +IF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR) + SET (RBDL_GEOMETRY_FOUND TRUE) +ENDIF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR) + + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(ERROR " Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) + +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LUAMODEL_INCLUDE_DIR + RBDL_LUAMODEL_LIBRARY + RBDL_URDFREADER_INCLUDE_DIR + RBDL_URDFREADER_LIBRARY + RBDL_MUSCLE_INCLUDE_DIR + RBDL_MUSCLE_LIBRARY + RBDL_GEOMETRY_INCLUDE_DIR + RBDL_GEOMETRY_LIBRARY + ) diff --git a/share/RBDLCasadiConfig.cmake.in b/share/RBDLCasadiConfig.cmake.in new file mode 100644 index 00000000..57398424 --- /dev/null +++ b/share/RBDLCasadiConfig.cmake.in @@ -0,0 +1,259 @@ +set(RBDLCasadi_VERSION @PROJECT_VERSION_MAJOR@.@PROJECT_VERSION_MINOR@.@PROJECT_VERSION_PATCH@) + +IF(CUSTOM_RBDLCasadi_PATH) + + FIND_PATH (RBDLCasadi_INCLUDE_DIR rbdl/rbdl.h + PATHS + ${CUSTOM_RBDLCasadi_PATH}/include/rbdl-casadi + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDLCasadi_LIBRARY rbdl-casadi + PATHS + ${CUSTOM_RBDLCasadi_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDLCasadi_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + PATHS + ${CUSTOM_RBDLCasadi_PATH}/include/rbdl-casadi + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDLCasadi_LUAMODEL_LIBRARY rbdl_luamodel-casadi + PATHS + ${CUSTOM_RBDLCasadi_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDLCasadi_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + PATHS + ${CUSTOM_RBDLCasadi_PATH}/include/rbdl-casadi + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDLCasadi_URDFREADER_LIBRARY rbdl_urdfreader-casadi + PATHS + ${CUSTOM_RBDLCasadi_PATH}/lib + NO_DEFAULT_PATH + ) + + + FIND_PATH (RBDLCasadi_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + PATHS + ${CUSTOM_RBDLCasadi_PATH}/include/rbdl-casadi + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDLCasadi_MUSCLE_LIBRARY rbdl_muscle-casadi + PATHS + ${CUSTOM_RBDLCasadi_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDLCasadi_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + PATHS + ${CUSTOM_RBDLCasadi_PATH}/include/rbdl-casadi + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDLCasadi_GEOMETRY_LIBRARY rbdl_geometry-casadi + PATHS + ${CUSTOM_RBDLCasadi_PATH}/lib + NO_DEFAULT_PATH + ) + +ELSE(CUSTOM_RBDLCasadi_PATH) + FIND_PATH (RBDLCasadi_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDLCasadi_PATH}/src + $ENV{RBDLCasadi_PATH}/include + $ENV{RBDLCasadi_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/rbdl-casadi + ${CMAKE_INSTALL_PREFIX}/Library/include/rbdl-casadi + /usr/include/rbdl-casadi + /usr/local/include/rbdl-casadi + ) + + FIND_LIBRARY (RBDLCasadi_LIBRARY rbdl-casadi + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDLCasadi_PATH}/lib + $ENV{RBDLCasadi_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDLCasadi_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDLCasadi_PATH}/src + $ENV{RBDLCasadi_PATH}/include + $ENV{RBDLCasadi_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/rbdl-casadi + ${CMAKE_INSTALL_PREFIX}/Library/include/rbdl-casadi + /usr/include/rbdl-casadi + /usr/local/include/rbdl-casadi + ) + + FIND_LIBRARY (RBDLCasadi_LUAMODEL_LIBRARY rbdl_luamodel-casadi + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDLCasadi_PATH} + $ENV{RBDLCasadi_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDLCasadi_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDLCasadi_PATH}/src + $ENV{RBDLCasadi_PATH}/include + $ENV{RBDLCasadi_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/rbdl-casadi + ${CMAKE_INSTALL_PREFIX}/Library/include/rbdl-casadi + /usr/local/include/rbdl-casadi + /usr/include/rbdl-casadi + ) + + FIND_LIBRARY (RBDLCasadi_URDFREADER_LIBRARY rbdl_urdfreader-casadi + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDLCasadi_PATH} + $ENV{RBDLCasadi_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + + FIND_PATH (RBDLCasadi_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDLCasadi_PATH}/src + $ENV{RBDLCasadi_PATH}/include + $ENV{RBDLCasadi_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/rbdl-casadi + ${CMAKE_INSTALL_PREFIX}/Library/include/rbdl-casadi + /usr/local/include/rbdl-casadi + /usr/include/rbdl-casadi + ) + + FIND_LIBRARY (RBDLCasadi_MUSCLE_LIBRARY rbdl_muscle-casadi + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDLCasadi_PATH} + $ENV{RBDLCasadi_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDLCasadi_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDLCasadi_PATH}/src + $ENV{RBDLCasadi_PATH}/include + $ENV{RBDLCasadi_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/rbdl-casadi + ${CMAKE_INSTALL_PREFIX}/Library/include/rbdl-casadi + /usr/local/include/rbdl-casadi + /usr/include/rbdl-casadi + ) + + FIND_LIBRARY (RBDLCasadi_GEOMETRY_LIBRARY rbdl_geometry-casadi + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDLCasadi_PATH} + $ENV{RBDLCasadi_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) +ENDIF(CUSTOM_RBDLCasadi_PATH) + +IF (RBDLCasadi_INCLUDE_DIR AND RBDLCasadi_LIBRARY) + SET (RBDLCasadi_FOUND TRUE) +ELSE(RBDLCasadi_INCLUDE_DIR AND RBDLCasadi_LIBRARY) + IF(RBDLCasadi_FIND_REQUIRED) + MESSAGE (SEND_ERROR " Could not find RBDL.") + MESSAGE (SEND_ERROR " Try setting CUSTOM_RBDLCasadi_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ELSE(RBDLCasadi_FIND_REQUIRED) + MESSAGE (STATUS " Could not find RBDL.") + MESSAGE (STATUS " Try setting CUSTOM_RBDLCasadi_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ENDIF(RBDLCasadi_FIND_REQUIRED) +ENDIF (RBDLCasadi_INCLUDE_DIR AND RBDLCasadi_LIBRARY) + + +IF (RBDLCasadi_LUAMODEL_LIBRARY AND RBDLCasadi_LUAMODEL_INCLUDE_DIR) + SET (RBDLCasadi_LUAMODEL_FOUND TRUE) +ENDIF (RBDLCasadi_LUAMODEL_LIBRARY AND RBDLCasadi_LUAMODEL_INCLUDE_DIR) + +IF (RBDLCasadi_URDFREADER_LIBRARY AND RBDLCasadi_URDFREADER_INCLUDE_DIR) + SET (RBDLCasadi_URDFREADER_FOUND TRUE) +ENDIF (RBDLCasadi_URDFREADER_LIBRARY AND RBDLCasadi_URDFREADER_INCLUDE_DIR) + +IF (RBDLCasadi_MUSCLE_LIBRARY AND RBDLCasadi_MUSCLE_INCLUDE_DIR) + SET (RBDLCasadi_MUSCLE_FOUND TRUE) +ENDIF (RBDLCasadi_MUSCLE_LIBRARY AND RBDLCasadi_MUSCLE_INCLUDE_DIR) + +IF (RBDLCasadi_GEOMETRY_LIBRARY AND RBDLCasadi_GEOMETRY_INCLUDE_DIR) + SET (RBDLCasadi_GEOMETRY_FOUND TRUE) +ENDIF (RBDLCasadi_GEOMETRY_LIBRARY AND RBDLCasadi_GEOMETRY_INCLUDE_DIR) + + +IF (RBDLCasadi_FOUND) + IF (NOT RBDLCasadi_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDLCasadi_LIBRARY}") + ENDIF (NOT RBDLCasadi_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDLCasadi_FIND_COMPONENTS} ) + IF (RBDLCasadi_${COMPONENT}_FOUND) + IF (NOT RBDLCasadi_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDLCasadi_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDLCasadi_FIND_QUIETLY) + ELSE (RBDLCasadi_${COMPONENT}_FOUND) + MESSAGE(ERROR " Could not find RBDL ${COMPONENT}") + ENDIF (RBDLCasadi_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) + +ENDIF (RBDLCasadi_FOUND) + +MARK_AS_ADVANCED ( + RBDLCasadi_INCLUDE_DIR + RBDLCasadi_LIBRARY + RBDLCasadi_LUAMODEL_INCLUDE_DIR + RBDLCasadi_LUAMODEL_LIBRARY + RBDLCasadi_URDFREADER_INCLUDE_DIR + RBDLCasadi_URDFREADER_LIBRARY + RBDLCasadi_MUSCLE_INCLUDE_DIR + RBDLCasadi_MUSCLE_LIBRARY + RBDLCasadi_GEOMETRY_INCLUDE_DIR + RBDLCasadi_GEOMETRY_LIBRARY + ) diff --git a/share/RBDLConfig.cmake.in b/share/RBDLConfig.cmake.in new file mode 100644 index 00000000..b680007a --- /dev/null +++ b/share/RBDLConfig.cmake.in @@ -0,0 +1,259 @@ +set(RBDL_VERSION @PROJECT_VERSION_MAJOR@.@PROJECT_VERSION_MINOR@.@PROJECT_VERSION_PATCH@) + +IF(CUSTOM_RBDL_PATH) + + FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_LIBRARY rbdl + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + + FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + + FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + + FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + PATHS + ${CUSTOM_RBDL_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry + PATHS + ${CUSTOM_RBDL_PATH}/lib + NO_DEFAULT_PATH + ) + +ELSE(CUSTOM_RBDL_PATH) + FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include + ${CMAKE_INSTALL_PREFIX}/Library/include + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_LIBRARY rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include + ${CMAKE_INSTALL_PREFIX}/Library/include + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include + ${CMAKE_INSTALL_PREFIX}/Library/include + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + + FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include + ${CMAKE_INSTALL_PREFIX}/Library/include + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + + FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include/ + ${CMAKE_INSTALL_PREFIX}/Library/include/ + /usr/local/include + /usr/include + ) + + FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ${CMAKE_INSTALL_PREFIX}/Library/lib + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) +ENDIF(CUSTOM_RBDL_PATH) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ELSE(RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + IF(RBDL_FIND_REQUIRED) + MESSAGE (SEND_ERROR " Could not find RBDL.") + MESSAGE (SEND_ERROR " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ELSE(RBDL_FIND_REQUIRED) + MESSAGE (STATUS " Could not find RBDL.") + MESSAGE (STATUS " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.") + ENDIF(RBDL_FIND_REQUIRED) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + + +IF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR) + SET (RBDL_LUAMODEL_FOUND TRUE) +ENDIF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR) + +IF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR) + SET (RBDL_URDFREADER_FOUND TRUE) +ENDIF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR) + +IF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR) + SET (RBDL_MUSCLE_FOUND TRUE) +ENDIF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR) + +IF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR) + SET (RBDL_GEOMETRY_FOUND TRUE) +ENDIF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR) + + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(ERROR " Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) + +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LUAMODEL_INCLUDE_DIR + RBDL_LUAMODEL_LIBRARY + RBDL_URDFREADER_INCLUDE_DIR + RBDL_URDFREADER_LIBRARY + RBDL_MUSCLE_INCLUDE_DIR + RBDL_MUSCLE_LIBRARY + RBDL_GEOMETRY_INCLUDE_DIR + RBDL_GEOMETRY_LIBRARY + ) diff --git a/share/rbdl-casadi.pc.cmake b/share/rbdl-casadi.pc.cmake new file mode 100644 index 00000000..7edb7b49 --- /dev/null +++ b/share/rbdl-casadi.pc.cmake @@ -0,0 +1,13 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=@CMAKE_INSTALL_FULL_LIBDIR@ +includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@/rbdl-casadi + +Name: RBDL-Casadi +Description: Rigid Body Dynamics Library +URL: https://github.com/orb-hd/rbdl-orb +Version: @RBDL_VERSION@ +Requires: casadi +Conflicts: +Libs: -L${libdir} -lrbdl-casadi -Wl,-rpath ${libdir} +Libs.private: +Cflags: -I${includedir} diff --git a/share/rbdl.pc.cmake b/share/rbdl.pc.cmake new file mode 100644 index 00000000..5ee1c60d --- /dev/null +++ b/share/rbdl.pc.cmake @@ -0,0 +1,13 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=@CMAKE_INSTALL_FULL_LIBDIR@ +includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@/rbdl + +Name: RBDL +Description: Rigid Body Dynamics Library +URL: https://github.com/orb-hd/rbdl-orb +Version: @RBDL_VERSION@ +Requires: eigen3 +Conflicts: +Libs: -L${libdir} -lrbdl -Wl,-rpath ${libdir} +Libs.private: +Cflags: -I${includedir} diff --git a/src/Constraint_Contact.cc b/src/Constraint_Contact.cc index 151b7a62..aa7d6374 100644 --- a/src/Constraint_Contact.cc +++ b/src/Constraint_Contact.cc @@ -46,7 +46,9 @@ ContactConstraint::ContactConstraint( T.push_back(groundConstraintUnitVector); dblA = std::numeric_limits::epsilon()*10.; +#ifndef RBDL_USE_CASADI_MATH assert(std::fabs(T[0].norm()-1.0)<= dblA); +#endif groundPoint = Math::Vector3dZero; @@ -231,10 +233,12 @@ void ContactConstraint:: dblA = 10.0*std::numeric_limits::epsilon(); //Make sure the normal is valid +#ifndef RBDL_USE_CASADI_MATH assert( std::fabs(normal.norm()-1.) < dblA); for(unsigned int i=0; i::epsilon()*10.; +#ifndef RBDL_USE_CASADI_MATH assert(std::fabs(T[0].norm()-1.0)<= dblA); +#endif positionConstraint[0]=positionLevelConstraint; velocityConstraint[0]=velocityLevelConstraint; @@ -355,10 +357,12 @@ void LoopConstraint:: dblA = 10.0*std::numeric_limits::epsilon(); //Make sure the normal is valid +#ifndef RBDL_USE_CASADI_MATH assert( std::fabs(constraintAxis.norm()-1.) < dblA); for(unsigned int i=0; igetBodyIds()[0] == body_id) { Vector3d pointErr = body_point - contactConstraints[i]->getBodyFrames()[0].r; - - if(pointErr.norm() < std::numeric_limits::epsilon()*100 - && contactConstraints[i]->getUserDefinedId() == userDefinedId) { +#ifdef RBDL_USE_CASADI_MATH + if(pointErr.is_zero() && contactConstraints[i]->getUserDefinedId() == userDefinedId) { +#else + if(pointErr.norm() < std::numeric_limits::epsilon()*100 && + contactConstraints[i]->getUserDefinedId() == userDefinedId) { +#endif constraintAppended = true; contactConstraints[i]->appendNormalVector(world_normal); } @@ -87,10 +90,9 @@ unsigned int ConstraintSet::AddContactConstraint ( if(constraintAppended == false) { - ContactConstraint con = ContactConstraint(body_id,body_point, world_normal, - constraint_name,userDefinedId); - - contactConstraints.push_back(std::make_shared(con)); + contactConstraints.push_back( + std::shared_ptr( + new ContactConstraint(body_id,body_point, world_normal, constraint_name,userDefinedId))); unsigned int idx = unsigned(contactConstraints.size()-1); contactConstraints[idx]->addToConstraintSet(insertAtRowInG); constraints.emplace_back(contactConstraints[idx]); @@ -200,6 +202,7 @@ unsigned int ConstraintSet::AddLoopConstraint ( //Using this awkward element by element comparison to maintain //compatibility with SimpleMath. +#ifndef RBDL_USE_CASADI_MATH for(unsigned int i=0; i tol || fabs(frameErrorSuc.r[i]) > tol) { framesNumericallyIdentical=false; @@ -210,6 +213,7 @@ unsigned int ConstraintSet::AddLoopConstraint ( } } } +#endif if(framesNumericallyIdentical && loopConstraints[idx]->getUserDefinedId() == userDefinedId) { @@ -221,15 +225,12 @@ unsigned int ConstraintSet::AddLoopConstraint ( if(constraintAppended==false) { - LoopConstraint loopCon( idPredecessor, idSuccessor, - XPredecessor, XSuccessor, - constraintAxisInPredecessor, - enableBaumgarteStabilization, - stabilizationTimeConstant, - constraintName, - userDefinedId); - - loopConstraints.push_back(std::make_shared(loopCon)); + loopConstraints.push_back( + std::shared_ptr( + new LoopConstraint( + idPredecessor, idSuccessor,XPredecessor, XSuccessor, + constraintAxisInPredecessor, enableBaumgarteStabilization, + stabilizationTimeConstant,constraintName,userDefinedId))); idx = unsigned(loopConstraints.size()-1); loopConstraints[idx]->addToConstraintSet(insertAtRowInG); constraints.emplace_back(loopConstraints[idx]); @@ -441,7 +442,9 @@ bool ConstraintSet::Bind (const Model &model) W.Identity(model.dof_count, model.dof_count); // HouseHolderQR crashes if matrix G has more rows than columns. +#ifndef RBDL_USE_CASADI_MATH GT_qr = Eigen::HouseholderQR (G.transpose()); +#endif GT_qr_Q = MatrixNd::Zero (model.dof_count, model.dof_count); Y = MatrixNd::Zero (model.dof_count, G.rows()); Z = MatrixNd::Zero (model.dof_count, model.dof_count - G.rows()); @@ -680,6 +683,10 @@ void SolveConstrainedSystemDirect ( LOG << "A = " << std::endl << A << std::endl; LOG << "b = " << std::endl << b << std::endl; +#ifdef RBDL_USE_CASADI_MATH + auto linsol = casadi::Linsol("linear_solver", "symbolicqr", A.sparsity()); + x = linsol.solve(A, b); +#else switch (linear_solver) { case (LinearSolverPartialPivLU) : x = A.partialPivLu().solve(b); @@ -695,6 +702,7 @@ void SolveConstrainedSystemDirect ( assert (0); break; } +#endif LOG << "x = " << std::endl << x << std::endl; } @@ -730,8 +738,12 @@ void SolveConstrainedSystemRangeSpaceSparse ( K = Y.transpose() * Y; a = gamma - Y.transpose() * z; - +#ifdef RBDL_USE_CASADI_MATH + auto linsol = casadi::Linsol("linear_solver", "symbolicqr", K.sparsity()); + lambda = linsol.solve(K, a); +#else lambda = K.llt().solve(a); +#endif qddot = c + G.transpose() * lambda; SparseSolveLTx (model, H, qddot); @@ -754,6 +766,12 @@ void SolveConstrainedSystemNullSpace ( Math::LinearSolver &linear_solver ) { + +#ifdef RBDL_USE_CASADI_MATH + auto GY = G * Y; + auto linsol = casadi::Linsol("linear_solver", "symbolicqr", GY.sparsity()); + qddot_y = linsol.solve(GY, gamma); +#else switch (linear_solver) { case (LinearSolverPartialPivLU) : qddot_y = (G * Y).partialPivLu().solve (gamma); @@ -769,11 +787,22 @@ void SolveConstrainedSystemNullSpace ( assert (0); break; } +#endif +#ifdef RBDL_USE_CASADI_MATH + auto ZHZ = (Z.transpose()*H*Z); + linsol = casadi::Linsol("linear_solver", "symbolicqr", ZHZ.sparsity()); + qddot_z = linsol.solve(ZHZ, Z.transpose()*(c - H*Y*qddot_y)); +#else qddot_z = (Z.transpose()*H*Z).llt().solve(Z.transpose()*(c - H*Y*qddot_y)); - +#endif qddot = Y * qddot_y + Z * qddot_z; +#ifdef RBDL_USE_CASADI_MATH + GY = G * Y; + linsol = casadi::Linsol("linear_solver", "symbolicqr", GY.sparsity()); + lambda = linsol.solve(GY, Y.transpose() * (H * qddot - c)); +#else switch (linear_solver) { case (LinearSolverPartialPivLU) : lambda = (G * Y).partialPivLu().solve (Y.transpose() * (H * qddot - c)); @@ -789,6 +818,7 @@ void SolveConstrainedSystemNullSpace ( assert (0); break; } +#endif } @@ -911,6 +941,7 @@ void CalcConstrainedSystemVariables ( } //============================================================================== +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool CalcAssemblyQ ( Model &model, @@ -1010,6 +1041,7 @@ bool CalcAssemblyQ ( Q = QInit; return false; } +#endif //============================================================================== RBDL_DLLAPI @@ -1112,6 +1144,7 @@ void ForwardDynamicsConstraintsRangeSpaceSparse ( } //============================================================================== +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace ( Model &model, @@ -1140,6 +1173,7 @@ void ForwardDynamicsConstraintsNullSpace ( , CS.force, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z, CS.linear_solver); } +#endif //============================================================================== RBDL_DLLAPI @@ -1198,6 +1232,7 @@ void ComputeConstraintImpulsesRangeSpaceSparse ( } //============================================================================== +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace ( Model &model, @@ -1226,6 +1261,7 @@ void ComputeConstraintImpulsesNullSpace ( , QDotPlus, CS.impulse, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z , CS.linear_solver); } +#endif //============================================================================== /** @brief Compute only the effects of external forces on the generalized @@ -1253,7 +1289,11 @@ void ForwardDynamicsApplyConstraintForces ( model.IA[i] = model.I[i].toMatrix();; model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); +#ifdef RBDL_USE_CASADI_MATH + { +#else if (CS.f_ext_constraints[i] != SpatialVector::Zero()) { +#endif LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i] << std::endl; @@ -1284,9 +1324,15 @@ void ForwardDynamicsApplyConstraintForces ( SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -1301,10 +1347,15 @@ void ForwardDynamicsApplyConstraintForces ( - model.U[i] * (model.U[i] / model.d[i]).transpose(); SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; - +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -1335,10 +1386,17 @@ void ForwardDynamicsApplyConstraintForces ( * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u ); +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -1686,6 +1744,10 @@ void ForwardDynamicsContactsKokkevis ( LOG << "K = " << std::endl << CS.K << std::endl; LOG << "a = " << std::endl << CS.a << std::endl; +#ifdef RBDL_USE_CASADI_MATH + auto linsol = casadi::Linsol("linear_solver", "symbolicqr", CS.K.sparsity()); + CS.force = linsol.solve(CS.K, CS.a); +#else switch (CS.linear_solver) { case (LinearSolverPartialPivLU) : CS.force = CS.K.partialPivLu().solve(CS.a); @@ -1701,6 +1763,7 @@ void ForwardDynamicsContactsKokkevis ( assert (0); break; } +#endif LOG << "f = " << CS.force.transpose() << std::endl; @@ -1737,7 +1800,7 @@ void ForwardDynamicsContactsKokkevis ( //============================================================================== - +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool isConstrainedSystemFullyActuated( Model &model, @@ -1777,6 +1840,7 @@ bool isConstrainedSystemFullyActuated( return isCompatible; } +#endif RBDL_DLLAPI void InverseDynamicsConstraints( @@ -1865,6 +1929,7 @@ void InverseDynamicsConstraints( } +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI void InverseDynamicsConstraintsRelaxed( Model &model, @@ -2010,6 +2075,7 @@ void InverseDynamicsConstraintsRelaxed( } +#endif void SolveLinearSystem ( const MatrixNd& A, @@ -2022,6 +2088,10 @@ void SolveLinearSystem ( throw Errors::RBDLSizeMismatchError("Mismatching sizes.\n"); } +#ifdef RBDL_USE_CASADI_MATH + auto linsol = casadi::Linsol("linear_solver", "symbolicqr", A.sparsity()); + x = linsol.solve(A, b); +#else // Solve the system A*x = b. switch (ls) { case (LinearSolverPartialPivLU) : @@ -2039,6 +2109,7 @@ void SolveLinearSystem ( throw Errors::RBDLError(errormsg.str()); break; } +#endif } //============================================================================== diff --git a/src/Dynamics.cc b/src/Dynamics.cc index 19bed336..97eba615 100644 --- a/src/Dynamics.cc +++ b/src/Dynamics.cc @@ -134,7 +134,12 @@ RBDL_DLLAPI void NonlinearEffects ( if (!model.mBodies[i].mIsVirtual) { model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); - if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { +#ifdef RBDL_USE_CASADI_MATH + if (f_ext != NULL) +#else + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) +#endif + { model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; } } else { @@ -358,7 +363,11 @@ RBDL_DLLAPI void ForwardDynamics ( model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); +#ifdef RBDL_USE_CASADI_MATH + if (f_ext != NULL) { +#else if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { +#endif LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl; model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; } @@ -389,11 +398,19 @@ RBDL_DLLAPI void ForwardDynamics ( + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -402,9 +419,13 @@ RBDL_DLLAPI void ForwardDynamics ( && model.mJoints[i].mJointType != JointTypeCustom) { model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef RBDL_USE_CASADI_MATH + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() + * model.multdof3_U[i]).inverse(); +#else model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval(); - +#endif Vector3d tau_temp(Tau.block(q_index,0,3,1)); model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i]; @@ -423,7 +444,14 @@ RBDL_DLLAPI void ForwardDynamics ( + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia @@ -431,7 +459,7 @@ RBDL_DLLAPI void ForwardDynamics ( model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); - +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -442,10 +470,15 @@ RBDL_DLLAPI void ForwardDynamics ( model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; +#ifdef RBDL_USE_CASADI_MATH + model.mCustomJoints[kI]->Dinv + = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U).inverse(); +#else model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U).inverse().eval(); - +#endif VectorNd tau_temp(Tau.block(q_index,0,dofI,1)); model.mCustomJoints[kI]->u = tau_temp - model.mCustomJoints[kI]->S.transpose() * model.pA[i]; @@ -464,11 +497,17 @@ RBDL_DLLAPI void ForwardDynamics ( * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u); +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); - +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; @@ -555,6 +594,9 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian ( LOG << "A = " << std::endl << *H << std::endl; LOG << "b = " << std::endl << *C * -1. + Tau << std::endl; +#ifdef RBDL_USE_CASADI_MATH + QDDot = H->inverse() * (*C * -1. + Tau); +#else switch (linear_solver) { case (LinearSolverPartialPivLU) : QDDot = H->partialPivLu().solve (*C * -1. + Tau); @@ -573,6 +615,7 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian ( assert (0); break; } +#endif if (free_C) { delete C; @@ -632,18 +675,28 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); +#endif } } else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom) { model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef RBDL_USE_CASADI_MATH + model.multdof3_Dinv[i] = + (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); +#else model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval(); - +#endif // LOG << "mCustomJoints[kI]->u[" << i << "] = " //<< model.mCustomJoints[kI]->u[i].transpose() << std::endl; @@ -655,20 +708,31 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose()); +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += + model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); +#endif } } else if (model.mJoints[i].mJointType == JointTypeCustom) { unsigned int kI = model.mJoints[i].custom_joint_index; unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; +#ifdef RBDL_USE_CASADI_MATH + model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U + ).inverse(); +#else model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U ).inverse().eval(); - +#endif // LOG << "mCustomJoints[kI]->u[" << i << "] = " //<< model.mCustomJoints[kI]->u.transpose() << std::endl; unsigned int lambda = model.lambda[i]; @@ -678,9 +742,14 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, - ( model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->U.transpose()); +#ifdef RBDL_USE_CASADI_MATH + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#else model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); +#endif } } } @@ -699,8 +768,11 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, if (lambda != 0) { SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i]; +#ifdef RBDL_USE_CASADI_MATH + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); - +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; } @@ -722,9 +794,12 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, * model.multdof3_Dinv[i] * model.multdof3_u[i]; +#ifdef RBDL_USE_CASADI_MATH + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); - +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; } @@ -745,9 +820,12 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u); +#ifdef RBDL_USE_CASADI_MATH + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#else model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); - +#endif LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; } diff --git a/src/Joint.cc b/src/Joint.cc index c6797326..05166105 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -40,11 +40,11 @@ RBDL_DLLAPI void jcalc ( } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) { model.X_J[joint_id] = jcalc_XJ (model, joint_id, q); jcalc_X_lambda_S(model, joint_id, q); - double Jqd = qdot[model.mJoints[joint_id].q_index]; + Scalar Jqd = qdot[model.mJoints[joint_id].q_index]; model.v_J[joint_id] = model.S[joint_id] * Jqd; - - Vector3d St = model.S[joint_id].block(0,0,3,1); - Vector3d c = model.X_J[joint_id].E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + + Vector3d St = model.S[joint_id].block<3, 1>(0,0); + Vector3d c = model.X_J[joint_id].E * model.mJoints[joint_id].mJointAxes[0].block<3, 1>(3,0); c = St.cross(c); c *= -Jqd * Jqd; model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]); @@ -70,16 +70,16 @@ RBDL_DLLAPI void jcalc ( omega[0], omega[1], omega[2], 0., 0., 0.); } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_J[joint_id].E = Matrix3d( c0 * c1, s0 * c1, -s1, @@ -96,9 +96,9 @@ RBDL_DLLAPI void jcalc ( model.multdof3_S[joint_id](2,0) = c1 * c2; model.multdof3_S[joint_id](2,1) = - s2; - double qdot0 = qdot[model.mJoints[joint_id].q_index]; - double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; - double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + Scalar qdot0 = qdot[model.mJoints[joint_id].q_index]; + Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); @@ -109,16 +109,16 @@ RBDL_DLLAPI void jcalc ( -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, 0.,0., 0.); } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_J[joint_id].E = Matrix3d( c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0, @@ -135,9 +135,9 @@ RBDL_DLLAPI void jcalc ( model.multdof3_S[joint_id](2,0) = s1; model.multdof3_S[joint_id](2,2) = 1.; - double qdot0 = qdot[model.mJoints[joint_id].q_index]; - double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; - double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + Scalar qdot0 = qdot[model.mJoints[joint_id].q_index]; + Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); @@ -149,16 +149,16 @@ RBDL_DLLAPI void jcalc ( 0., 0., 0. ); } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_J[joint_id].E = Matrix3d( c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0, @@ -174,9 +174,9 @@ RBDL_DLLAPI void jcalc ( model.multdof3_S[joint_id](2,0) = -s1; model.multdof3_S[joint_id](2,2) = 1.; - double qdot0 = qdot[model.mJoints[joint_id].q_index]; - double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; - double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + Scalar qdot0 = qdot[model.mJoints[joint_id].q_index]; + Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); @@ -188,9 +188,9 @@ RBDL_DLLAPI void jcalc ( 0., 0., 0. ); } else if(model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ){ - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; model.X_J[joint_id].E = Matrix3d::Identity(); model.X_J[joint_id].r = Vector3d (q0, q1, q2); @@ -199,9 +199,9 @@ RBDL_DLLAPI void jcalc ( model.multdof3_S[joint_id](4,1) = 1.; model.multdof3_S[joint_id](5,2) = 1.; - double qdot0 = qdot[model.mJoints[joint_id].q_index]; - double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; - double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + Scalar qdot0 = qdot[model.mJoints[joint_id].q_index]; + Scalar qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + Scalar qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); @@ -298,7 +298,7 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( SpatialTransform XJ = jcalc_XJ (model, joint_id, q); model.X_lambda[joint_id] = XJ * model.X_T[joint_id]; // Set the joint axis - Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block<3, 1>(3,0); model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0], model.mJoints[joint_id].mJointAxes[0][1], @@ -321,16 +321,16 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.multdof3_S[joint_id](1,1) = 1.; model.multdof3_S[joint_id](2,2) = 1.; } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_lambda[joint_id] = SpatialTransform ( Matrix3d( @@ -352,16 +352,16 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.multdof3_S[joint_id](2,0) = c1 * c2; model.multdof3_S[joint_id](2,1) = - s2; } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_lambda[joint_id] = SpatialTransform ( Matrix3d( @@ -383,16 +383,16 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.multdof3_S[joint_id](2,0) = s1; model.multdof3_S[joint_id](2,2) = 1.; } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ ) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; - double s0 = sin (q0); - double c0 = cos (q0); - double s1 = sin (q1); - double c1 = cos (q1); - double s2 = sin (q2); - double c2 = cos (q2); + Scalar s0 = sin (q0); + Scalar c0 = cos (q0); + Scalar s1 = sin (q1); + Scalar c1 = cos (q1); + Scalar s2 = sin (q2); + Scalar c2 = cos (q2); model.X_lambda[joint_id] = SpatialTransform ( Matrix3d( @@ -414,12 +414,12 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.multdof3_S[joint_id](2,0) = -s1; model.multdof3_S[joint_id](2,2) = 1.; } else if (model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ) { - double q0 = q[model.mJoints[joint_id].q_index]; - double q1 = q[model.mJoints[joint_id].q_index + 1]; - double q2 = q[model.mJoints[joint_id].q_index + 2]; + Scalar q0 = q[model.mJoints[joint_id].q_index]; + Scalar q1 = q[model.mJoints[joint_id].q_index + 1]; + Scalar q2 = q[model.mJoints[joint_id].q_index + 2]; model.X_lambda[joint_id] = SpatialTransform ( - Matrix3d::Identity (3,3), + Matrix3d::Identity (), Vector3d (q0, q1, q2)) * model.X_T[joint_id]; diff --git a/src/Kinematics.cc b/src/Kinematics.cc index d8003314..0ea05d17 100644 --- a/src/Kinematics.cc +++ b/src/Kinematics.cc @@ -288,12 +288,12 @@ RBDL_DLLAPI void CalcPointJacobian ( G.block(0,q_index, 3, 1) = point_trans.apply( model.X_base[j].inverse().apply( - model.S[j])).block(3,0,3,1); + model.S[j])).block<3, 1>(3,0); } else if (model.mJoints[j].mDoFCount == 3) { G.block(0, q_index, 3, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() - * model.multdof3_S[j]).block(3,0,3,3); + * model.multdof3_S[j]).block<3, 3>(3,0); } } else { unsigned int k = model.mJoints[j].custom_joint_index; @@ -350,12 +350,12 @@ RBDL_DLLAPI void CalcPointJacobian6D ( G.block(0,q_index, 6, 1) = point_trans.apply( model.X_base[j].inverse().apply( - model.S[j])).block(0,0,6,1); + model.S[j])).block<6, 1>(0,0); } else if (model.mJoints[j].mDoFCount == 3) { G.block(0, q_index, 6, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() - * model.multdof3_S[j]).block(0,0,6,3); + * model.multdof3_S[j]).block<6, 3>(0,0); } } else { unsigned int k = model.mJoints[j].custom_joint_index; @@ -605,6 +605,7 @@ RBDL_DLLAPI SpatialVector CalcPointAcceleration6D( + SpatialVector (0, 0, 0, a_dash[0], a_dash[1], a_dash[2])); } +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool InverseKinematics ( Model &model, const VectorNd &Qinit, @@ -739,7 +740,7 @@ unsigned int InverseKinematicsConstraintSet::AddPointConstraint( body_ids.push_back(body_id); body_points.push_back(body_point); target_positions.push_back(target_pos); - target_orientations.push_back(Matrix3d::Zero(3,3)); + target_orientations.push_back(Matrix3d::Zero()); constraint_weight.push_back(weight); constraint_row_index.push_back(num_constraints); num_constraints = num_constraints + 3; @@ -757,7 +758,7 @@ unsigned int InverseKinematicsConstraintSet::AddPointConstraintXY( body_ids.push_back(body_id); body_points.push_back(body_point); target_positions.push_back(target_pos); - target_orientations.push_back(Matrix3d::Zero(3,3)); + target_orientations.push_back(Matrix3d::Zero()); constraint_weight.push_back(weight); constraint_row_index.push_back(num_constraints); num_constraints = num_constraints + 2; @@ -775,7 +776,7 @@ unsigned int InverseKinematicsConstraintSet::AddPointConstraintZ( body_ids.push_back(body_id); body_points.push_back(body_point); target_positions.push_back(target_pos); - target_orientations.push_back(Matrix3d::Zero(3,3)); + target_orientations.push_back(Matrix3d::Zero()); constraint_weight.push_back(weight); constraint_row_index.push_back(num_constraints); num_constraints = num_constraints + 1; @@ -792,7 +793,7 @@ unsigned int InverseKinematicsConstraintSet::AddPointConstraintCoMXY( body_ids.push_back(body_id); body_points.push_back(Vector3d::Zero()); target_positions.push_back(target_pos); - target_orientations.push_back(Matrix3d::Zero(3,3)); + target_orientations.push_back(Matrix3d::Zero()); constraint_weight.push_back(weight); constraint_row_index.push_back(num_constraints); num_constraints = num_constraints + 2; @@ -1001,6 +1002,5 @@ bool InverseKinematics ( return false; } - +#endif } - diff --git a/src/Model.cc b/src/Model.cc index 975874c0..e90301fb 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -71,7 +71,7 @@ Model::Model() f.push_back (zero_spatial); SpatialRigidBodyInertia rbi(0., Vector3d (0., 0., 0.), - Matrix3d::Zero(3,3)); + Matrix3d::Zero()); Ic.push_back (rbi); I.push_back(rbi); hc.push_back (zero_spatial); @@ -219,9 +219,18 @@ unsigned int AddBodyMultiDofJoint ( joint.mJointAxes[j][4], joint.mJointAxes[j][5]); +#ifdef RBDL_USE_CASADI_MATH + if (rotation.is_zero()) { +#else if (rotation == Vector3d (0., 0., 0.)) { +#endif single_dof_joint = Joint (JointTypePrismatic, translation); - } else if (translation == Vector3d (0., 0., 0.)) { + } +#ifdef RBDL_USE_CASADI_MATH + if (translation.is_zero()) { +#else + else if (translation == Vector3d (0., 0., 0.)) { +#endif single_dof_joint = Joint (JointTypeRevolute, rotation); } } @@ -368,7 +377,7 @@ unsigned int Model::AddBody( c_J.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); // workspace for joints with 3 dof - multdof3_S.push_back (Matrix63::Zero(6,3)); + multdof3_S.push_back (Matrix63::Zero()); multdof3_U.push_back (Matrix63::Zero()); multdof3_Dinv.push_back (Matrix3d::Zero()); multdof3_u.push_back (Vector3d::Zero()); @@ -398,7 +407,7 @@ unsigned int Model::AddBody( // Dynamic variables c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); - IA.push_back(SpatialMatrix::Zero(6,6)); + IA.push_back(SpatialMatrix::Zero()); pA.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); diff --git a/src/rbdl_mathutils.cc b/src/rbdl_mathutils.cc index 8bf73ed9..ea3fd0d9 100644 --- a/src/rbdl_mathutils.cc +++ b/src/rbdl_mathutils.cc @@ -33,6 +33,7 @@ RBDL_DLLAPI Matrix3d Matrix3dZero ( RBDL_DLLAPI SpatialVector SpatialVectorZero ( 0., 0., 0., 0., 0., 0.); +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { x = VectorNd::Zero(x.size()); @@ -55,12 +56,12 @@ RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { for (j = 0; j < n; j++) { pi = j; - double pv = fabs (A(j,pivot[j])); + Scalar pv = fabs (A(j,pivot[j])); // LOG << "j = " << j << " pv = " << pv << std::endl; // find the pivot for (k = j; k < n; k++) { - double pt = fabs (A(j,pivot[k])); + Scalar pt = fabs (A(j,pivot[k])); if (pt > pv) { pv = pt; pi = k; @@ -73,13 +74,13 @@ RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { } for (i = j + 1; i < n; i++) { - if (fabs(A(j,pivot[j])) <= std::numeric_limits::epsilon()) { + if (Scalar(fabs(A(j,pivot[j]))) <= Scalar(std::numeric_limits::epsilon())) { std::cerr << "Error: pivoting failed for matrix A = " << std::endl; std::cerr << "A = " << std::endl << A << std::endl; std::cerr << "b = " << b << std::endl; } // assert (fabs(A(j,pivot[j])) > std::numeric_limits::epsilon()); - double d = A(i,pivot[j])/A(j,pivot[j]); + Scalar d = A(i,pivot[j])/A(j,pivot[j]); b[i] -= b[j] * d; @@ -120,6 +121,7 @@ RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { return true; } +#endif RBDL_DLLAPI void SpatialMatrixSetSubmatrix( SpatialMatrix &dest, @@ -141,16 +143,17 @@ RBDL_DLLAPI void SpatialMatrixSetSubmatrix( dest(row*3 + 2,col*3 + 2) = matrix(2,2); } +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI bool SpatialMatrixCompareEpsilon ( const SpatialMatrix &matrix_a, const SpatialMatrix &matrix_b, - double epsilon) { - assert (epsilon >= 0.); + Scalar epsilon) { + assert (epsilon >= Scalar(0.)); unsigned int i, j; for (i = 0; i < 6; i++) { for (j = 0; j < 6; j++) { - if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) { + if (Scalar(fabs(matrix_a(i,j) - matrix_b(i,j))) >= Scalar(epsilon)) { std::cerr << "Expected:" << std::endl << matrix_a << std::endl << "but was" << std::endl @@ -166,12 +169,12 @@ RBDL_DLLAPI bool SpatialMatrixCompareEpsilon ( RBDL_DLLAPI bool SpatialVectorCompareEpsilon ( const SpatialVector &vector_a, const SpatialVector &vector_b, - double epsilon) { - assert (epsilon >= 0.); + Scalar epsilon) { + assert (epsilon >= Scalar(0.)); unsigned int i; for (i = 0; i < 6; i++) { - if (fabs(vector_a[i] - vector_b[i]) >= epsilon) { + if (Scalar(fabs(vector_a[i] - vector_b[i])) >= Scalar(epsilon)) { std::cerr << "Expected:" << std::endl << vector_a << std::endl << "but was" << std::endl @@ -182,10 +185,11 @@ RBDL_DLLAPI bool SpatialVectorCompareEpsilon ( return true; } +#endif RBDL_DLLAPI Matrix3d parallel_axis ( const Matrix3d &inertia, - double mass, + Scalar mass, const Vector3d &com) { Matrix3d com_cross = VectorCrossMatrix (com); @@ -203,8 +207,8 @@ RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &r) { ); } -RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) { - double s, c; +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const Scalar &xrot) { + Scalar s, c; s = sin (xrot); c = cos (xrot); @@ -218,8 +222,8 @@ RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) { ); } -RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) { - double s, c; +RBDL_DLLAPI SpatialMatrix Xroty_mat (const Scalar &yrot) { + Scalar s, c; s = sin (yrot); c = cos (yrot); @@ -233,8 +237,8 @@ RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) { ); } -RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot) { - double s, c; +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const Scalar &zrot) { + Scalar s, c; s = sin (zrot); c = cos (zrot); @@ -305,7 +309,7 @@ RBDL_DLLAPI void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd } RBDL_DLLAPI void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) { - for (int i = model.qdot_size; i > 0; i--) { + for (unsigned int i = model.qdot_size; i > 0; i--) { x[i - 1] = x[i - 1] / L(i - 1,i - 1); unsigned int j = model.lambda_q[i]; while (j != 0) { diff --git a/src/rbdl_utils.cc b/src/rbdl_utils.cc index f8a7a4bd..01a16fe0 100644 --- a/src/rbdl_utils.cc +++ b/src/rbdl_utils.cc @@ -24,6 +24,7 @@ namespace Utils using namespace std; using namespace Math; +#ifndef RBDL_USE_CASADI_MATH string get_dof_name (const SpatialVector &joint_dof) { if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) { @@ -44,6 +45,7 @@ string get_dof_name (const SpatialVector &joint_dof) dof_stream << "custom_axis (" << joint_dof.transpose() << ")"; return dof_stream.str(); } +#endif string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) @@ -60,8 +62,8 @@ string get_body_name (const RigidBodyDynamics::Model &model, return model.GetBodyName(body_id); } -RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) -{ +#ifndef RBDL_USE_CASADI_MATH +RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) { stringstream result (""); unsigned int q_index = 0; @@ -81,7 +83,9 @@ RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) return result.str(); } +#endif +#ifndef RBDL_USE_CASADI_MATH std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) { @@ -147,7 +151,9 @@ std::string print_hierarchy (const RigidBodyDynamics::Model &model, return result.str(); } +#endif +#ifndef RBDL_USE_CASADI_MATH RBDL_DLLAPI std::string GetModelHierarchy (const Model &model) { stringstream result (""); @@ -156,6 +162,7 @@ RBDL_DLLAPI std::string GetModelHierarchy (const Model &model) return result.str(); } +#endif RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model) { @@ -185,7 +192,7 @@ RBDL_DLLAPI void CalcCenterOfMass ( const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd *qddot, - double &mass, + Scalar &mass, Math::Vector3d &com, Math::Vector3d *com_velocity, Math::Vector3d *com_acceleration, @@ -216,9 +223,9 @@ RBDL_DLLAPI void CalcCenterOfMass ( } } - SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3)); - SpatialVector htot (SpatialVector::Zero(6)); - SpatialVector hdot_tot (SpatialVector::Zero(6)); + SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero()); + SpatialVector htot (SpatialVector::Zero()); + SpatialVector hdot_tot (SpatialVector::Zero()); for (size_t i = model.mBodies.size() - 1; i > 0; i--) { unsigned int lambda = model.lambda[i]; @@ -301,9 +308,9 @@ RBDL_DLLAPI void CalcZeroMomentPoint ( model.Ic[i] * model.v[i]); } - SpatialRigidBodyInertia I_tot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3)); - SpatialVector h_tot (SpatialVector::Zero(6)); - SpatialVector hdot_tot (SpatialVector::Zero(6)); + SpatialRigidBodyInertia I_tot (0., Vector3d (0., 0., 0.), Matrix3d::Zero()); + SpatialVector h_tot (SpatialVector::Zero()); + SpatialVector hdot_tot (SpatialVector::Zero()); // compute total change of momentum and CoM wrt to root body (idx = 0) // by recursively summing up local change of momentum @@ -325,7 +332,7 @@ RBDL_DLLAPI void CalcZeroMomentPoint ( } // compute CoM from mass and total inertia - const double mass = I_tot.m; + const Scalar mass = I_tot.m; const Vector3d com = I_tot.h / mass; // project angular momentum onto CoM @@ -353,12 +360,12 @@ RBDL_DLLAPI void CalcZeroMomentPoint ( return; } -RBDL_DLLAPI double CalcPotentialEnergy ( +RBDL_DLLAPI Scalar CalcPotentialEnergy ( Model &model, const Math::VectorNd &q, bool update_kinematics) { - double mass; + Scalar mass; Vector3d com; CalcCenterOfMass ( model, @@ -380,7 +387,7 @@ RBDL_DLLAPI double CalcPotentialEnergy ( return mass * com.dot(g); } -RBDL_DLLAPI double CalcKineticEnergy ( +RBDL_DLLAPI Scalar CalcKineticEnergy ( Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, @@ -390,7 +397,7 @@ RBDL_DLLAPI double CalcKineticEnergy ( UpdateKinematicsCustom (model, &q, &qdot, NULL); } - double result = 0.; + Scalar result = 0.; for (size_t i = 1; i < model.mBodies.size(); i++) { result += 0.5 * model.v[i].transpose() * (model.I[i] * model.v[i]); diff --git a/tests/Human36Fixture.h b/tests/Human36Fixture.h index fcee396f..65f1ce47 100644 --- a/tests/Human36Fixture.h +++ b/tests/Human36Fixture.h @@ -245,7 +245,7 @@ struct Human36 { Body hand_body = create_body (SegmentHand); Body head_body = create_body (SegmentHead); - Matrix3d zero_matrix (Matrix3d::Zero(3,3)); + Matrix3d zero_matrix (Matrix3d::Zero()); Body null_body (0., Vector3d (0., 0., 0.), zero_matrix); Joint free_flyer (