commit 5b6d7ec170b52b7c64ad374b86fa96d2291c973b Author: Martin Felis Date: Sat Oct 3 22:55:14 2020 +0200 Initial commit diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..1d44113 --- /dev/null +++ b/.clang-format @@ -0,0 +1,13 @@ +--- +BasedOnStyle: Google +AlignAfterOpenBracket: AlwaysBreak +AlignOperands: 'true' +AllowAllArgumentsOnNextLine: 'false' +AllowAllParametersOfDeclarationOnNextLine: 'false' +BinPackArguments: 'false' +BinPackParameters: 'false' +BreakBeforeBinaryOperators: NonAssignment +ExperimentalAutoDetectBinPacking: 'false' +ReflowComments: 'false' + +... diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..12e56ad --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.*.swp + +.idea/** +build** +cmake-build-** diff --git a/3rdparty/libccd/.gitignore b/3rdparty/libccd/.gitignore new file mode 100644 index 0000000..f32c823 --- /dev/null +++ b/3rdparty/libccd/.gitignore @@ -0,0 +1,14 @@ +Makefile.in +autom4te.cache/* +aclocal.m4 +config.guess +config.sub +configure +depcomp +install-sh +ltmain.sh +missing +*~ +src/gjk/config.h.in +build/* +ccd.pc diff --git a/3rdparty/libccd/.travis.yml b/3rdparty/libccd/.travis.yml new file mode 100644 index 0000000..2cde2b9 --- /dev/null +++ b/3rdparty/libccd/.travis.yml @@ -0,0 +1,23 @@ +dist: trusty +sudo: required + +language: c +compiler: + - gcc + - clang + +env: + global: + - PREFIX="$TRAVIS_BUILD_DIR/build/install" + matrix: + - USE_AUTOTOOLS=yes + - USE_CMAKE=yes + - USE_MAKEFILE=yes USE_DOUBLE=yes + - USE_MAKEFILE=yes USE_SINGLE=yes + +script: + - mkdir -p "$PREFIX" + - if [[ "$USE_AUTOTOOLS" == "yes" ]]; then ./bootstrap && cd build && ../configure --prefix "$PREFIX"; fi + - if [[ "$USE_CMAKE" == "yes" ]]; then cd build && cmake "-DCMAKE_INSTALL_PREFIX=$PREFIX" ..; fi + - if [[ "$USE_MAKEFILE" == "yes" ]]; then cd src; fi + - make && make install diff --git a/3rdparty/libccd/BSD-LICENSE b/3rdparty/libccd/BSD-LICENSE new file mode 100644 index 0000000..1cb2dc5 --- /dev/null +++ b/3rdparty/libccd/BSD-LICENSE @@ -0,0 +1,44 @@ +libccd +------- + +Copyright (c)2010-2012 Daniel Fiser , +Intelligent and Mobile Robotics Group, Department of Cybernetics, +Faculty of Electrical Engineering, Czech Technical University in Prague. +All rights reserved. + + +This work was supported by SYMBRION and REPLICATOR projects. +The SYMBRION project is funded by European Commission within the work +"Future and Emergent Technologies Proactive" under grant agreement no. +216342. +The REPLICATOR project is funded within the work programme "Cognitive +Systems, Interaction, Robotics" under grant agreement no. 216240. +http://www.symbrion.eu/ +http://www.replicators.eu/ + + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + - Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + - Neither the name of the University nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/3rdparty/libccd/CMakeLists.txt b/3rdparty/libccd/CMakeLists.txt new file mode 100644 index 0000000..31aa7b7 --- /dev/null +++ b/3rdparty/libccd/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.15) + +if(POLICY CMP0042) + cmake_policy(SET CMP0042 NEW) +endif() + +# Can not explicitly declared the software as C in project command due to bug: +# https://gitlab.kitware.com/cmake/cmake/issues/16967 +project(libccd) + +set(CCD_VERSION_MAJOR 2) +set(CCD_VERSION_MINOR 0) +set(CCD_VERSION ${CCD_VERSION_MAJOR}.${CCD_VERSION_MINOR}) + +set(CCD_SOVERSION 2) + +project (CCD + VERSION ${CCD_VERSION} + LANGUAGES C) + +# Include GNUInstallDirs to get canonical paths +include(GNUInstallDirs) +include(CTest) + +option(BUILD_DOCUMENTATION "Build the documentation" OFF) + +option(BUILD_SHARED_LIBS "Build libccd as a shared library" ON) + +option(ENABLE_DOUBLE_PRECISION + "Enable double precision computations instead of single precision" OFF) + +# Option for some bundle-like build system in order not to expose +# any FCL binary symbols in their public ABI +option(CCD_HIDE_ALL_SYMBOLS "Hide all binary symbols" OFF) +if (CCD_HIDE_ALL_SYMBOLS) + add_definitions("-DCCD_STATIC_DEFINE") +endif() + +# set the default build type +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING + "Choose the type of build; options are Debug Release RelWithDebInfo MinSizeRel" + FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY + STRINGS + Debug + Release + RelWithDebInfo + MinSizeRel) +endif() + +add_subdirectory(src) + +if(BUILD_DOCUMENTATION) + add_subdirectory(doc) +endif() + +include(CMakePackageConfigHelpers) + +configure_package_config_file(ccd-config.cmake.in ccd-config.cmake + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/ccd" + PATH_VARS CMAKE_INSTALL_INCLUDEDIR CMAKE_INSTALL_LIBDIR + NO_CHECK_REQUIRED_COMPONENTS_MACRO) + +write_basic_package_version_file(ccd-config-version.cmake + VERSION ${CCD_VERSION} COMPATIBILITY AnyNewerVersion) + +install(FILES + "${CMAKE_BINARY_DIR}/ccd-config.cmake" + "${CMAKE_BINARY_DIR}/ccd-config-version.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/ccd") + +set(CCD_PKGCONFIG_DESCRIPTION + "Library for collision detection between convex shapes") +configure_file(ccd.pc.in ccd.pc @ONLY) +install(FILES "${CMAKE_BINARY_DIR}/ccd.pc" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/pkgconfig") + +install(FILES BSD-LICENSE DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/doc/ccd") diff --git a/3rdparty/libccd/Makefile.am b/3rdparty/libccd/Makefile.am new file mode 100644 index 0000000..6f338cc --- /dev/null +++ b/3rdparty/libccd/Makefile.am @@ -0,0 +1,6 @@ +SUBDIRS = src + +EXTRA_DIST = doc \ + BSD-LICENSE \ + README.md \ + make-release.sh diff --git a/3rdparty/libccd/README.md b/3rdparty/libccd/README.md new file mode 100644 index 0000000..274521d --- /dev/null +++ b/3rdparty/libccd/README.md @@ -0,0 +1,296 @@ +# libccd [![Build Status](https://travis-ci.org/danfis/libccd.svg?branch=master)](https://travis-ci.org/danfis/libccd) + +***libccd*** is library for a collision detection between two convex shapes. +libccd implements variation on Gilbert–Johnson–Keerthi algorithm plus Expand +Polytope Algorithm (EPA) and also implements algorithm Minkowski Portal +Refinement (MPR, a.k.a. XenoCollide) as described in Game Programming Gems 7. + +libccd is the only available open source library of my knowledge that include +MPR algorithm working in 3-D space. However, there is a library called +[mpr2d](http://code.google.com/p/mpr2d/), implemented in D programming +language, that works in 2-D space. + +libccd is currently part of: + +1. [ODE](http://www.ode.org/) library (see ODE's *./configure --help* how to enable it), +2. [FCL](http://www.ros.org/wiki/fcl) library from [Willow Garage](http://www.willowgarage.com/), +3. [Bullet3](http://bulletphysics.org/) library (https://github.com/bulletphysics/bullet3). + +For implementation details on GJK algorithm, see +http://www.win.tue.nl/~gino/solid/jgt98convex.pdf. + + +## Dependencies + +This library is currently based only on standard libraries. +The only exception are testsuites that are built on top of CU +(https://github.com/danfis/cu) library licensed under LGPL, however only +testing depends on it and libccd library itself can be distributed without it. + + +## License + +libccd is licensed under OSI-approved 3-clause BSD License, text of license +is distributed along with source code in BSD-LICENSE file. +Each file should include license notice, the rest should be considered as +licensed under 3-clause BSD License. + + +## Compile And Install + +libccd contains several mechanisms for compiling and installing. Using a simple Makefile, using autotools, and using CMake. + +### 1. Using Makefile + +Directory src/ contains Makefile that should contain everything needed for compilation and installation: +```sh + $ cd src/ + $ make + $ make install +``` + +Library libccd is by default compiled in double precision of floating point numbers - you can change this by options *USE_SINGLE/USE_DOUBLE*, i.e.: +```sh + $ make USE_SINGLE=yes +``` +will compile library in single precision. + +Installation directory can be changed by options PREFIX, INCLUDEDIR and LIBDIR. +For more info type 'make help'. + +### 2. Using Autotools + +libccd also contains support for autotools: +Generate configure script etc.: +```sh + $ ./bootstrap +``` + +Create new build/ directory: +```sh + $ mkdir build && cd build +``` + +Run configure script: +```sh + $ ../configure +``` + +Run make and make install: +```sh + $ make && make install +``` + +configure script can change the way libccd is compiled and installed, most significant option is *--enable-double-precision* which enables double precision (single is default in this case). + +### 3. Using CMake + +To build using `make`: +```sh + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" .. + $ make && make install +``` + +To build using `ninja`: +```sh + $ mkdir build && cd build + $ cmake -G Ninja .. + $ ninja && ninja install +``` + +Other build tools may be using by specifying a different generator. For example: +```sh + $ cmake -G Xcode .. +``` + +```bat + > cmake -G "Visual Studio 14 2015" .. +``` + +To compile using double precision, set the `ENABLE_DOUBLE_PRECISION` option: +```sh + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DENABLE_DOUBLE_PRECISION=ON .. + $ make && make install +``` + +To build libccd as a shared library, set the `BUILD_SHARED_LIBS` option: +```sh + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON .. + $ make && make install +``` + +To build the test suite, set the `BUILD_TESTING` option: +```sh + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DBUILD_TESTING=ON .. + $ make && make test +``` + +The installation directory may be changed using the `CMAKE_INSTALL_PREFIX` variable: +```sh + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DCMAKE_INSTALL_PREFIX=/path/to/install .. + $ make && make install +``` + +## GJK - Intersection Test + +This section describes how to use libccd for testing if two convex objects intersects (i.e., 'yes/no' test) using Gilbert-Johnson-Keerthi (GJK) algorithm. + +Procedure is very simple (and is similar for usages of library): + +1. Include ** file. +2. Implement support function for specific shapes. Support function is function that returns furthest point from object (shape) in specified direction. +3. Set up *ccd_t* structure. +4. Run ccdGJKIntersect() function on desired objects. + +Here is skeleton of simple program: +```cpp + #include + #include // for work with quaternions + + /** Support function for box */ + void support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) + { + // assume that obj_t is user-defined structure that holds info about + // object (in this case box: x, y, z, pos, quat - dimensions of box, + // position and rotation) + obj_t *obj = (obj_t *)_obj; + ccd_vec3_t dir; + ccd_quat_t qinv; + + // apply rotation on direction vector + ccdVec3Copy(&dir, _dir); + ccdQuatInvert2(&qinv, &obj->quat); + ccdQuatRotVec(&dir, &qinv); + + // compute support point in specified direction + ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * box->x * CCD_REAL(0.5), + ccdSign(ccdVec3Y(&dir)) * box->y * CCD_REAL(0.5), + ccdSign(ccdVec3Z(&dir)) * box->z * CCD_REAL(0.5)); + + // transform support point according to position and rotation of object + ccdQuatRotVec(v, &obj->quat); + ccdVec3Add(v, &obj->pos); + } + + int main(int argc, char *argv[]) + { + ... + + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.max_iterations = 100; // maximal number of iterations + + int intersect = ccdGJKIntersect(obj1, obj2, &ccd); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + } +``` + +## GJK + EPA - Penetration Of Two Objects + +If you want to obtain also penetration info about two intersection objects ccdGJKPenetration() function can be used. + +Procedure is almost same as for previous case: +```cpp + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.max_iterations = 100; // maximal number of iterations + ccd.epa_tolerance = 0.0001; // maximal tolerance fro EPA part + + ccd_real_t depth; + ccd_vec3_t dir, pos; + int intersect = ccdGJKPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + // now intersect holds 0 if obj1 and obj2 intersect, -1 otherwise + // in depth, dir and pos is stored penetration depth, direction of + // separation vector and position in global coordinate system + } +``` + +## MPR - Intersection Test + +libccd also provides MPR - Minkowski Portal Refinement algorithm that can be used for testing if two objects intersects. + +Procedure is similar to the one used for GJK algorithm. Support function is same but also function that returns center (or any point near center) of given object must be implemented: +```cpp + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + + /** Center function - returns center of object */ + void center(const void *_obj, ccd_vec3_t *center) + { + obj_t *obj = (obj_t *)_obj; + ccdVec3Copy(center, &obj->pos); + } + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.center1 = center; // center function for first object + ccd.center2 = center; // center function for second object + ccd.mpr_tolerance = 0.0001; // maximal tolerance + + int intersect = ccdMPRIntersect(obj1, obj2, &ccd); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + } +``` + + +## MPR - Penetration Of Two Objects + +Using MPR algorithm for obtaining penetration info about two intersection objects is equally easy as in previous case instead ccdMPRPenetration() function is used: +```cpp + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + /** Center function is same as in prevous case */ + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.center1 = center; // center function for first object + ccd.center2 = center; // center function for second object + ccd.mpr_tolerance = 0.0001; // maximal tolerance + + ccd_real_t depth; + ccd_vec3_t dir, pos; + int intersect = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + // now intersect holds 0 if obj1 and obj2 intersect, -1 otherwise + // in depth, dir and pos is stored penetration depth, direction of + // separation vector and position in global coordinate system + } +``` diff --git a/3rdparty/libccd/bootstrap b/3rdparty/libccd/bootstrap new file mode 100755 index 0000000..2ece90c --- /dev/null +++ b/3rdparty/libccd/bootstrap @@ -0,0 +1,7 @@ +#!/bin/sh + +libtoolize -f -c +aclocal +autoheader -f +autoconf +automake -a --foreign -f -c diff --git a/3rdparty/libccd/ccd-config.cmake.in b/3rdparty/libccd/ccd-config.cmake.in new file mode 100644 index 0000000..8f13af9 --- /dev/null +++ b/3rdparty/libccd/ccd-config.cmake.in @@ -0,0 +1,14 @@ +@PACKAGE_INIT@ + +set(CCD_VERSION_MAJOR @CCD_VERSION_MAJOR@) +set(CCD_VERSION_MINOR @CCD_VERSION_MINOR@) +set(CCD_VERSION @CCD_VERSION@) + +set(CCD_SOVERSION @CCD_SOVERSION@) + +set(CCD_FOUND ON) +set_and_check(CCD_INCLUDE_DIRS "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@") +set_and_check(CCD_LIBRARY_DIRS "@PACKAGE_CMAKE_INSTALL_LIBDIR@") +set(CCD_LIBRARIES ccd) + +include("${CMAKE_CURRENT_LIST_DIR}/ccd-targets.cmake") diff --git a/3rdparty/libccd/ccd.pc.in b/3rdparty/libccd/ccd.pc.in new file mode 100644 index 0000000..dbddb3d --- /dev/null +++ b/3rdparty/libccd/ccd.pc.in @@ -0,0 +1,13 @@ +# Generated by CMake @CMAKE_VERSION@ for ccd + +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@ +includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ + +Name: @PROJECT_NAME@ +Description: @CCD_PKGCONFIG_DESCRIPTION@ +Version: @CCD_VERSION@ +Requires: @CCD_PKGCONFIG_REQUIRES@ +Libs: -L${libdir} -lccd @CCD_PKGCONFIG_EXTRA_LIBS@ +Cflags: -I${includedir} diff --git a/3rdparty/libccd/configure.ac b/3rdparty/libccd/configure.ac new file mode 100644 index 0000000..abd80c2 --- /dev/null +++ b/3rdparty/libccd/configure.ac @@ -0,0 +1,50 @@ +# -*- Autoconf -*- +# Process this file with autoconf to produce a configure script. + +#AC_PREREQ([2.65]) +AC_INIT([libccd], [2.0], [danfis@danfis.cz]) +AC_CONFIG_SRCDIR([src/ccd.c]) +AC_CONFIG_HEADERS([src/ccd/config.h]) +AM_INIT_AUTOMAKE + +# Checks for programs. +AC_PROG_CXX +AC_PROG_CC +AC_PROG_INSTALL +AC_DISABLE_SHARED +LT_INIT + +# Checks for libraries. +AC_CHECK_LIB([m], [main]) +# FIXME: Replace `main' with a function in `-lrt': +AC_CHECK_LIB([rt], [main]) + +# Checks for header files. +AC_CHECK_HEADERS([float.h stdlib.h string.h unistd.h]) + +# Checks for typedefs, structures, and compiler characteristics. +AC_TYPE_SIZE_T + +# Checks for library functions. +AC_FUNC_FORK +AC_FUNC_REALLOC +AC_CHECK_FUNCS([clock_gettime]) + +use_double=no +AC_ARG_ENABLE(double-precision, + AS_HELP_STRING([--enable-double-precision], + [enable double precision computations instead of single precision]), + [use_double=yes]) +if test $use_double = no +then + AC_DEFINE([CCD_SINGLE], [], [use single precision]) +else + AC_DEFINE([CCD_DOUBLE], [], [use double precision]) +fi + + +AC_CONFIG_FILES([Makefile + src/Makefile + src/testsuites/Makefile + src/testsuites/cu/Makefile]) +AC_OUTPUT diff --git a/3rdparty/libccd/doc/CMakeLists.txt b/3rdparty/libccd/doc/CMakeLists.txt new file mode 100644 index 0000000..42b353f --- /dev/null +++ b/3rdparty/libccd/doc/CMakeLists.txt @@ -0,0 +1,39 @@ +find_program(SPHINX_EXECUTABLE NAMES sphinx-build sphinx-build2) + +if(NOT SPHINX_EXECUTABLE) + message(FATAL_ERROR "Could NOT find required executable sphinx-build") +endif() + +add_custom_target(doc ALL) + +set(CCD_DOCTREE_DIR "${CMAKE_CURRENT_BINARY_DIR}/.doctrees") +set(CCD_HTML_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/html") + +add_custom_target(html COMMAND + "${SPHINX_EXECUTABLE}" -b html -d "${CCD_DOCTREE_DIR}" -q + "${CMAKE_CURRENT_SOURCE_DIR}" "${CCD_HTML_OUTPUT_DIR}") +add_dependencies(doc html) + +install(DIRECTORY "${CCD_HTML_OUTPUT_DIR}" + DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/doc/ccd") + +set(CCD_DOC_ADDITIONAL_MAKE_CLEAN_FILES + "${CCD_DOCTREE_DIR}" + "${CCD_HTML_OUTPUT_DIR}") + +if(NOT WIN32) + set(CCD_MAN_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/man") + + add_custom_target(man COMMAND + "${SPHINX_EXECUTABLE}" -b man -d "${CCD_DOCTREE_DIR}" -q + "${CMAKE_CURRENT_SOURCE_DIR}" "${CCD_MAN_OUTPUT_DIR}") + add_dependencies(doc man) + + install(DIRECTORY "${CCD_MAN_OUTPUT_DIR}/" + DESTINATION "${CMAKE_INSTALL_MANDIR}/man1") + + list(APPEND CCD_DOC_ADDITIONAL_MAKE_CLEAN_FILES "${CCD_MAN_OUTPUT_DIR}") +endif() + +set_directory_properties(PROPERTIES + ADDITIONAL_MAKE_CLEAN_FILES ${CCD_DOC_ADDITIONAL_MAKE_CLEAN_FILES}) diff --git a/3rdparty/libccd/doc/Makefile b/3rdparty/libccd/doc/Makefile new file mode 100644 index 0000000..831bb18 --- /dev/null +++ b/3rdparty/libccd/doc/Makefile @@ -0,0 +1,225 @@ +# Makefile for Sphinx documentation +# + +# You can set these variables from the command line. +SPHINXOPTS = +SPHINXBUILD = sphinx-build +PAPER = +BUILDDIR = _build + +# Internal variables. +PAPEROPT_a4 = -D latex_paper_size=a4 +PAPEROPT_letter = -D latex_paper_size=letter +ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . +# the i18n builder cannot share the environment and doctrees with the others +I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . + +.PHONY: help +help: + @echo "Please use \`make ' where is one of" + @echo " html to make standalone HTML files" + @echo " dirhtml to make HTML files named index.html in directories" + @echo " singlehtml to make a single large HTML file" + @echo " pickle to make pickle files" + @echo " json to make JSON files" + @echo " htmlhelp to make HTML files and a HTML help project" + @echo " qthelp to make HTML files and a qthelp project" + @echo " applehelp to make an Apple Help Book" + @echo " devhelp to make HTML files and a Devhelp project" + @echo " epub to make an epub" + @echo " epub3 to make an epub3" + @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" + @echo " latexpdf to make LaTeX files and run them through pdflatex" + @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" + @echo " text to make text files" + @echo " man to make manual pages" + @echo " texinfo to make Texinfo files" + @echo " info to make Texinfo files and run them through makeinfo" + @echo " gettext to make PO message catalogs" + @echo " changes to make an overview of all changed/added/deprecated items" + @echo " xml to make Docutils-native XML files" + @echo " pseudoxml to make pseudoxml-XML files for display purposes" + @echo " linkcheck to check all external links for integrity" + @echo " doctest to run all doctests embedded in the documentation (if enabled)" + @echo " coverage to run coverage check of the documentation (if enabled)" + @echo " dummy to check syntax errors of document sources" + +.PHONY: clean +clean: + rm -rf $(BUILDDIR)/* + +.PHONY: html +html: + $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." + +.PHONY: dirhtml +dirhtml: + $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." + +.PHONY: singlehtml +singlehtml: + $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml + @echo + @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." + +.PHONY: pickle +pickle: + $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle + @echo + @echo "Build finished; now you can process the pickle files." + +.PHONY: json +json: + $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json + @echo + @echo "Build finished; now you can process the JSON files." + +.PHONY: htmlhelp +htmlhelp: + $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp + @echo + @echo "Build finished; now you can run HTML Help Workshop with the" \ + ".hhp project file in $(BUILDDIR)/htmlhelp." + +.PHONY: qthelp +qthelp: + $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp + @echo + @echo "Build finished; now you can run "qcollectiongenerator" with the" \ + ".qhcp project file in $(BUILDDIR)/qthelp, like this:" + @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/libccd.qhcp" + @echo "To view the help file:" + @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/libccd.qhc" + +.PHONY: applehelp +applehelp: + $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp + @echo + @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." + @echo "N.B. You won't be able to view it unless you put it in" \ + "~/Library/Documentation/Help or install it in your application" \ + "bundle." + +.PHONY: devhelp +devhelp: + $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp + @echo + @echo "Build finished." + @echo "To view the help file:" + @echo "# mkdir -p $$HOME/.local/share/devhelp/libccd" + @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/libccd" + @echo "# devhelp" + +.PHONY: epub +epub: + $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub + @echo + @echo "Build finished. The epub file is in $(BUILDDIR)/epub." + +.PHONY: epub3 +epub3: + $(SPHINXBUILD) -b epub3 $(ALLSPHINXOPTS) $(BUILDDIR)/epub3 + @echo + @echo "Build finished. The epub3 file is in $(BUILDDIR)/epub3." + +.PHONY: latex +latex: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo + @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." + @echo "Run \`make' in that directory to run these through (pdf)latex" \ + "(use \`make latexpdf' here to do that automatically)." + +.PHONY: latexpdf +latexpdf: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through pdflatex..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: latexpdfja +latexpdfja: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through platex and dvipdfmx..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: text +text: + $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text + @echo + @echo "Build finished. The text files are in $(BUILDDIR)/text." + +.PHONY: man +man: + $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man + @echo + @echo "Build finished. The manual pages are in $(BUILDDIR)/man." + +.PHONY: texinfo +texinfo: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo + @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." + @echo "Run \`make' in that directory to run these through makeinfo" \ + "(use \`make info' here to do that automatically)." + +.PHONY: info +info: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo "Running Texinfo files through makeinfo..." + make -C $(BUILDDIR)/texinfo info + @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." + +.PHONY: gettext +gettext: + $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale + @echo + @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." + +.PHONY: changes +changes: + $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes + @echo + @echo "The overview file is in $(BUILDDIR)/changes." + +.PHONY: linkcheck +linkcheck: + $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck + @echo + @echo "Link check complete; look for any errors in the above output " \ + "or in $(BUILDDIR)/linkcheck/output.txt." + +.PHONY: doctest +doctest: + $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest + @echo "Testing of doctests in the sources finished, look at the " \ + "results in $(BUILDDIR)/doctest/output.txt." + +.PHONY: coverage +coverage: + $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage + @echo "Testing of coverage in the sources finished, look at the " \ + "results in $(BUILDDIR)/coverage/python.txt." + +.PHONY: xml +xml: + $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml + @echo + @echo "Build finished. The XML files are in $(BUILDDIR)/xml." + +.PHONY: pseudoxml +pseudoxml: + $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml + @echo + @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." + +.PHONY: dummy +dummy: + $(SPHINXBUILD) -b dummy $(ALLSPHINXOPTS) $(BUILDDIR)/dummy + @echo + @echo "Build finished. Dummy builder generates no files." diff --git a/3rdparty/libccd/doc/_build/.dir b/3rdparty/libccd/doc/_build/.dir new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/doc/_static/.dir b/3rdparty/libccd/doc/_static/.dir new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/doc/_templates/.dir b/3rdparty/libccd/doc/_templates/.dir new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/doc/compile-and-install.rst b/3rdparty/libccd/doc/compile-and-install.rst new file mode 100644 index 0000000..274ec64 --- /dev/null +++ b/3rdparty/libccd/doc/compile-and-install.rst @@ -0,0 +1,122 @@ +Compile And Install +==================== + +libccd contains several mechanisms for compiling and installing. +Using a simple Makefile, using autotools, and using CMake. + + +1. Using Makefile +------------------ +Directory ``src/`` contains Makefile that should contain everything needed for compilation and installation: + +.. code-block:: bash + + $ cd src/ + $ make + $ make install + +Library libccd is by default compiled in double precision of floating point +numbers - you can change this by options ``USE_SINGLE``/``USE_DOUBLE``, i.e.: + +.. code-block:: bash + + $ make USE_SINGLE=yes + +will compile library in single precision. +Installation directory can be changed by options ``PREFIX``, ``INCLUDEDIR`` +and ``LIBDIR``. +For more info type '``make help``'. + + +2. Using Autotools +------------------- +libccd also contains support for autotools: +Generate configure script etc.: + +.. code-block:: bash + + $ ./bootstrap + +Create new ``build/`` directory: + +.. code-block:: bash + + $ mkdir build && cd build + +Run configure script: + +.. code-block:: bash + + $ ../configure + +Run make and make install: + +.. code-block:: bash + + $ make && make install + +configure script can change the way libccd is compiled and installed, most +significant option is ``--enable-double-precision`` which enables double +precision (single is default in this case). + +3. Using CMake +--------------- + +To build using ``make``: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" .. + $ make && make install + +To build using ``ninja``: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G Ninja .. + $ ninja && ninja install + +Other build tools may be using by specifying a different generator. For example: + +.. code-block:: bash + + $ cmake -G Xcode .. + +.. code-block:: batch + + > cmake -G "Visual Studio 14 2015" .. + +To compile using double precision, set the ``ENABLE_DOUBLE_PRECISION`` option: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DENABLE_DOUBLE_PRECISION=ON .. + $ make && make install + +To build libccd as a shared library, set the ``BUILD_SHARED_LIBS`` option: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON .. + $ make && make install + +To build the test suite, set the ``BUILD_TESTING`` option: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DBUILD_TESTING=ON .. + $ make && make test + + +The installation directory may be changed by specifying the ``CMAKE_INSTALL_PREFIX`` variable: + +.. code-block:: bash + + $ mkdir build && cd build + $ cmake -G "Unix Makefiles" -DCMAKE_INSTALL_PREFIX=/path/to/install .. + $ make && make install diff --git a/3rdparty/libccd/doc/conf.py b/3rdparty/libccd/doc/conf.py new file mode 100644 index 0000000..9fbf413 --- /dev/null +++ b/3rdparty/libccd/doc/conf.py @@ -0,0 +1,338 @@ +# -*- coding: utf-8 -*- +# +# libccd documentation build configuration file, created by +# sphinx-quickstart on Fri Sep 9 14:53:37 2016. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +# import os +# import sys +# sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.mathjax'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The encoding of source files. +# +# source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'libccd' +copyright = u'2013, Daniel Fiser ' +author = u'Daniel Fiser' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = u'2.0' +# The full version, including alpha/beta/rc tags. +release = u'2.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +# +# today = '' +# +# Else, today_fmt is used as the format for a strftime call. +# +# today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This patterns also effect to html_static_path and html_extra_path +exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +# +# default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +# +# add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +# +# add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +# +# show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +# modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +# keep_warnings = False + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = False + + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +# html_theme_path = [] + +# The name for this set of Sphinx documents. +# " v documentation" by default. +# +# html_title = u'libccd Documentation' + +# A shorter title for the navigation bar. Default is the same as html_title. +# +# html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +# +# html_logo = None + +# The name of an image file (relative to this directory) to use as a favicon of +# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +# +# html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +# +# html_extra_path = [] + +# If not None, a 'Last updated on:' timestamp is inserted at every page +# bottom, using the given strftime format. +# The empty string is equivalent to '%b %d, %Y'. +# +# html_last_updated_fmt = None + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +# +# html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +# +# html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +# +# html_additional_pages = {} + +# If false, no module index is generated. +# +# html_domain_indices = True + +# If false, no index is generated. +# +# html_use_index = True + +# If true, the index is split into individual pages for each letter. +# +# html_split_index = False + +# If true, links to the reST sources are added to the pages. +# +# html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +# +# html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +# +# html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +# +# html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +# html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' +# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr', 'zh' +# +# html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# 'ja' uses this config value. +# 'zh' user can custom change `jieba` dictionary path. +# +# html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +# +# html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'libccddoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + # + # 'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + # + # 'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + # + # 'preamble': '', + + # Latex figure (float) alignment + # + # 'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + (master_doc, 'libccd.tex', u'libccd Documentation', + author, 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +# +# latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +# +# latex_use_parts = False + +# If true, show page references after internal links. +# +# latex_show_pagerefs = False + +# If true, show URL addresses after external links. +# +# latex_show_urls = False + +# Documents to append as an appendix to all manuals. +# +# latex_appendices = [] + +# It false, will not define \strong, \code, itleref, \crossref ... but only +# \sphinxstrong, ..., \sphinxtitleref, ... To help avoid clash with user added +# packages. +# +# latex_keep_old_macro_names = True + +# If false, no module index is generated. +# +# latex_domain_indices = True + + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'libccd', u'libccd Documentation', + [author], 1) +] + +# If true, show URL addresses after external links. +# +# man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'libccd', u'libccd Documentation', + author, 'libccd', 'One line description of project.', + 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +# +# texinfo_appendices = [] + +# If false, no module index is generated. +# +# texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +# +# texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +# +# texinfo_no_detailmenu = False diff --git a/3rdparty/libccd/doc/examples.rst b/3rdparty/libccd/doc/examples.rst new file mode 100644 index 0000000..59c0129 --- /dev/null +++ b/3rdparty/libccd/doc/examples.rst @@ -0,0 +1,185 @@ +Example of Usage +================= + +1. GJK - Intersection Test +--------------------------- +This section describes how to use **libccd** for testing if two convex objects intersects (i.e., 'yes/no' test) using Gilbert-Johnson-Keerthi (GJK) algorithm. + +Procedure is very simple (and similar to the usage of the rest of the +library): + +#. Include ```` file. +#. Implement support function for specific shapes. Support function is + function that returns furthest point from object (shape) in specified + direction. +#. Set up ``ccd_t`` structure. +#. Run ``ccdGJKIntersect()`` function on desired objects. + + +Here is a skeleton of simple program: + +.. code-block:: c + + #include + #include // for work with quaternions + + /** Support function for box */ + void support(const void *obj, const ccd_vec3_t *dir, + ccd_vec3_t *vec) + { + // assume that obj_t is user-defined structure that holds info about + // object (in this case box: x, y, z, pos, quat - dimensions of box, + // position and rotation) + obj_t *obj = (obj_t *)_obj; + ccd_vec3_t dir; + ccd_quat_t qinv; + + // apply rotation on direction vector + ccdVec3Copy(&dir, _dir); + ccdQuatInvert2(&qinv, &obj->quat); + ccdQuatRotVec(&dir, &qinv); + + // compute support point in specified direction + ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * box->x * CCD_REAL(0.5), + ccdSign(ccdVec3Y(&dir)) * box->y * CCD_REAL(0.5), + ccdSign(ccdVec3Z(&dir)) * box->z * CCD_REAL(0.5)); + + // transform support point according to position and rotation of object + ccdQuatRotVec(v, &obj->quat); + ccdVec3Add(v, &obj->pos); + } + + + int main(int argc, char *argv[]) + { + ... + + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.max_iterations = 100; // maximal number of iterations + + int intersect = ccdGJKIntersect(obj1, obj2, &ccd); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + } + + + + +2. GJK + EPA - Penetration Of Two Objects +------------------------------------------ + +If you want to obtain also penetration info about two intersection objects +``ccdGJKPenetration()`` function can be used. + +Procedure is almost the same as for the previous case: + +.. code-block:: c + + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.max_iterations = 100; // maximal number of iterations + ccd.epa_tolerance = 0.0001; // maximal tolerance fro EPA part + + ccd_real_t depth; + ccd_vec3_t dir, pos; + int intersect = ccdGJKPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + // in depth, dir and pos is stored penetration depth, direction of + // separation vector and position in global coordinate system + } + + +3. MPR - Intersection Test +--------------------------- + +**libccd** also provides *MPR* - Minkowski Portal Refinement algorithm that +can be used for testing if two objects intersects. + +Procedure is similar to the one used for GJK algorithm. Support function is +the same but also function that returns a center (or any point near center) +of a given object must be implemented: + +.. code-block:: c + + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + + /** Center function - returns center of object */ + void center(const void *_obj, ccd_vec3_t *center) + { + obj_t *obj = (obj_t *)_obj; + ccdVec3Copy(center, &obj->pos); + } + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.center1 = center; // center function for first object + ccd.center2 = center; // center function for second object + ccd.mpr_tolerance = 0.0001; // maximal tolerance + + int intersect = ccdMPRIntersect(obj1, obj2, &ccd); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + } + + +4. MPR - Penetration Of Two Objects +------------------------------------ + +Using MPR algorithm for obtaining penetration info about two intersection +objects is equally easy as in the previous case instead but +``ccdMPRPenetration()`` function is used: + +.. code-block:: c + + #include + #include // for work with quaternions + + /** Support function is same as in previous case */ + /** Center function is same as in prevous case */ + + int main(int argc, char *argv[]) + { + ... + ccd_t ccd; + CCD_INIT(&ccd); // initialize ccd_t struct + + // set up ccd_t struct + ccd.support1 = support; // support function for first object + ccd.support2 = support; // support function for second object + ccd.center1 = center; // center function for first object + ccd.center2 = center; // center function for second object + ccd.mpr_tolerance = 0.0001; // maximal tolerance + + ccd_real_t depth; + ccd_vec3_t dir, pos; + int intersect = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + // now intersect holds true if obj1 and obj2 intersect, false otherwise + // in depth, dir and pos is stored penetration depth, direction of + // separation vector and position in global coordinate system + } + diff --git a/3rdparty/libccd/doc/index.rst b/3rdparty/libccd/doc/index.rst new file mode 100644 index 0000000..9709251 --- /dev/null +++ b/3rdparty/libccd/doc/index.rst @@ -0,0 +1,25 @@ +.. libccd documentation master file, created by + sphinx-quickstart2 on Thu May 23 13:49:12 2013. + +libccd's documentation +======================= + +See homepage: http://libccd.danfis.cz + +Contents: + +.. toctree:: + :maxdepth: 2 + + compile-and-install.rst + examples.rst + reference.rst + + +.. Indices and tables +.. ================== + +.. * :ref:`genindex` +.. * :ref:`modindex` +.. * :ref:`search` + diff --git a/3rdparty/libccd/doc/reference.rst b/3rdparty/libccd/doc/reference.rst new file mode 100644 index 0000000..2d26cdd --- /dev/null +++ b/3rdparty/libccd/doc/reference.rst @@ -0,0 +1,6 @@ +Reference +========== + +.. literalinclude:: ../src/ccd/ccd.h + :language: c + :linenos: diff --git a/3rdparty/libccd/make-release.sh b/3rdparty/libccd/make-release.sh new file mode 100755 index 0000000..6be5fce --- /dev/null +++ b/3rdparty/libccd/make-release.sh @@ -0,0 +1,31 @@ +#!/bin/bash + +# Creates .tar.gz package of specified version. +# Takes one argument - identification of commit + +NAME=libccd +COMMIT="" +CMD="git archive" + +# read arguments +COMMIT="$1" + +if [ "$COMMIT" = "" ]; then + echo "Usage: $0 commit [--notest] [--nodoc]" + echo "Error: you must specify commit which should be packed" + exit -1; +fi; + + +PREFIX=${NAME}-$COMMIT/ +FN=${NAME}-$COMMIT.tar.gz + +if echo "$COMMIT" | grep '^v[0-9]\.[0-9]\+' >/dev/null 2>&1; then + tmp=$(echo "$COMMIT" | sed 's/^v//') + PREFIX=${NAME}-$tmp/ + FN=${NAME}-$tmp.tar.gz +fi + +$CMD --prefix="$PREFIX" --format=tar $COMMIT | gzip >"$FN" +echo "Package: $FN" + diff --git a/3rdparty/libccd/src/.gitignore b/3rdparty/libccd/src/.gitignore new file mode 100644 index 0000000..e3fa613 --- /dev/null +++ b/3rdparty/libccd/src/.gitignore @@ -0,0 +1,5 @@ +*.o +*.a +ccd/config.h +ccd/config.h.in + diff --git a/3rdparty/libccd/src/CMakeLists.txt b/3rdparty/libccd/src/CMakeLists.txt new file mode 100644 index 0000000..1407080 --- /dev/null +++ b/3rdparty/libccd/src/CMakeLists.txt @@ -0,0 +1,88 @@ +if(DEFINED CCD_SINGLE OR DEFINED CCD_DOUBLE) + # make sure only DOUBLE or SINGLE is set; default to SINGLE + if(CCD_SINGLE) + set(CCD_DOUBLE OFF) + else() + set(CCD_SINGLE ON) + endif() + if(CCD_DOUBLE) + set(CCD_SINGLE OFF) + endif() +elseif(ENABLE_DOUBLE_PRECISION) + set(CCD_DOUBLE ON) + set(CCD_SINGLE OFF) +else() + set(CCD_DOUBLE OFF) + set(CCD_SINGLE ON) +endif() + +configure_file(ccd/config.h.cmake.in ccd/config.h) + +set(CCD_INCLUDES + ccd/ccd.h + ccd/compiler.h + ccd/ccd_export.h + ccd/quat.h + ccd/vec3.h + "${CMAKE_CURRENT_BINARY_DIR}/ccd/config.h") + +set(CCD_SOURCES + alloc.h + ccd.c + dbg.h + list.h + mpr.c + polytope.c + polytope.h + simplex.h + support.c + support.h + vec3.c) + +add_library(ccd ${CCD_INCLUDES} ${CCD_SOURCES}) +set_target_properties(ccd PROPERTIES + PUBLIC_HEADER "${CCD_INCLUDES}" + SOVERSION ${CCD_SOVERSION} + VERSION ${CCD_VERSION}) +target_include_directories(ccd PUBLIC + $ + $) + +if(NOT WIN32) + find_library(LIBM_LIBRARY NAMES m) + if(NOT LIBM_LIBRARY) + message(FATAL_ERROR "Could NOT find required library LibM") + endif() + target_link_libraries(ccd "${LIBM_LIBRARY}") + if(BUILD_SHARED_LIBS) + set(CCD_PKGCONFIG_EXTRA_LIBS -lm PARENT_SCOPE) + endif() +endif() + +export(TARGETS ccd FILE "${CMAKE_BINARY_DIR}/ccd-targets.cmake") + +install(TARGETS ccd + EXPORT ccd-targets + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" + INCLUDES DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}" + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/ccd" + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}") +install(EXPORT ccd-targets DESTINATION "${CMAKE_INSTALL_LIBDIR}/ccd") + +macro (check_compiler_visibility) + include (CheckCXXCompilerFlag) + check_cxx_compiler_flag(-fvisibility=hidden COMPILER_SUPPORTS_VISIBILITY) +endmacro() + +if(UNIX) + check_compiler_visibility() + if (COMPILER_SUPPORTS_VISIBILITY) + set_target_properties(ccd + PROPERTIES COMPILE_FLAGS "-fvisibility=hidden") + endif() +endif() + +if(NOT WIN32 AND BUILD_TESTING AND NOT CCD_HIDE_ALL_SYMBOLS) + add_subdirectory(testsuites) +endif() diff --git a/3rdparty/libccd/src/Makefile b/3rdparty/libccd/src/Makefile new file mode 100644 index 0000000..b5b7b79 --- /dev/null +++ b/3rdparty/libccd/src/Makefile @@ -0,0 +1,80 @@ +### +# libccd +# --------------------------------- +# Copyright (c)2010 Daniel Fiser +# +# +# This file is part of libccd. +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file BDS-LICENSE for details or see +# . +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +## + +-include Makefile.include + +CFLAGS += -I. -fvisibility=hidden + +TARGETS = libccd.a +OBJS = ccd.o mpr.o support.o vec3.o polytope.o + +all: $(TARGETS) + +libccd.a: $(OBJS) + ar cr $@ $(OBJS) + ranlib $@ + +ccd/config.h: ccd/config.h.m4 + $(M4) $(CONFIG_FLAGS) $< >$@ + +%.o: %.c %.h ccd/config.h + $(CC) $(CFLAGS) $(DEFS) -c -o $@ $< +%.o: %.c ccd/config.h + $(CC) $(CFLAGS) $(DEFS) -c -o $@ $< +%.h: ccd/config.h +%.c: ccd/config.h + +install: + mkdir -p $(PREFIX)/$(INCLUDEDIR)/ccd + mkdir -p $(PREFIX)/$(LIBDIR) + cp ccd/*.h $(PREFIX)/$(INCLUDEDIR)/ccd/ + cp libccd.a $(PREFIX)/$(LIBDIR) + +clean: + rm -f $(OBJS) + rm -f $(TARGETS) + rm -f ccd/config.h + if [ -d testsuites ]; then $(MAKE) -C testsuites clean; fi; + +check: + $(MAKE) -C testsuites check +check-valgrind: + $(MAKE) -C testsuites check-valgrind + +help: + @echo "Targets:" + @echo " all - Build library" + @echo " install - Install library into system" + @echo "" + @echo "Options:" + @echo " CC - Path to C compiler" + @echo " M4 - Path to m4 macro processor" + @echo "" + @echo " DEBUG 'yes'/'no' - Turn on/off debugging (default: 'no')" + @echo " PROFIL 'yes'/'no' - Compiles profiling info (default: 'no')" + @echo " NOWALL 'yes'/'no' - Turns off -Wall gcc option (default: 'no')" + @echo " NOPEDANTIC 'yes'/'no' - Turns off -pedantic gcc option (default: 'no')" + @echo "" + @echo " USE_SINGLE 'yes' - Use single precision (default: 'no')" + @echo " USE_DOUBLE 'yes' - Use double precision (default: 'yes')" + @echo "" + @echo " PREFIX - Prefix where library will be installed (default: /usr/local)" + @echo " INCLUDEDIR - Directory where header files will be installed (PREFIX/INCLUDEDIR) (default: include)" + @echo " LIBDIR - Directory where library will be installed (PREFIX/LIBDIR) (default: lib)" + @echo "" + +.PHONY: all clean check check-valgrind help diff --git a/3rdparty/libccd/src/Makefile.am b/3rdparty/libccd/src/Makefile.am new file mode 100644 index 0000000..53b433a --- /dev/null +++ b/3rdparty/libccd/src/Makefile.am @@ -0,0 +1,18 @@ +SUBDIRS = . testsuites + +lib_LTLIBRARIES = libccd.la + +libccd_la_SOURCES = alloc.h \ + ccd/compiler.h \ + dbg.h \ + ccd.c ccd/ccd.h \ + ccd/ccd_export.h \ + list.h \ + polytope.c polytope.h \ + ccd/quat.h \ + simplex.h \ + support.c support.h \ + vec3.c ccd/vec3.h \ + mpr.c + +libccd_la_CFLAGS = -fvisibility=hidden diff --git a/3rdparty/libccd/src/Makefile.include b/3rdparty/libccd/src/Makefile.include new file mode 100644 index 0000000..ba48492 --- /dev/null +++ b/3rdparty/libccd/src/Makefile.include @@ -0,0 +1,100 @@ +### +# libccd +# --------------------------------- +# Copyright (c)2010 Daniel Fiser +# +# +# This file is part of libccd. +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file BDS-LICENSE for details or see +# . +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +## + +CC ?= gcc +M4 ?= m4 +PYTHON ?= python + +SYSTEM = $(shell uname) + +SYSTEM_CXXFLAGS = +SYSTEM_LDFLAGS = + +ifeq '$(SYSTEM)' 'FreeBSD' + SYSTEM_CXXFLAGS = -Wno-long-long +else +endif + +NOWALL ?= no +NOPEDANTIC ?= no +DEBUG ?= no +PROFIL ?= no + +ifeq '$(PROFIL)' 'yes' + DEBUG = yes +endif + +ifeq '$(DEBUG)' 'yes' + CFLAGS = -g +endif +ifeq '$(PROFIL)' 'yes' + CFLAGS += -pg +endif + +ifneq '$(NOWALL)' 'yes' + CFLAGS += -Wall +endif +ifneq '$(NOPEDANTIC)' 'yes' + CFLAGS += -pedantic +endif + +CONFIG_FLAGS = +USE_DOUBLE ?= yes +USE_SINGLE ?= no + +ifeq '$(USE_SINGLE)' 'yes' + CONFIG_FLAGS += -DUSE_SINGLE + USE_DOUBLE = no +endif +ifeq '$(USE_DOUBLE)' 'yes' + CONFIG_FLAGS += -DUSE_DOUBLE +endif + +CFLAGS += --std=gnu99 +LDFLAGS += $(SYSTEM_LDFLAGS) + +CHECKTARGETS = +check-dep: $(CHECKTARGETS) + +PREFIX ?= /usr/local +INCLUDEDIR ?= include +LIBDIR ?= lib + +showvars: + @echo "SYSTEM = "$(SYSTEM) + @echo "" + @echo "CC = $(CC)" + @echo "M4 = $(M4)" + @echo "" + @echo "DEBUG = $(DEBUG)" + @echo "PROFIL = $(PROFIL)" + @echo "NOWALL = $(NOWALL)" + @echo "NOPEDANTIC = $(NOPEDANTIC)" + @echo "USE_SINGLE = $(USE_SINGLE)" + @echo "USE_DOUBLE = $(USE_DOUBLE)" + @echo "" + @echo "CFLAGS = $(CFLAGS)" + @echo "LDFLAGS = $(LDFLAGS)" + @echo "CONFIG_FLAGS = $(CONFIG_FLAGS)" + @echo "" + @echo "PREFIX = $(PREFIX)" + @echo "INCLUDEDIR = $(INCLUDEDIR)" + @echo "LIBDIR = $(LIBDIR)" + @echo "" + +.DEFAULT_GOAL := all +.PHONY: showvars diff --git a/3rdparty/libccd/src/alloc.h b/3rdparty/libccd/src/alloc.h new file mode 100644 index 0000000..7cba3a4 --- /dev/null +++ b/3rdparty/libccd/src/alloc.h @@ -0,0 +1,50 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_ALLOC_H__ +#define __CCD_ALLOC_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/** + * Functions and macros required for memory allocation. + */ + +/* Memory allocation: */ +#define __CCD_ALLOC_MEMORY(type, ptr_old, size) \ + (type *)realloc((void *)ptr_old, (size)) + +/** Allocate memory for one element of type. */ +#define CCD_ALLOC(type) \ + __CCD_ALLOC_MEMORY(type, NULL, sizeof(type)) + +/** Allocate memory for array of elements of type type. */ +#define CCD_ALLOC_ARR(type, num_elements) \ + __CCD_ALLOC_MEMORY(type, NULL, sizeof(type) * (num_elements)) + +#define CCD_REALLOC_ARR(ptr, type, num_elements) \ + __CCD_ALLOC_MEMORY(type, ptr, sizeof(type) * (num_elements)) + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_ALLOC_H__ */ diff --git a/3rdparty/libccd/src/ccd.c b/3rdparty/libccd/src/ccd.c new file mode 100644 index 0000000..23b63c0 --- /dev/null +++ b/3rdparty/libccd/src/ccd.c @@ -0,0 +1,996 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2012 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include +#include +#include +#include +#include "simplex.h" +#include "polytope.h" +#include "alloc.h" +#include "dbg.h" + + +/** Performs GJK algorithm. Returns 0 if intersection was found and simplex + * is filled with resulting polytope. */ +static int __ccdGJK(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *simplex); + +/** Performs GJK+EPA algorithm. Returns 0 if intersection was found and + * pt is filled with resulting polytope and nearest with pointer to + * nearest element (vertex, edge, face) of polytope to origin. */ +static int __ccdGJKEPA(const void *obj1, const void *obj2, + const ccd_t *ccd, + ccd_pt_t *pt, ccd_pt_el_t **nearest); + + +/** Returns true if simplex contains origin. + * This function also alteres simplex and dir according to further + * processing of GJK algorithm. */ +static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir); +static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir); +static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir); +static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir); + +/** d = a x b x c */ +_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, + const ccd_vec3_t *c, ccd_vec3_t *d); + + +/** Transforms simplex to polytope. It is assumed that simplex has 4 + * vertices. */ +static int simplexToPolytope4(const void *obj1, const void *obj2, + const ccd_t *ccd, + ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest); + +/** Transforms simplex to polytope, three vertices required */ +static int simplexToPolytope3(const void *obj1, const void *obj2, + const ccd_t *ccd, + const ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest); + +/** Transforms simplex to polytope, two vertices required */ +static int simplexToPolytope2(const void *obj1, const void *obj2, + const ccd_t *ccd, + const ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest); + +/** Expands polytope using new vertex v. + * Return 0 on success, -2 on memory allocation failure.*/ +static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, + const ccd_support_t *newv); + +/** Finds next support point (at stores it in out argument). + * Returns 0 on success, -1 otherwise */ +static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd, + const ccd_pt_el_t *el, + ccd_support_t *out); + + + +void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir) +{ + ccdVec3Set(dir, CCD_ONE, CCD_ZERO, CCD_ZERO); +} + +int ccdGJKIntersect(const void *obj1, const void *obj2, const ccd_t *ccd) +{ + ccd_simplex_t simplex; + return __ccdGJK(obj1, obj2, ccd, &simplex) == 0; +} + +int ccdGJKSeparate(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_vec3_t *sep) +{ + ccd_pt_t polytope; + ccd_pt_el_t *nearest; + int ret; + + ccdPtInit(&polytope); + + ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest); + + // set separation vector + if (nearest) + ccdVec3Copy(sep, &nearest->witness); + + ccdPtDestroy(&polytope); + + return ret; +} + + +static int penEPAPosCmp(const void *a, const void *b) +{ + ccd_pt_vertex_t *v1, *v2; + v1 = *(ccd_pt_vertex_t **)a; + v2 = *(ccd_pt_vertex_t **)b; + + if (ccdEq(v1->dist, v2->dist)){ + return 0; + }else if (v1->dist < v2->dist){ + return -1; + }else{ + return 1; + } +} + +static int penEPAPos(const ccd_pt_t *pt, const ccd_pt_el_t *nearest, + ccd_vec3_t *pos) +{ + ccd_pt_vertex_t *v; + ccd_pt_vertex_t **vs; + size_t i, len; + ccd_real_t scale; + + // compute median + len = 0; + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + len++; + } + + vs = CCD_ALLOC_ARR(ccd_pt_vertex_t *, len); + if (vs == NULL) + return -1; + + i = 0; + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + vs[i++] = v; + } + + qsort(vs, len, sizeof(ccd_pt_vertex_t *), penEPAPosCmp); + + ccdVec3Set(pos, CCD_ZERO, CCD_ZERO, CCD_ZERO); + scale = CCD_ZERO; + if (len % 2 == 1) + len++; + + for (i = 0; i < len / 2; i++){ + ccdVec3Add(pos, &vs[i]->v.v1); + ccdVec3Add(pos, &vs[i]->v.v2); + scale += CCD_REAL(2.); + } + ccdVec3Scale(pos, CCD_ONE / scale); + + free(vs); + + return 0; +} + +int ccdGJKPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) +{ + ccd_pt_t polytope; + ccd_pt_el_t *nearest; + int ret; + + ccdPtInit(&polytope); + + ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest); + + // set separation vector + if (ret == 0 && nearest){ + // compute depth of penetration + *depth = CCD_SQRT(nearest->dist); + + // store normalized direction vector + ccdVec3Copy(dir, &nearest->witness); + ccdVec3Normalize(dir); + + // compute position + if (penEPAPos(&polytope, nearest, pos) != 0){ + ccdPtDestroy(&polytope); + return -2; + } + } + + ccdPtDestroy(&polytope); + + return ret; +} + + +static int __ccdGJK(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *simplex) +{ + unsigned long iterations; + ccd_vec3_t dir; // direction vector + ccd_support_t last; // last support point + int do_simplex_res; + + // initialize simplex struct + ccdSimplexInit(simplex); + + // get first direction + ccd->first_dir(obj1, obj2, &dir); + // get first support point + __ccdSupport(obj1, obj2, &dir, ccd, &last); + // and add this point to simplex as last one + ccdSimplexAdd(simplex, &last); + + // set up direction vector to as (O - last) which is exactly -last + ccdVec3Copy(&dir, &last.v); + ccdVec3Scale(&dir, -CCD_ONE); + + // start iterations + for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) { + // obtain support point + __ccdSupport(obj1, obj2, &dir, ccd, &last); + + // check if farthest point in Minkowski difference in direction dir + // isn't somewhere before origin (the test on negative dot product) + // - because if it is, objects are not intersecting at all. + if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){ + return -1; // intersection not found + } + + // add last support vector to simplex + ccdSimplexAdd(simplex, &last); + + // if doSimplex returns 1 if objects intersect, -1 if objects don't + // intersect and 0 if algorithm should continue + do_simplex_res = doSimplex(simplex, &dir); + if (do_simplex_res == 1){ + return 0; // intersection found + }else if (do_simplex_res == -1){ + return -1; // intersection not found + } + + if (ccdIsZero(ccdVec3Len2(&dir))){ + return -1; // intersection not found + } + } + + // intersection wasn't found + return -1; +} + +static int __ccdGJKEPA(const void *obj1, const void *obj2, + const ccd_t *ccd, + ccd_pt_t *polytope, ccd_pt_el_t **nearest) +{ + ccd_simplex_t simplex; + ccd_support_t supp; // support point + int ret, size; + + *nearest = NULL; + + // run GJK and obtain terminal simplex + ret = __ccdGJK(obj1, obj2, ccd, &simplex); + if (ret != 0) + return -1; + + // transform simplex to polytope - simplex won't be used anymore + size = ccdSimplexSize(&simplex); + if (size == 4){ + ret = simplexToPolytope4(obj1, obj2, ccd, &simplex, polytope, nearest); + }else if (size == 3){ + ret = simplexToPolytope3(obj1, obj2, ccd, &simplex, polytope, nearest); + }else{ // size == 2 + ret = simplexToPolytope2(obj1, obj2, ccd, &simplex, polytope, nearest); + } + + if (ret == -1){ + // touching contact + return 0; + }else if (ret == -2){ + // failed memory allocation + return -2; + } + + + while (1){ + // get triangle nearest to origin + *nearest = ccdPtNearest(polytope); + + // get next support point + if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0) + break; + + // expand nearest triangle using new point - supp + if (expandPolytope(polytope, *nearest, &supp) != 0) + return -2; + } + + return 0; +} + + + +static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) +{ + const ccd_support_t *A, *B; + ccd_vec3_t AB, AO, tmp; + ccd_real_t dot; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other point + B = ccdSimplexPoint(simplex, 0); + // compute AB oriented segment + ccdVec3Sub2(&AB, &B->v, &A->v); + // compute AO vector + ccdVec3Copy(&AO, &A->v); + ccdVec3Scale(&AO, -CCD_ONE); + + // dot product AB . AO + dot = ccdVec3Dot(&AB, &AO); + + // check if origin doesn't lie on AB segment + ccdVec3Cross(&tmp, &AB, &AO); + if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){ + return 1; + } + + // check if origin is in area where AB segment is + if (ccdIsZero(dot) || dot < CCD_ZERO){ + // origin is in outside are of A + ccdSimplexSet(simplex, 0, A); + ccdSimplexSetSize(simplex, 1); + ccdVec3Copy(dir, &AO); + }else{ + // origin is in area where AB segment is + + // keep simplex untouched and set direction to + // AB x AO x AB + tripleCross(&AB, &AO, &AB, dir); + } + + return 0; +} + +static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) +{ + const ccd_support_t *A, *B, *C; + ccd_vec3_t AO, AB, AC, ABC, tmp; + ccd_real_t dot, dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = ccdSimplexPoint(simplex, 1); + C = ccdSimplexPoint(simplex, 0); + + // check touching contact + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL); + if (ccdIsZero(dist)){ + return 1; + } + + // check if triangle is really triangle (has area > 0) + // if not simplex can't be expanded and thus no itersection is found + if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){ + return -1; + } + + // compute AO vector + ccdVec3Copy(&AO, &A->v); + ccdVec3Scale(&AO, -CCD_ONE); + + // compute AB and AC segments and ABC vector (perpendircular to triangle) + ccdVec3Sub2(&AB, &B->v, &A->v); + ccdVec3Sub2(&AC, &C->v, &A->v); + ccdVec3Cross(&ABC, &AB, &AC); + + ccdVec3Cross(&tmp, &ABC, &AC); + dot = ccdVec3Dot(&tmp, &AO); + if (ccdIsZero(dot) || dot > CCD_ZERO){ + dot = ccdVec3Dot(&AC, &AO); + if (ccdIsZero(dot) || dot > CCD_ZERO){ + // C is already in place + ccdSimplexSet(simplex, 1, A); + ccdSimplexSetSize(simplex, 2); + tripleCross(&AC, &AO, &AC, dir); + }else{ +ccd_do_simplex3_45: + dot = ccdVec3Dot(&AB, &AO); + if (ccdIsZero(dot) || dot > CCD_ZERO){ + ccdSimplexSet(simplex, 0, B); + ccdSimplexSet(simplex, 1, A); + ccdSimplexSetSize(simplex, 2); + tripleCross(&AB, &AO, &AB, dir); + }else{ + ccdSimplexSet(simplex, 0, A); + ccdSimplexSetSize(simplex, 1); + ccdVec3Copy(dir, &AO); + } + } + }else{ + ccdVec3Cross(&tmp, &AB, &ABC); + dot = ccdVec3Dot(&tmp, &AO); + if (ccdIsZero(dot) || dot > CCD_ZERO){ + goto ccd_do_simplex3_45; + }else{ + dot = ccdVec3Dot(&ABC, &AO); + if (ccdIsZero(dot) || dot > CCD_ZERO){ + ccdVec3Copy(dir, &ABC); + }else{ + ccd_support_t Ctmp; + ccdSupportCopy(&Ctmp, C); + ccdSimplexSet(simplex, 0, B); + ccdSimplexSet(simplex, 1, &Ctmp); + + ccdVec3Copy(dir, &ABC); + ccdVec3Scale(dir, -CCD_ONE); + } + } + } + + return 0; +} + +static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) +{ + const ccd_support_t *A, *B, *C, *D; + ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB; + int B_on_ACD, C_on_ADB, D_on_ABC; + int AB_O, AC_O, AD_O; + ccd_real_t dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = ccdSimplexPoint(simplex, 2); + C = ccdSimplexPoint(simplex, 1); + D = ccdSimplexPoint(simplex, 0); + + // check if tetrahedron is really tetrahedron (has volume > 0) + // if it is not simplex can't be expanded and thus no intersection is + // found + dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL); + if (ccdIsZero(dist)){ + return -1; + } + + // check if origin lies on some of tetrahedron's face - if so objects + // intersect + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL); + if (ccdIsZero(dist)) + return 1; + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL); + if (ccdIsZero(dist)) + return 1; + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL); + if (ccdIsZero(dist)) + return 1; + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL); + if (ccdIsZero(dist)) + return 1; + + // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors + ccdVec3Copy(&AO, &A->v); + ccdVec3Scale(&AO, -CCD_ONE); + ccdVec3Sub2(&AB, &B->v, &A->v); + ccdVec3Sub2(&AC, &C->v, &A->v); + ccdVec3Sub2(&AD, &D->v, &A->v); + ccdVec3Cross(&ABC, &AB, &AC); + ccdVec3Cross(&ACD, &AC, &AD); + ccdVec3Cross(&ADB, &AD, &AB); + + // side (positive or negative) of B, C, D relative to planes ACD, ADB + // and ABC respectively + B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB)); + C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC)); + D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD)); + + // whether origin is on same side of ACD, ADB, ABC as B, C, D + // respectively + AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD; + AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB; + AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC; + + if (AB_O && AC_O && AD_O){ + // origin is in tetrahedron + return 1; + + // rearrange simplex to triangle and call doSimplex3() + }else if (!AB_O){ + // B is farthest from the origin among all of the tetrahedron's + // points, so remove it from the list and go on with the triangle + // case + + // D and C are in place + ccdSimplexSet(simplex, 2, A); + ccdSimplexSetSize(simplex, 3); + }else if (!AC_O){ + // C is farthest + ccdSimplexSet(simplex, 1, D); + ccdSimplexSet(simplex, 0, B); + ccdSimplexSet(simplex, 2, A); + ccdSimplexSetSize(simplex, 3); + }else{ // (!AD_O) + ccdSimplexSet(simplex, 0, C); + ccdSimplexSet(simplex, 1, B); + ccdSimplexSet(simplex, 2, A); + ccdSimplexSetSize(simplex, 3); + } + + return doSimplex3(simplex, dir); +} + +static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir) +{ + if (ccdSimplexSize(simplex) == 2){ + // simplex contains segment only one segment + return doSimplex2(simplex, dir); + }else if (ccdSimplexSize(simplex) == 3){ + // simplex contains triangle + return doSimplex3(simplex, dir); + }else{ // ccdSimplexSize(simplex) == 4 + // tetrahedron - this is the only shape which can encapsule origin + // so doSimplex4() also contains test on it + return doSimplex4(simplex, dir); + } +} + + +_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, + const ccd_vec3_t *c, ccd_vec3_t *d) +{ + ccd_vec3_t e; + ccdVec3Cross(&e, a, b); + ccdVec3Cross(d, &e, c); +} + + + +/** Transforms simplex to polytope. It is assumed that simplex has 4 + * vertices! */ +static int simplexToPolytope4(const void *obj1, const void *obj2, + const ccd_t *ccd, + ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest) +{ + const ccd_support_t *a, *b, *c, *d; + int use_polytope3; + ccd_real_t dist; + ccd_pt_vertex_t *v[4]; + ccd_pt_edge_t *e[6]; + size_t i; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + c = ccdSimplexPoint(simplex, 2); + d = ccdSimplexPoint(simplex, 3); + + // check if origin lies on some of tetrahedron's face - if so use + // simplexToPolytope3() + use_polytope3 = 0; + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL); + if (ccdIsZero(dist)){ + use_polytope3 = 1; + } + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL); + if (ccdIsZero(dist)){ + use_polytope3 = 1; + ccdSimplexSet(simplex, 1, c); + ccdSimplexSet(simplex, 2, d); + } + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &d->v, NULL); + if (ccdIsZero(dist)){ + use_polytope3 = 1; + ccdSimplexSet(simplex, 2, d); + } + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &b->v, &c->v, &d->v, NULL); + if (ccdIsZero(dist)){ + use_polytope3 = 1; + ccdSimplexSet(simplex, 0, b); + ccdSimplexSet(simplex, 1, c); + ccdSimplexSet(simplex, 2, d); + } + + if (use_polytope3){ + ccdSimplexSetSize(simplex, 3); + return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest); + } + + // no touching contact - simply create tetrahedron + for (i = 0; i < 4; i++){ + v[i] = ccdPtAddVertex(pt, ccdSimplexPoint(simplex, i)); + } + + e[0] = ccdPtAddEdge(pt, v[0], v[1]); + e[1] = ccdPtAddEdge(pt, v[1], v[2]); + e[2] = ccdPtAddEdge(pt, v[2], v[0]); + e[3] = ccdPtAddEdge(pt, v[3], v[0]); + e[4] = ccdPtAddEdge(pt, v[3], v[1]); + e[5] = ccdPtAddEdge(pt, v[3], v[2]); + + // ccdPtAdd*() functions return NULL either if the memory allocation + // failed of if any of the input pointers are NULL, so the bad + // allocation can be checked by the last calls of ccdPtAddFace() + // because the rest of the bad allocations eventually "bubble up" here + if (ccdPtAddFace(pt, e[0], e[1], e[2]) == NULL + || ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL + || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL + || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){ + return -2; + } + + return 0; +} + +/** Transforms simplex to polytope, three vertices required */ +static int simplexToPolytope3(const void *obj1, const void *obj2, + const ccd_t *ccd, + const ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest) +{ + const ccd_support_t *a, *b, *c; + ccd_support_t d, d2; + ccd_vec3_t ab, ac, dir; + ccd_pt_vertex_t *v[5]; + ccd_pt_edge_t *e[9]; + ccd_real_t dist, dist2; + + *nearest = NULL; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + c = ccdSimplexPoint(simplex, 2); + + // If only one triangle left from previous GJK run origin lies on this + // triangle. So it is necessary to expand triangle into two + // tetrahedrons connected with base (which is exactly abc triangle). + + // get next support point in direction of normal of triangle + ccdVec3Sub2(&ab, &b->v, &a->v); + ccdVec3Sub2(&ac, &c->v, &a->v); + ccdVec3Cross(&dir, &ab, &ac); + __ccdSupport(obj1, obj2, &dir, ccd, &d); + dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); + + // and second one take in opposite direction + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &d2); + dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); + + // check if face isn't already on edge of minkowski sum and thus we + // have touching contact + if (ccdIsZero(dist) || ccdIsZero(dist2)){ + v[0] = ccdPtAddVertex(pt, a); + v[1] = ccdPtAddVertex(pt, b); + v[2] = ccdPtAddVertex(pt, c); + e[0] = ccdPtAddEdge(pt, v[0], v[1]); + e[1] = ccdPtAddEdge(pt, v[1], v[2]); + e[2] = ccdPtAddEdge(pt, v[2], v[0]); + *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]); + if (*nearest == NULL) + return -2; + + return -1; + } + + // form polyhedron + v[0] = ccdPtAddVertex(pt, a); + v[1] = ccdPtAddVertex(pt, b); + v[2] = ccdPtAddVertex(pt, c); + v[3] = ccdPtAddVertex(pt, &d); + v[4] = ccdPtAddVertex(pt, &d2); + + e[0] = ccdPtAddEdge(pt, v[0], v[1]); + e[1] = ccdPtAddEdge(pt, v[1], v[2]); + e[2] = ccdPtAddEdge(pt, v[2], v[0]); + + e[3] = ccdPtAddEdge(pt, v[3], v[0]); + e[4] = ccdPtAddEdge(pt, v[3], v[1]); + e[5] = ccdPtAddEdge(pt, v[3], v[2]); + + e[6] = ccdPtAddEdge(pt, v[4], v[0]); + e[7] = ccdPtAddEdge(pt, v[4], v[1]); + e[8] = ccdPtAddEdge(pt, v[4], v[2]); + + if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL + || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL + || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL + + || ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL + || ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL + || ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){ + return -2; + } + + return 0; +} + +/** Transforms simplex to polytope, two vertices required */ +static int simplexToPolytope2(const void *obj1, const void *obj2, + const ccd_t *ccd, + const ccd_simplex_t *simplex, + ccd_pt_t *pt, ccd_pt_el_t **nearest) +{ + const ccd_support_t *a, *b; + ccd_vec3_t ab, ac, dir; + ccd_support_t supp[4]; + ccd_pt_vertex_t *v[6]; + ccd_pt_edge_t *e[12]; + size_t i; + int found; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + + // This situation is a bit tricky. If only one segment comes from + // previous run of GJK - it means that either this segment is on + // minkowski edge (and thus we have touch contact) or it it isn't and + // therefore segment is somewhere *inside* minkowski sum and it *must* + // be possible to fully enclose this segment with polyhedron formed by + // at least 8 triangle faces. + + // get first support point (any) + found = 0; + for (i = 0; i < ccd_points_on_sphere_len; i++){ + __ccdSupport(obj1, obj2, &ccd_points_on_sphere[i], ccd, &supp[0]); + if (!ccdVec3Eq(&a->v, &supp[0].v) && !ccdVec3Eq(&b->v, &supp[0].v)){ + found = 1; + break; + } + } + if (!found) + goto simplexToPolytope2_touching_contact; + + // get second support point in opposite direction than supp[0] + ccdVec3Copy(&dir, &supp[0].v); + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &supp[1]); + if (ccdVec3Eq(&a->v, &supp[1].v) || ccdVec3Eq(&b->v, &supp[1].v)) + goto simplexToPolytope2_touching_contact; + + // next will be in direction of normal of triangle a,supp[0],supp[1] + ccdVec3Sub2(&ab, &supp[0].v, &a->v); + ccdVec3Sub2(&ac, &supp[1].v, &a->v); + ccdVec3Cross(&dir, &ab, &ac); + __ccdSupport(obj1, obj2, &dir, ccd, &supp[2]); + if (ccdVec3Eq(&a->v, &supp[2].v) || ccdVec3Eq(&b->v, &supp[2].v)) + goto simplexToPolytope2_touching_contact; + + // and last one will be in opposite direction + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &supp[3]); + if (ccdVec3Eq(&a->v, &supp[3].v) || ccdVec3Eq(&b->v, &supp[3].v)) + goto simplexToPolytope2_touching_contact; + + goto simplexToPolytope2_not_touching_contact; +simplexToPolytope2_touching_contact: + v[0] = ccdPtAddVertex(pt, a); + v[1] = ccdPtAddVertex(pt, b); + *nearest = (ccd_pt_el_t *)ccdPtAddEdge(pt, v[0], v[1]); + if (*nearest == NULL) + return -2; + + return -1; + +simplexToPolytope2_not_touching_contact: + // form polyhedron + v[0] = ccdPtAddVertex(pt, a); + v[1] = ccdPtAddVertex(pt, &supp[0]); + v[2] = ccdPtAddVertex(pt, b); + v[3] = ccdPtAddVertex(pt, &supp[1]); + v[4] = ccdPtAddVertex(pt, &supp[2]); + v[5] = ccdPtAddVertex(pt, &supp[3]); + + e[0] = ccdPtAddEdge(pt, v[0], v[1]); + e[1] = ccdPtAddEdge(pt, v[1], v[2]); + e[2] = ccdPtAddEdge(pt, v[2], v[3]); + e[3] = ccdPtAddEdge(pt, v[3], v[0]); + + e[4] = ccdPtAddEdge(pt, v[4], v[0]); + e[5] = ccdPtAddEdge(pt, v[4], v[1]); + e[6] = ccdPtAddEdge(pt, v[4], v[2]); + e[7] = ccdPtAddEdge(pt, v[4], v[3]); + + e[8] = ccdPtAddEdge(pt, v[5], v[0]); + e[9] = ccdPtAddEdge(pt, v[5], v[1]); + e[10] = ccdPtAddEdge(pt, v[5], v[2]); + e[11] = ccdPtAddEdge(pt, v[5], v[3]); + + if (ccdPtAddFace(pt, e[4], e[5], e[0]) == NULL + || ccdPtAddFace(pt, e[5], e[6], e[1]) == NULL + || ccdPtAddFace(pt, e[6], e[7], e[2]) == NULL + || ccdPtAddFace(pt, e[7], e[4], e[3]) == NULL + + || ccdPtAddFace(pt, e[8], e[9], e[0]) == NULL + || ccdPtAddFace(pt, e[9], e[10], e[1]) == NULL + || ccdPtAddFace(pt, e[10], e[11], e[2]) == NULL + || ccdPtAddFace(pt, e[11], e[8], e[3]) == NULL){ + return -2; + } + + return 0; +} + +/** Expands polytope's tri by new vertex v. Triangle tri is replaced by + * three triangles each with one vertex in v. */ +static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, + const ccd_support_t *newv) +{ + ccd_pt_vertex_t *v[5]; + ccd_pt_edge_t *e[8]; + ccd_pt_face_t *f[2]; + + + // element can be either segment or triangle + if (el->type == CCD_PT_EDGE){ + // In this case, segment should be replaced by new point. + // Simpliest case is when segment stands alone and in this case + // this segment is replaced by two other segments both connected to + // newv. + // Segment can be also connected to max two faces and in that case + // each face must be replaced by two other faces. To do this + // correctly it is necessary to have correctly ordered edges and + // vertices which is exactly what is done in following code. + // + + ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]); + + ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]); + + if (f[0]){ + ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]); + if (e[0] == (ccd_pt_edge_t *)el){ + e[0] = e[2]; + }else if (e[1] == (ccd_pt_edge_t *)el){ + e[1] = e[2]; + } + ccdPtEdgeVertices(e[0], &v[1], &v[3]); + if (v[1] != v[0] && v[3] != v[0]){ + e[2] = e[0]; + e[0] = e[1]; + e[1] = e[2]; + if (v[1] == v[2]) + v[1] = v[3]; + }else{ + if (v[1] == v[0]) + v[1] = v[3]; + } + + if (f[1]){ + ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]); + if (e[2] == (ccd_pt_edge_t *)el){ + e[2] = e[4]; + }else if (e[3] == (ccd_pt_edge_t *)el){ + e[3] = e[4]; + } + ccdPtEdgeVertices(e[2], &v[3], &v[4]); + if (v[3] != v[2] && v[4] != v[2]){ + e[4] = e[2]; + e[2] = e[3]; + e[3] = e[4]; + if (v[3] == v[0]) + v[3] = v[4]; + }else{ + if (v[3] == v[2]) + v[3] = v[4]; + } + } + + + v[4] = ccdPtAddVertex(pt, newv); + + ccdPtDelFace(pt, f[0]); + if (f[1]){ + ccdPtDelFace(pt, f[1]); + ccdPtDelEdge(pt, (ccd_pt_edge_t *)el); + } + + e[4] = ccdPtAddEdge(pt, v[4], v[2]); + e[5] = ccdPtAddEdge(pt, v[4], v[0]); + e[6] = ccdPtAddEdge(pt, v[4], v[1]); + if (f[1]) + e[7] = ccdPtAddEdge(pt, v[4], v[3]); + + + if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL + || ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){ + return -2; + } + + if (f[1]){ + if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL + || ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){ + return -2; + } + }else{ + if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL) + return -2; + } + } + }else{ // el->type == CCD_PT_FACE + // replace triangle by tetrahedron without base (base would be the + // triangle that will be removed) + + // get triplet of surrounding edges and vertices of triangle face + ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]); + ccdPtEdgeVertices(e[0], &v[0], &v[1]); + ccdPtEdgeVertices(e[1], &v[2], &v[3]); + + // following code sorts edges to have e[0] between vertices 0-1, + // e[1] between 1-2 and e[2] between 2-0 + if (v[2] != v[1] && v[3] != v[1]){ + // swap e[1] and e[2] + e[3] = e[1]; + e[1] = e[2]; + e[2] = e[3]; + } + if (v[3] != v[0] && v[3] != v[1]) + v[2] = v[3]; + + // remove triangle face + ccdPtDelFace(pt, (ccd_pt_face_t *)el); + + // expand triangle to tetrahedron + v[3] = ccdPtAddVertex(pt, newv); + e[3] = ccdPtAddEdge(pt, v[3], v[0]); + e[4] = ccdPtAddEdge(pt, v[3], v[1]); + e[5] = ccdPtAddEdge(pt, v[3], v[2]); + + if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL + || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL + || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){ + return -2; + } + } + + return 0; +} + +/** Finds next support point (and stores it in out argument). + * Returns 0 on success, -1 otherwise */ +static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd, + const ccd_pt_el_t *el, + ccd_support_t *out) +{ + ccd_vec3_t *a, *b, *c; + ccd_real_t dist; + + if (el->type == CCD_PT_VERTEX) + return -1; + + // touch contact + if (ccdIsZero(el->dist)) + return -1; + + __ccdSupport(obj1, obj2, &el->witness, ccd, out); + + // Compute dist of support point along element witness point direction + // so we can determine whether we expanded a polytope surrounding the + // origin a bit. + dist = ccdVec3Dot(&out->v, &el->witness); + + if (dist - el->dist < ccd->epa_tolerance) + return -1; + + if (el->type == CCD_PT_EDGE){ + // fetch end points of edge + ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b); + + // get distance from segment + dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); + }else{ // el->type == CCD_PT_FACE + // fetch vertices of triangle face + ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c); + + // check if new point can significantly expand polytope + dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); + } + + if (dist < ccd->epa_tolerance) + return -1; + + return 0; +} diff --git a/3rdparty/libccd/src/ccd/ccd.h b/3rdparty/libccd/src/ccd/ccd.h new file mode 100644 index 0000000..cec809e --- /dev/null +++ b/3rdparty/libccd/src/ccd/ccd.h @@ -0,0 +1,154 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010,2011 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_H__ +#define __CCD_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/** + * Type of *support* function that takes pointer to 3D object and direction + * and returns (via vec argument) furthest point from object in specified + * direction. + */ +typedef void (*ccd_support_fn)(const void *obj, const ccd_vec3_t *dir, + ccd_vec3_t *vec); + +/** + * Returns (via dir argument) first direction vector that will be used in + * initialization of algorithm. + */ +typedef void (*ccd_first_dir_fn)(const void *obj1, const void *obj2, + ccd_vec3_t *dir); + + +/** + * Returns (via center argument) geometric center (some point near center) + * of given object. + */ +typedef void (*ccd_center_fn)(const void *obj1, ccd_vec3_t *center); + +/** + * Main structure of CCD algorithm. + */ +struct _ccd_t { + ccd_first_dir_fn first_dir; //!< Returns initial direction where first + //!< support point will be searched + ccd_support_fn support1; //!< Function that returns support point of + //!< first object + ccd_support_fn support2; //!< Function that returns support point of + //!< second object + + ccd_center_fn center1; //!< Function that returns geometric center of + //!< first object + ccd_center_fn center2; //!< Function that returns geometric center of + //!< second object + + unsigned long max_iterations; //!< Maximal number of iterations + ccd_real_t epa_tolerance; + ccd_real_t mpr_tolerance; //!< Boundary tolerance for MPR algorithm + ccd_real_t dist_tolerance; +}; +typedef struct _ccd_t ccd_t; + +/** + * Default first direction. + */ +CCD_EXPORT void ccdFirstDirDefault(const void *o1, const void *o2, + ccd_vec3_t *dir); + +#define CCD_INIT(ccd) \ + do { \ + (ccd)->first_dir = ccdFirstDirDefault; \ + (ccd)->support1 = NULL; \ + (ccd)->support2 = NULL; \ + (ccd)->center1 = NULL; \ + (ccd)->center2 = NULL; \ + \ + (ccd)->max_iterations = (unsigned long)-1; \ + (ccd)->epa_tolerance = CCD_REAL(0.0001); \ + (ccd)->mpr_tolerance = CCD_REAL(0.0001); \ + (ccd)->dist_tolerance = CCD_REAL(1E-6); \ + } while(0) + + +/** + * Returns true if two given objects interest. + */ +CCD_EXPORT int ccdGJKIntersect(const void *obj1, const void *obj2, + const ccd_t *ccd); + +/** + * This function computes separation vector of two objects. Separation + * vector is minimal translation of obj2 to get obj1 and obj2 speparated + * (without intersection). + * Returns 0 if obj1 and obj2 intersect and sep is filled with translation + * vector. If obj1 and obj2 don't intersect -1 is returned. + * If memory allocation fails -2 is returned. + */ +CCD_EXPORT int ccdGJKSeparate(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_vec3_t *sep); + +/** + * Computes penetration of obj2 into obj1. + * Depth of penetration, direction and position is returned. It means that + * if obj2 is translated by distance depth in direction dir objects will + * have touching contact, pos should be position in global coordinates + * where force should take a place. + * + * CCD+EPA algorithm is used. + * + * Returns 0 if obj1 and obj2 intersect and depth, dir and pos are filled + * if given non-NULL pointers. + * If obj1 and obj2 don't intersect -1 is returned. + * If memory allocation fails -2 is returned. + */ +CCD_EXPORT int ccdGJKPenetration(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_real_t *depth, + ccd_vec3_t *dir, ccd_vec3_t *pos); + +/** + * Returns true if two given objects intersect - MPR algorithm is used. + */ +CCD_EXPORT int ccdMPRIntersect(const void *obj1, const void *obj2, + const ccd_t *ccd); + +/** + * Computes penetration of obj2 into obj1. + * Depth of penetration, direction and position is returned, i.e. if obj2 + * is translated by computed depth in resulting direction obj1 and obj2 + * would have touching contact. Position is point in global coordinates + * where force should take a place. + * + * Minkowski Portal Refinement algorithm is used (MPR, a.k.a. XenoCollide, + * see Game Programming Gem 7). + * + * Returns 0 if obj1 and obj2 intersect, otherwise -1 is returned. + */ +CCD_EXPORT int ccdMPRPenetration(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_real_t *depth, + ccd_vec3_t *dir, ccd_vec3_t *pos); + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_H__ */ diff --git a/3rdparty/libccd/src/ccd/ccd_export.h b/3rdparty/libccd/src/ccd/ccd_export.h new file mode 100644 index 0000000..e898d41 --- /dev/null +++ b/3rdparty/libccd/src/ccd/ccd_export.h @@ -0,0 +1,26 @@ +#ifndef CCD_EXPORT_H +#define CCD_EXPORT_H + +#ifdef CCD_STATIC_DEFINE +# define CCD_EXPORT +#else +# ifdef _MSC_VER +# ifdef ccd_EXPORTS +# define CCD_EXPORT __declspec(dllexport) +# else /* ccd_EXPORTS */ +# define CCD_EXPORT __declspec(dllimport) +# endif /* ccd_EXPORTS */ +# else +# ifndef CCD_EXPORT +# ifdef ccd_EXPORTS + /* We are building this library */ +# define CCD_EXPORT __attribute__((visibility("default"))) +# else + /* We are using this library */ +# define CCD_EXPORT __attribute__((visibility("default"))) +# endif +# endif +# endif +#endif + +#endif diff --git a/3rdparty/libccd/src/ccd/compiler.h b/3rdparty/libccd/src/ccd/compiler.h new file mode 100644 index 0000000..701f8a5 --- /dev/null +++ b/3rdparty/libccd/src/ccd/compiler.h @@ -0,0 +1,64 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_COMPILER_H__ +#define __CCD_COMPILER_H__ + +#include + +#define ccd_offsetof(TYPE, MEMBER) offsetof(TYPE, MEMBER) + +#define ccd_container_of(ptr, type, member) \ + (type *)( (char *)ptr - ccd_offsetof(type, member)) + +/** + * Marks inline function. + */ +#ifdef __GNUC__ +# define _ccd_inline static inline __attribute__((always_inline)) +#else /* __GNUC__ */ +# define _ccd_inline static __inline +#endif /* __GNUC__ */ + + +/** + * __prefetch(x) - prefetches the cacheline at "x" for read + * __prefetchw(x) - prefetches the cacheline at "x" for write + */ +#ifdef __GNUC__ +# define _ccd_prefetch(x) __builtin_prefetch(x) +# define _ccd_prefetchw(x) __builtin_prefetch(x,1) +#else /* __GNUC__ */ +# define _ccd_prefetch(x) ((void)0) +# define _ccd_prefetchw(x) ((void)0) +#endif /* __GNUC__ */ + + +#ifdef __ICC +// disable unused parameter warning +# pragma warning(disable:869) +// disable annoying "operands are evaluated in unspecified order" warning +# pragma warning(disable:981) +#endif /* __ICC */ + +#ifdef _MSC_VER +// disable unsafe function warning +# define _CRT_SECURE_NO_WARNINGS +#endif /* _MSC_VER */ + +#endif /* __CCD_COMPILER_H__ */ + diff --git a/3rdparty/libccd/src/ccd/config.h.cmake.in b/3rdparty/libccd/src/ccd/config.h.cmake.in new file mode 100644 index 0000000..0515009 --- /dev/null +++ b/3rdparty/libccd/src/ccd/config.h.cmake.in @@ -0,0 +1,7 @@ +#ifndef __CCD_CONFIG_H__ +#define __CCD_CONFIG_H__ + +#cmakedefine CCD_SINGLE +#cmakedefine CCD_DOUBLE + +#endif /* __CCD_CONFIG_H__ */ diff --git a/3rdparty/libccd/src/ccd/config.h.m4 b/3rdparty/libccd/src/ccd/config.h.m4 new file mode 100644 index 0000000..4c11fba --- /dev/null +++ b/3rdparty/libccd/src/ccd/config.h.m4 @@ -0,0 +1,7 @@ +#ifndef __CCD_CONFIG_H__ +#define __CCD_CONFIG_H__ + +ifdef(`USE_SINGLE', `#define CCD_SINGLE') +ifdef(`USE_DOUBLE', `#define CCD_DOUBLE') + +#endif /* __CCD_CONFIG_H__ */ diff --git a/3rdparty/libccd/src/ccd/quat.h b/3rdparty/libccd/src/ccd/quat.h new file mode 100644 index 0000000..d167220 --- /dev/null +++ b/3rdparty/libccd/src/ccd/quat.h @@ -0,0 +1,231 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_QUAT_H__ +#define __CCD_QUAT_H__ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_quat_t { + ccd_real_t q[4]; //!< x, y, z, w +}; +typedef struct _ccd_quat_t ccd_quat_t; + +#define CCD_QUAT(name, x, y, z, w) \ + ccd_quat_t name = { {x, y, z, w} } + +_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q); +_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q); + +_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w); +_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src); + +_ccd_inline int ccdQuatNormalize(ccd_quat_t *q); + +_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q, + ccd_real_t angle, const ccd_vec3_t *axis); + +_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k); + +/** + * q = q * q2 + */ +_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2); + +/** + * q = a * b + */ +_ccd_inline void ccdQuatMul2(ccd_quat_t *q, + const ccd_quat_t *a, const ccd_quat_t *b); + +/** + * Inverts quaternion. + * Returns 0 on success. + */ +_ccd_inline int ccdQuatInvert(ccd_quat_t *q); +_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src); + + +/** + * Rotate vector v by quaternion q. + */ +_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q); + + +/**** INLINES ****/ +_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q) +{ + ccd_real_t len; + + len = q->q[0] * q->q[0]; + len += q->q[1] * q->q[1]; + len += q->q[2] * q->q[2]; + len += q->q[3] * q->q[3]; + + return len; +} + +_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q) +{ + return CCD_SQRT(ccdQuatLen2(q)); +} + +_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w) +{ + q->q[0] = x; + q->q[1] = y; + q->q[2] = z; + q->q[3] = w; +} + +_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src) +{ + *dest = *src; +} + + +_ccd_inline int ccdQuatNormalize(ccd_quat_t *q) +{ + ccd_real_t len = ccdQuatLen(q); + if (len < CCD_EPS) + return 0; + + ccdQuatScale(q, CCD_ONE / len); + return 1; +} + +_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q, + ccd_real_t angle, const ccd_vec3_t *axis) +{ + ccd_real_t a, x, y, z, n, s; + + a = angle/2; + x = ccdVec3X(axis); + y = ccdVec3Y(axis); + z = ccdVec3Z(axis); + n = CCD_SQRT(x*x + y*y + z*z); + + // axis==0? (treat this the same as angle==0 with an arbitrary axis) + if (n < CCD_EPS){ + q->q[0] = q->q[1] = q->q[2] = CCD_ZERO; + q->q[3] = CCD_ONE; + }else{ + s = sin(a)/n; + + q->q[3] = cos(a); + q->q[0] = x*s; + q->q[1] = y*s; + q->q[2] = z*s; + + ccdQuatNormalize(q); + } +} + + +_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k) +{ + size_t i; + for (i = 0; i < 4; i++) + q->q[i] *= k; +} + +_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2) +{ + ccd_quat_t a; + ccdQuatCopy(&a, q); + ccdQuatMul2(q, &a, q2); +} + +_ccd_inline void ccdQuatMul2(ccd_quat_t *q, + const ccd_quat_t *a, const ccd_quat_t *b) +{ + q->q[0] = a->q[3] * b->q[0] + + a->q[0] * b->q[3] + + a->q[1] * b->q[2] + - a->q[2] * b->q[1]; + q->q[1] = a->q[3] * b->q[1] + + a->q[1] * b->q[3] + - a->q[0] * b->q[2] + + a->q[2] * b->q[0]; + q->q[2] = a->q[3] * b->q[2] + + a->q[2] * b->q[3] + + a->q[0] * b->q[1] + - a->q[1] * b->q[0]; + q->q[3] = a->q[3] * b->q[3] + - a->q[0] * b->q[0] + - a->q[1] * b->q[1] + - a->q[2] * b->q[2]; +} + +_ccd_inline int ccdQuatInvert(ccd_quat_t *q) +{ + ccd_real_t len2 = ccdQuatLen2(q); + if (len2 < CCD_EPS) + return -1; + + len2 = CCD_ONE / len2; + + q->q[0] = -q->q[0] * len2; + q->q[1] = -q->q[1] * len2; + q->q[2] = -q->q[2] * len2; + q->q[3] = q->q[3] * len2; + + return 0; +} +_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src) +{ + ccdQuatCopy(dest, src); + return ccdQuatInvert(dest); +} + +_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q) +{ + // original version: 31 mul + 21 add + // optimized version: 18 mul + 12 add + // formula: v = v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v) + ccd_real_t cross1_x, cross1_y, cross1_z, cross2_x, cross2_y, cross2_z; + ccd_real_t x, y, z, w; + ccd_real_t vx, vy, vz; + + vx = ccdVec3X(v); + vy = ccdVec3Y(v); + vz = ccdVec3Z(v); + + w = q->q[3]; + x = q->q[0]; + y = q->q[1]; + z = q->q[2]; + + cross1_x = y * vz - z * vy + w * vx; + cross1_y = z * vx - x * vz + w * vy; + cross1_z = x * vy - y * vx + w * vz; + cross2_x = y * cross1_z - z * cross1_y; + cross2_y = z * cross1_x - x * cross1_z; + cross2_z = x * cross1_y - y * cross1_x; + ccdVec3Set(v, vx + 2 * cross2_x, vy + 2 * cross2_y, vz + 2 * cross2_z); +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_QUAT_H__ */ diff --git a/3rdparty/libccd/src/ccd/vec3.h b/3rdparty/libccd/src/ccd/vec3.h new file mode 100644 index 0000000..0c6b29f --- /dev/null +++ b/3rdparty/libccd/src/ccd/vec3.h @@ -0,0 +1,340 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010-2013 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_VEC3_H__ +#define __CCD_VEC3_H__ + +#include +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + + +#ifndef CCD_SINGLE +# ifndef CCD_DOUBLE +# error You must define CCD_SINGLE or CCD_DOUBLE +# endif /* CCD_DOUBLE */ +#endif /* CCD_SINGLE */ + +#ifdef WIN32 +# define CCD_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#endif /* WIN32 */ + +#ifdef CCD_SINGLE +# ifdef CCD_DOUBLE +# error You can define either CCD_SINGLE or CCD_DOUBLE, not both! +# endif /* CCD_DOUBLE */ + +typedef float ccd_real_t; + +//# define CCD_EPS 1E-6 +# define CCD_EPS FLT_EPSILON + +# define CCD_REAL_MAX FLT_MAX + +# define CCD_REAL(x) (x ## f) /*!< form a constant */ +# define CCD_SQRT(x) (sqrtf(x)) /*!< square root */ +# define CCD_FABS(x) (fabsf(x)) /*!< absolute value */ +# define CCD_FMAX(x, y) (fmaxf((x), (y))) /*!< maximum of two floats */ + +# ifndef CCD_FMIN +# define CCD_FMIN(x, y) (fminf((x), (y))) /*!< minimum of two floats */ +# endif /* CCD_FMIN */ + +#endif /* CCD_SINGLE */ + +#ifdef CCD_DOUBLE +typedef double ccd_real_t; + +//# define CCD_EPS 1E-10 +# define CCD_EPS DBL_EPSILON + +# define CCD_REAL_MAX DBL_MAX + +# define CCD_REAL(x) (x) /*!< form a constant */ +# define CCD_SQRT(x) (sqrt(x)) /*!< square root */ +# define CCD_FABS(x) (fabs(x)) /*!< absolute value */ +# define CCD_FMAX(x, y) (fmax((x), (y))) /*!< maximum of two floats */ + +# ifndef CCD_FMIN +# define CCD_FMIN(x, y) (fmin((x), (y))) /*!< minimum of two floats */ +# endif /* CCD_FMIN */ + +#endif /* CCD_DOUBLE */ + +#define CCD_ONE CCD_REAL(1.) +#define CCD_ZERO CCD_REAL(0.) + +struct _ccd_vec3_t { + ccd_real_t v[3]; +}; +typedef struct _ccd_vec3_t ccd_vec3_t; + + +/** + * Holds origin (0,0,0) - this variable is meant to be read-only! + */ +CCD_EXPORT extern ccd_vec3_t *ccd_vec3_origin; + +/** + * Array of points uniformly distributed on unit sphere. + */ +CCD_EXPORT extern ccd_vec3_t *ccd_points_on_sphere; +CCD_EXPORT extern size_t ccd_points_on_sphere_len; + +/** Returns sign of value. */ +_ccd_inline int ccdSign(ccd_real_t val); +/** Returns true if val is zero. **/ +_ccd_inline int ccdIsZero(ccd_real_t val); +/** Returns true if a and b equal. **/ +_ccd_inline int ccdEq(ccd_real_t a, ccd_real_t b); + + +#define CCD_VEC3_STATIC(x, y, z) \ + { { (x), (y), (z) } } + +#define CCD_VEC3(name, x, y, z) \ + ccd_vec3_t name = CCD_VEC3_STATIC((x), (y), (z)) + +_ccd_inline ccd_real_t ccdVec3X(const ccd_vec3_t *v); +_ccd_inline ccd_real_t ccdVec3Y(const ccd_vec3_t *v); +_ccd_inline ccd_real_t ccdVec3Z(const ccd_vec3_t *v); + +/** + * Returns true if a and b equal. + */ +_ccd_inline int ccdVec3Eq(const ccd_vec3_t *a, const ccd_vec3_t *b); + +/** + * Returns squared length of vector. + */ +_ccd_inline ccd_real_t ccdVec3Len2(const ccd_vec3_t *v); + +/** + * Returns distance between a and b. + */ +_ccd_inline ccd_real_t ccdVec3Dist2(const ccd_vec3_t *a, const ccd_vec3_t *b); + + +_ccd_inline void ccdVec3Set(ccd_vec3_t *v, ccd_real_t x, ccd_real_t y, ccd_real_t z); + +/** + * v = w + */ +_ccd_inline void ccdVec3Copy(ccd_vec3_t *v, const ccd_vec3_t *w); + +/** + * Substracts coordinates of vector w from vector v. v = v - w + */ +_ccd_inline void ccdVec3Sub(ccd_vec3_t *v, const ccd_vec3_t *w); + +/** + * Adds coordinates of vector w to vector v. v = v + w + */ +_ccd_inline void ccdVec3Add(ccd_vec3_t *v, const ccd_vec3_t *w); + +/** + * d = v - w + */ +_ccd_inline void ccdVec3Sub2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w); + +/** + * d = d * k; + */ +_ccd_inline void ccdVec3Scale(ccd_vec3_t *d, ccd_real_t k); + +/** + * Normalizes given vector to unit length. + */ +_ccd_inline void ccdVec3Normalize(ccd_vec3_t *d); + + +/** + * Dot product of two vectors. + */ +_ccd_inline ccd_real_t ccdVec3Dot(const ccd_vec3_t *a, const ccd_vec3_t *b); + +/** + * Cross product: d = a x b. + */ +_ccd_inline void ccdVec3Cross(ccd_vec3_t *d, const ccd_vec3_t *a, const ccd_vec3_t *b); + + +/** + * Returns distance^2 of point P to segment ab. + * If witness is non-NULL it is filled with coordinates of point from which + * was computed distance to point P. + */ +CCD_EXPORT ccd_real_t ccdVec3PointSegmentDist2(const ccd_vec3_t *P, + const ccd_vec3_t *a, + const ccd_vec3_t *b, + ccd_vec3_t *witness); + +/** + * Returns distance^2 of point P from triangle formed by triplet a, b, c. + * If witness vector is provided it is filled with coordinates of point + * from which was computed distance to point P. + */ +CCD_EXPORT ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P, + const ccd_vec3_t *a, + const ccd_vec3_t *b, + const ccd_vec3_t *c, + ccd_vec3_t *witness); + + +/**** INLINES ****/ +_ccd_inline int ccdSign(ccd_real_t val) +{ + if (ccdIsZero(val)){ + return 0; + }else if (val < CCD_ZERO){ + return -1; + } + return 1; +} + +_ccd_inline int ccdIsZero(ccd_real_t val) +{ + return CCD_FABS(val) < CCD_EPS; +} + +_ccd_inline int ccdEq(ccd_real_t _a, ccd_real_t _b) +{ + ccd_real_t ab; + ccd_real_t a, b; + + ab = CCD_FABS(_a - _b); + if (CCD_FABS(ab) < CCD_EPS) + return 1; + + a = CCD_FABS(_a); + b = CCD_FABS(_b); + if (b > a){ + return ab < CCD_EPS * b; + }else{ + return ab < CCD_EPS * a; + } +} + + +_ccd_inline ccd_real_t ccdVec3X(const ccd_vec3_t *v) +{ + return v->v[0]; +} + +_ccd_inline ccd_real_t ccdVec3Y(const ccd_vec3_t *v) +{ + return v->v[1]; +} + +_ccd_inline ccd_real_t ccdVec3Z(const ccd_vec3_t *v) +{ + return v->v[2]; +} + +_ccd_inline int ccdVec3Eq(const ccd_vec3_t *a, const ccd_vec3_t *b) +{ + return ccdEq(ccdVec3X(a), ccdVec3X(b)) + && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) + && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); +} + +_ccd_inline ccd_real_t ccdVec3Len2(const ccd_vec3_t *v) +{ + return ccdVec3Dot(v, v); +} + +_ccd_inline ccd_real_t ccdVec3Dist2(const ccd_vec3_t *a, const ccd_vec3_t *b) +{ + ccd_vec3_t ab; + ccdVec3Sub2(&ab, a, b); + return ccdVec3Len2(&ab); +} + +_ccd_inline void ccdVec3Set(ccd_vec3_t *v, ccd_real_t x, ccd_real_t y, ccd_real_t z) +{ + v->v[0] = x; + v->v[1] = y; + v->v[2] = z; +} + +_ccd_inline void ccdVec3Copy(ccd_vec3_t *v, const ccd_vec3_t *w) +{ + *v = *w; +} + +_ccd_inline void ccdVec3Sub(ccd_vec3_t *v, const ccd_vec3_t *w) +{ + v->v[0] -= w->v[0]; + v->v[1] -= w->v[1]; + v->v[2] -= w->v[2]; +} +_ccd_inline void ccdVec3Sub2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w) +{ + d->v[0] = v->v[0] - w->v[0]; + d->v[1] = v->v[1] - w->v[1]; + d->v[2] = v->v[2] - w->v[2]; +} + +_ccd_inline void ccdVec3Add(ccd_vec3_t *v, const ccd_vec3_t *w) +{ + v->v[0] += w->v[0]; + v->v[1] += w->v[1]; + v->v[2] += w->v[2]; +} + +_ccd_inline void ccdVec3Scale(ccd_vec3_t *d, ccd_real_t k) +{ + d->v[0] *= k; + d->v[1] *= k; + d->v[2] *= k; +} + +_ccd_inline void ccdVec3Normalize(ccd_vec3_t *d) +{ + ccd_real_t k = CCD_ONE / CCD_SQRT(ccdVec3Len2(d)); + ccdVec3Scale(d, k); +} + +_ccd_inline ccd_real_t ccdVec3Dot(const ccd_vec3_t *a, const ccd_vec3_t *b) +{ + ccd_real_t dot; + + dot = a->v[0] * b->v[0]; + dot += a->v[1] * b->v[1]; + dot += a->v[2] * b->v[2]; + return dot; +} + +_ccd_inline void ccdVec3Cross(ccd_vec3_t *d, const ccd_vec3_t *a, const ccd_vec3_t *b) +{ + d->v[0] = (a->v[1] * b->v[2]) - (a->v[2] * b->v[1]); + d->v[1] = (a->v[2] * b->v[0]) - (a->v[0] * b->v[2]); + d->v[2] = (a->v[0] * b->v[1]) - (a->v[1] * b->v[0]); +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_VEC3_H__ */ diff --git a/3rdparty/libccd/src/dbg.h b/3rdparty/libccd/src/dbg.h new file mode 100644 index 0000000..f4852c1 --- /dev/null +++ b/3rdparty/libccd/src/dbg.h @@ -0,0 +1,65 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_DBG_H__ +#define __CCD_DBG_H__ + +/** + * Some macros which can be used for printing debug info to stderr if macro + * NDEBUG not defined. + * + * DBG_PROLOGUE can be specified as string and this string will be + * prepended to output text + */ +#ifndef NDEBUG + +#include + +#ifndef DBG_PROLOGUE +# define DBG_PROLOGUE +#endif + +# define DBG(format, ...) do { \ + fprintf(stderr, DBG_PROLOGUE "%s :: " format "\n", __func__, ## __VA_ARGS__); \ + fflush(stderr); \ + } while (0) + +# define DBG2(str) do { \ + fprintf(stderr, DBG_PROLOGUE "%s :: " str "\n", __func__); \ + fflush(stderr); \ + } while (0) + +# define DBG_VEC3(vec, prefix) do {\ + fprintf(stderr, DBG_PROLOGUE "%s :: %s[%lf %lf %lf]\n", \ + __func__, prefix, ccdVec3X(vec), ccdVec3Y(vec), ccdVec3Z(vec)); \ + fflush(stderr); \ + } while (0) +/* +# define DBG_VEC3(vec, prefix) do {\ + fprintf(stderr, DBG_PROLOGUE "%s :: %s[%.20lf %.20lf %.20lf]\n", \ + __func__, prefix, ccdVec3X(vec), ccdVec3Y(vec), ccdVec3Z(vec)); \ + fflush(stderr); \ + } while (0) +*/ + +#else +# define DBG(format, ...) +# define DBG2(str) +# define DBG_VEC3(v, prefix) +#endif + +#endif /* __CCD_DBG_H__ */ diff --git a/3rdparty/libccd/src/list.h b/3rdparty/libccd/src/list.h new file mode 100644 index 0000000..4fd482b --- /dev/null +++ b/3rdparty/libccd/src/list.h @@ -0,0 +1,155 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_LIST_H__ +#define __CCD_LIST_H__ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_list_t { + struct _ccd_list_t *next, *prev; +}; +typedef struct _ccd_list_t ccd_list_t; + + + +/** + * Get the struct for this entry. + * @ptr: the &ccd_list_t pointer. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_struct within the struct. + */ +#define ccdListEntry(ptr, type, member) \ + ccd_container_of(ptr, type, member) + +/** + * Iterates over list. + */ +#define ccdListForEach(list, item) \ + for (item = (list)->next; \ + _ccd_prefetch((item)->next), item != (list); \ + item = (item)->next) + +/** + * Iterates over list safe against remove of list entry + */ +#define ccdListForEachSafe(list, item, tmp) \ + for (item = (list)->next, tmp = (item)->next; \ + item != (list); \ + item = tmp, tmp = (item)->next) + +/** + * Iterates over list of given type. + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define ccdListForEachEntry(head, pos, postype, member) \ + for (pos = ccdListEntry((head)->next, postype, member); \ + _ccd_prefetch(pos->member.next), &pos->member != (head); \ + pos = ccdListEntry(pos->member.next, postype, member)) + +/** + * Iterates over list of given type safe against removal of list entry + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define ccdListForEachEntrySafe(head, pos, postype, n, ntype, member) \ + for (pos = ccdListEntry((head)->next, postype, member), \ + n = ccdListEntry(pos->member.next, postype, member); \ + &pos->member != (head); \ + pos = n, n = ccdListEntry(n->member.next, ntype, member)) + + +/** + * Initialize list. + */ +_ccd_inline void ccdListInit(ccd_list_t *l); + +_ccd_inline ccd_list_t *ccdListNext(ccd_list_t *l); +_ccd_inline ccd_list_t *ccdListPrev(ccd_list_t *l); + +/** + * Returns true if list is empty. + */ +_ccd_inline int ccdListEmpty(const ccd_list_t *head); + +/** + * Appends item to end of the list l. + */ +_ccd_inline void ccdListAppend(ccd_list_t *l, ccd_list_t *item); + +/** + * Removes item from list. + */ +_ccd_inline void ccdListDel(ccd_list_t *item); + + + +/// +/// INLINES: +/// + +_ccd_inline void ccdListInit(ccd_list_t *l) +{ + l->next = l; + l->prev = l; +} + +_ccd_inline ccd_list_t *ccdListNext(ccd_list_t *l) +{ + return l->next; +} + +_ccd_inline ccd_list_t *ccdListPrev(ccd_list_t *l) +{ + return l->prev; +} + +_ccd_inline int ccdListEmpty(const ccd_list_t *head) +{ + return head->next == head; +} + +_ccd_inline void ccdListAppend(ccd_list_t *l, ccd_list_t *new) +{ + new->prev = l->prev; + new->next = l; + l->prev->next = new; + l->prev = new; +} + +_ccd_inline void ccdListDel(ccd_list_t *item) +{ + item->next->prev = item->prev; + item->prev->next = item->next; + item->next = item; + item->prev = item; +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_LIST_H__ */ diff --git a/3rdparty/libccd/src/mpr.c b/3rdparty/libccd/src/mpr.c new file mode 100644 index 0000000..2ad6c87 --- /dev/null +++ b/3rdparty/libccd/src/mpr.c @@ -0,0 +1,543 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010,2011 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include +#include +#include "simplex.h" +#include "dbg.h" + +/** Finds origin (center) of Minkowski difference (actually it can be any + * interior point of Minkowski difference. */ +_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_support_t *center); + +/** Discovers initial portal - that is tetrahedron that intersects with + * origin ray (ray from center of Minkowski diff to (0,0,0). + * + * Returns -1 if already recognized that origin is outside Minkowski + * portal. + * Returns 1 if origin lies on v1 of simplex (only v0 and v1 are present + * in simplex). + * Returns 2 if origin lies on v0-v1 segment. + * Returns 0 if portal was built. + */ +static int discoverPortal(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *portal); + + +/** Expands portal towards origin and determine if objects intersect. + * Already established portal must be given as argument. + * If intersection is found 0 is returned, -1 otherwise */ +static int refinePortal(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *portal); + +/** Finds penetration info by expanding provided portal. */ +static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos); + +/** Finds penetration info if origin lies on portal's v1 */ +static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos); + +/** Find penetration info if origin lies on portal's segment v0-v1 */ +static void findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos); + +/** Finds position vector from fully established portal */ +static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd, + const ccd_simplex_t *portal, ccd_vec3_t *pos); + +/** Extends portal with new support point. + * Portal must have face v1-v2-v3 arranged to face outside portal. */ +_ccd_inline void expandPortal(ccd_simplex_t *portal, + const ccd_support_t *v4); + +/** Fill dir with direction outside portal. Portal's v1-v2-v3 face must be + * arranged in correct order! */ +_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir); + +/** Returns true if portal encapsules origin (0,0,0), dir is direction of + * v1-v2-v3 face. */ +_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal, + const ccd_vec3_t *dir); + +/** Returns true if portal with new point v4 would reach specified + * tolerance (i.e. returns true if portal can _not_ significantly expand + * within Minkowski difference). + * + * v4 is candidate for new point in portal, dir is direction in which v4 + * was obtained. */ +_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal, + const ccd_support_t *v4, + const ccd_vec3_t *dir, + const ccd_t *ccd); + +/** Returns true if portal expanded by new point v4 could possibly contain + * origin, dir is direction in which v4 was obtained. */ +_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal, + const ccd_support_t *v4, + const ccd_vec3_t *dir); + + +int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd) +{ + ccd_simplex_t portal; + int res; + + // Phase 1: Portal discovery - find portal that intersects with origin + // ray (ray from center of Minkowski diff to origin of coordinates) + res = discoverPortal(obj1, obj2, ccd, &portal); + if (res < 0) + return 0; + if (res > 0) + return 1; + + // Phase 2: Portal refinement + res = refinePortal(obj1, obj2, ccd, &portal); + return (res == 0 ? 1 : 0); +} + +int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) +{ + ccd_simplex_t portal; + int res; + + // Phase 1: Portal discovery + res = discoverPortal(obj1, obj2, ccd, &portal); + if (res < 0){ + // Origin isn't inside portal - no collision. + return -1; + + }else if (res == 1){ + // Touching contact on portal's v1. + findPenetrTouch(obj1, obj2, ccd, &portal, depth, dir, pos); + + }else if (res == 2){ + // Origin lies on v0-v1 segment. + findPenetrSegment(obj1, obj2, ccd, &portal, depth, dir, pos); + + }else if (res == 0){ + // Phase 2: Portal refinement + res = refinePortal(obj1, obj2, ccd, &portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + findPenetr(obj1, obj2, ccd, &portal, depth, dir, pos); + } + + return 0; +} + + + +_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_support_t *center) +{ + ccd->center1(obj1, ¢er->v1); + ccd->center2(obj2, ¢er->v2); + ccdVec3Sub2(¢er->v, ¢er->v1, ¢er->v2); +} + +static int discoverPortal(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *portal) +{ + ccd_vec3_t dir, va, vb; + ccd_real_t dot; + int cont; + + // vertex 0 is center of portal + findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0)); + ccdSimplexSetSize(portal, 1); + + if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO); + ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v); + ccdVec3Scale(&dir, CCD_REAL(-1.)); + ccdVec3Normalize(&dir); + __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1)); + ccdSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir); + if (ccdIsZero(dot) || dot < CCD_ZERO) + return -1; + + + // vertex 2 + ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v, + &ccdSimplexPoint(portal, 1)->v); + if (ccdIsZero(ccdVec3Len2(&dir))){ + if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + ccdVec3Normalize(&dir); + __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2)); + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir); + if (ccdIsZero(dot) || dot < CCD_ZERO) + return -1; + + ccdSimplexSetSize(portal, 3); + + // vertex 3 direction + ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 0)->v); + ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 0)->v); + ccdVec3Cross(&dir, &va, &vb); + ccdVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v); + if (dot > CCD_ZERO){ + ccdSimplexSwap(portal, 1, 2); + ccdVec3Scale(&dir, CCD_REAL(-1.)); + } + + while (ccdSimplexSize(portal) < 4){ + __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3)); + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir); + if (ccdIsZero(dot) || dot < CCD_ZERO) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 3)->v); + dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v); + if (dot < CCD_ZERO && !ccdIsZero(dot)){ + ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v, + &ccdSimplexPoint(portal, 2)->v); + dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v); + if (dot < CCD_ZERO && !ccdIsZero(dot)){ + ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 0)->v); + ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 0)->v); + ccdVec3Cross(&dir, &va, &vb); + ccdVec3Normalize(&dir); + }else{ + ccdSimplexSetSize(portal, 4); + } + } + + return 0; +} + +static int refinePortal(const void *obj1, const void *obj2, + const ccd_t *ccd, ccd_simplex_t *portal) +{ + ccd_vec3_t dir; + ccd_support_t v4; + + while (1){ + // compute direction outside the portal (from v0 throught v1,v2,v3 + // face) + portalDir(portal, &dir); + + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; + + // get next support point + __ccdSupport(obj1, obj2, &dir, ccd, &v4); + + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) + || portalReachTolerance(portal, &v4, &dir, ccd)){ + return -1; + } + + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + expandPortal(portal, &v4); + } + + return -1; +} + + +static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos) +{ + ccd_vec3_t dir; + ccd_support_t v4; + unsigned long iterations; + + iterations = 0UL; + while (1){ + // compute portal direction and obtain next support point + portalDir(portal, &dir); + __ccdSupport(obj1, obj2, &dir, ccd, &v4); + + // reached tolerance -> find penetration info + if (portalReachTolerance(portal, &v4, &dir, ccd) + || iterations > ccd->max_iterations){ + *depth = ccdVec3PointTriDist2(ccd_vec3_origin, + &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 3)->v, + pdir); + *depth = CCD_SQRT(*depth); + if (ccdIsZero(*depth)){ + // If depth is zero, then we have a touching contact. + // So following findPenetrTouch(), we assign zero to + // the direction vector (it can actually be anything + // according to the decription of ccdMPRPenetration + // function). + ccdVec3Copy(pdir, ccd_vec3_origin); + }else{ + ccdVec3Normalize(pdir); + } + + // barycentric coordinates: + findPos(obj1, obj2, ccd, portal, pos); + + return; + } + + expandPortal(portal, &v4); + + iterations++; + } +} + +static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = CCD_REAL(0.); + ccdVec3Copy(dir, ccd_vec3_origin); + + ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1); + ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2); + ccdVec3Scale(pos, 0.5); +} + +static void findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd, + ccd_simplex_t *portal, + ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) +{ + /* + ccd_vec3_t vec; + ccd_real_t k; + */ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1); + ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2); + ccdVec3Scale(pos, CCD_REAL(0.5)); + + /* + ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 0)->v); + k = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v)); + k /= CCD_SQRT(ccdVec3Len2(&vec)); + ccdVec3Scale(&vec, -k); + ccdVec3Add(pos, &vec); + */ + + ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v); + *depth = CCD_SQRT(ccdVec3Len2(dir)); + ccdVec3Normalize(dir); +} + + +static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd, + const ccd_simplex_t *portal, ccd_vec3_t *pos) +{ + ccd_vec3_t dir; + size_t i; + ccd_real_t b[4], sum, inv; + ccd_vec3_t vec, p1, p2; + + portalDir(portal, &dir); + + // use barycentric coordinates of tetrahedron to find origin + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 2)->v); + b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v); + + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v, + &ccdSimplexPoint(portal, 2)->v); + b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v); + + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v, + &ccdSimplexPoint(portal, 1)->v); + b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v); + + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 1)->v); + b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (ccdIsZero(sum) || sum < CCD_ZERO){ + b[0] = CCD_REAL(0.); + + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 3)->v); + b[1] = ccdVec3Dot(&vec, &dir); + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v, + &ccdSimplexPoint(portal, 1)->v); + b[2] = ccdVec3Dot(&vec, &dir); + ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v, + &ccdSimplexPoint(portal, 2)->v); + b[3] = ccdVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = CCD_REAL(1.) / sum; + + ccdVec3Copy(&p1, ccd_vec3_origin); + ccdVec3Copy(&p2, ccd_vec3_origin); + for (i = 0; i < 4; i++){ + ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v1); + ccdVec3Scale(&vec, b[i]); + ccdVec3Add(&p1, &vec); + + ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v2); + ccdVec3Scale(&vec, b[i]); + ccdVec3Add(&p2, &vec); + } + ccdVec3Scale(&p1, inv); + ccdVec3Scale(&p2, inv); + + ccdVec3Copy(pos, &p1); + ccdVec3Add(pos, &p2); + ccdVec3Scale(pos, 0.5); +} + +_ccd_inline void expandPortal(ccd_simplex_t *portal, + const ccd_support_t *v4) +{ + ccd_real_t dot; + ccd_vec3_t v4v0; + + ccdVec3Cross(&v4v0, &v4->v, &ccdSimplexPoint(portal, 0)->v); + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &v4v0); + if (dot > CCD_ZERO){ + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &v4v0); + if (dot > CCD_ZERO){ + ccdSimplexSet(portal, 1, v4); + }else{ + ccdSimplexSet(portal, 3, v4); + } + }else{ + dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &v4v0); + if (dot > CCD_ZERO){ + ccdSimplexSet(portal, 2, v4); + }else{ + ccdSimplexSet(portal, 1, v4); + } + } +} + +_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir) +{ + ccd_vec3_t v2v1, v3v1; + + ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v, + &ccdSimplexPoint(portal, 1)->v); + ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v, + &ccdSimplexPoint(portal, 1)->v); + ccdVec3Cross(dir, &v2v1, &v3v1); + ccdVec3Normalize(dir); +} + +_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal, + const ccd_vec3_t *dir) +{ + ccd_real_t dot; + dot = ccdVec3Dot(dir, &ccdSimplexPoint(portal, 1)->v); + return ccdIsZero(dot) || dot > CCD_ZERO; +} + +_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal, + const ccd_support_t *v4, + const ccd_vec3_t *dir, + const ccd_t *ccd) +{ + ccd_real_t dv1, dv2, dv3, dv4; + ccd_real_t dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, dir); + dv2 = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, dir); + dv3 = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, dir); + dv4 = ccdVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = CCD_FMIN(dot1, dot2); + dot1 = CCD_FMIN(dot1, dot3); + + return ccdEq(dot1, ccd->mpr_tolerance) || dot1 < ccd->mpr_tolerance; +} + +_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal, + const ccd_support_t *v4, + const ccd_vec3_t *dir) +{ + ccd_real_t dot; + dot = ccdVec3Dot(&v4->v, dir); + return ccdIsZero(dot) || dot > CCD_ZERO; +} diff --git a/3rdparty/libccd/src/polytope.c b/3rdparty/libccd/src/polytope.c new file mode 100644 index 0000000..e594832 --- /dev/null +++ b/3rdparty/libccd/src/polytope.c @@ -0,0 +1,298 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include +#include +#include "polytope.h" +#include "alloc.h" + +_ccd_inline void _ccdPtNearestUpdate(ccd_pt_t *pt, ccd_pt_el_t *el) +{ + if (ccdEq(pt->nearest_dist, el->dist)){ + if (el->type < pt->nearest_type){ + pt->nearest = el; + pt->nearest_dist = el->dist; + pt->nearest_type = el->type; + } + }else if (el->dist < pt->nearest_dist){ + pt->nearest = el; + pt->nearest_dist = el->dist; + pt->nearest_type = el->type; + } +} + +static void _ccdPtNearestRenew(ccd_pt_t *pt) +{ + ccd_pt_vertex_t *v; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + + pt->nearest_dist = CCD_REAL_MAX; + pt->nearest_type = 3; + pt->nearest = NULL; + + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)v); + } + + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)e); + } + + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)f); + } +} + + + +void ccdPtInit(ccd_pt_t *pt) +{ + ccdListInit(&pt->vertices); + ccdListInit(&pt->edges); + ccdListInit(&pt->faces); + + pt->nearest = NULL; + pt->nearest_dist = CCD_REAL_MAX; + pt->nearest_type = 3; +} + +void ccdPtDestroy(ccd_pt_t *pt) +{ + ccd_pt_face_t *f, *f2; + ccd_pt_edge_t *e, *e2; + ccd_pt_vertex_t *v, *v2; + + // first delete all faces + ccdListForEachEntrySafe(&pt->faces, f, ccd_pt_face_t, f2, ccd_pt_face_t, list){ + ccdPtDelFace(pt, f); + } + + // delete all edges + ccdListForEachEntrySafe(&pt->edges, e, ccd_pt_edge_t, e2, ccd_pt_edge_t, list){ + ccdPtDelEdge(pt, e); + } + + // delete all vertices + ccdListForEachEntrySafe(&pt->vertices, v, ccd_pt_vertex_t, v2, ccd_pt_vertex_t, list){ + ccdPtDelVertex(pt, v); + } +} + + +ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v) +{ + ccd_pt_vertex_t *vert; + + vert = CCD_ALLOC(ccd_pt_vertex_t); + if (vert == NULL) + return NULL; + + vert->type = CCD_PT_VERTEX; + ccdSupportCopy(&vert->v, v); + + vert->dist = ccdVec3Len2(&vert->v.v); + ccdVec3Copy(&vert->witness, &vert->v.v); + + ccdListInit(&vert->edges); + + // add vertex to list + ccdListAppend(&pt->vertices, &vert->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)vert); + + return vert; +} + +ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1, + ccd_pt_vertex_t *v2) +{ + const ccd_vec3_t *a, *b; + ccd_pt_edge_t *edge; + + if (v1 == NULL || v2 == NULL) + return NULL; + + edge = CCD_ALLOC(ccd_pt_edge_t); + if (edge == NULL) + return NULL; + + edge->type = CCD_PT_EDGE; + edge->vertex[0] = v1; + edge->vertex[1] = v2; + edge->faces[0] = edge->faces[1] = NULL; + + a = &edge->vertex[0]->v.v; + b = &edge->vertex[1]->v.v; + edge->dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &edge->witness); + + ccdListAppend(&edge->vertex[0]->edges, &edge->vertex_list[0]); + ccdListAppend(&edge->vertex[1]->edges, &edge->vertex_list[1]); + + ccdListAppend(&pt->edges, &edge->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)edge); + + return edge; +} + +ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1, + ccd_pt_edge_t *e2, + ccd_pt_edge_t *e3) +{ + const ccd_vec3_t *a, *b, *c; + ccd_pt_face_t *face; + ccd_pt_edge_t *e; + size_t i; + + if (e1 == NULL || e2 == NULL || e3 == NULL) + return NULL; + + face = CCD_ALLOC(ccd_pt_face_t); + if (face == NULL) + return NULL; + + face->type = CCD_PT_FACE; + face->edge[0] = e1; + face->edge[1] = e2; + face->edge[2] = e3; + + // obtain triplet of vertices + a = &face->edge[0]->vertex[0]->v.v; + b = &face->edge[0]->vertex[1]->v.v; + e = face->edge[1]; + if (e->vertex[0] != face->edge[0]->vertex[0] + && e->vertex[0] != face->edge[0]->vertex[1]){ + c = &e->vertex[0]->v.v; + }else{ + c = &e->vertex[1]->v.v; + } + face->dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &face->witness); + + + for (i = 0; i < 3; i++){ + if (face->edge[i]->faces[0] == NULL){ + face->edge[i]->faces[0] = face; + }else{ + face->edge[i]->faces[1] = face; + } + } + + ccdListAppend(&pt->faces, &face->list); + + // update position in .nearest array + _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)face); + + return face; +} + + +void ccdPtRecomputeDistances(ccd_pt_t *pt) +{ + ccd_pt_vertex_t *v; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + const ccd_vec3_t *a, *b, *c; + ccd_real_t dist; + + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + dist = ccdVec3Len2(&v->v.v); + v->dist = dist; + ccdVec3Copy(&v->witness, &v->v.v); + } + + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + a = &e->vertex[0]->v.v; + b = &e->vertex[1]->v.v; + dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &e->witness); + e->dist = dist; + } + + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + // obtain triplet of vertices + a = &f->edge[0]->vertex[0]->v.v; + b = &f->edge[0]->vertex[1]->v.v; + e = f->edge[1]; + if (e->vertex[0] != f->edge[0]->vertex[0] + && e->vertex[0] != f->edge[0]->vertex[1]){ + c = &e->vertex[0]->v.v; + }else{ + c = &e->vertex[1]->v.v; + } + + dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &f->witness); + f->dist = dist; + } +} + +ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt) +{ + if (!pt->nearest){ + _ccdPtNearestRenew(pt); + } + return pt->nearest; +} + + +void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn) +{ + FILE *fout; + + fout = fopen(fn, "a"); + if (fout == NULL) + return; + + ccdPtDumpSVT2(pt, fout); + + fclose(fout); +} + +void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *fout) +{ + ccd_pt_vertex_t *v, *a, *b, *c; + ccd_pt_edge_t *e; + ccd_pt_face_t *f; + size_t i; + + fprintf(fout, "-----\n"); + + fprintf(fout, "Points:\n"); + i = 0; + ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ + v->id = i++; + fprintf(fout, "%lf %lf %lf\n", + ccdVec3X(&v->v.v), ccdVec3Y(&v->v.v), ccdVec3Z(&v->v.v)); + } + + fprintf(fout, "Edges:\n"); + ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){ + fprintf(fout, "%d %d\n", e->vertex[0]->id, e->vertex[1]->id); + } + + fprintf(fout, "Faces:\n"); + ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){ + a = f->edge[0]->vertex[0]; + b = f->edge[0]->vertex[1]; + c = f->edge[1]->vertex[0]; + if (c == a || c == b){ + c = f->edge[1]->vertex[1]; + } + fprintf(fout, "%d %d %d\n", a->id, b->id, c->id); + } +} diff --git a/3rdparty/libccd/src/polytope.h b/3rdparty/libccd/src/polytope.h new file mode 100644 index 0000000..f721e64 --- /dev/null +++ b/3rdparty/libccd/src/polytope.h @@ -0,0 +1,322 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_POLYTOPE_H__ +#define __CCD_POLYTOPE_H__ + +#include +#include +#include "support.h" +#include "list.h" + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +#define CCD_PT_VERTEX 1 +#define CCD_PT_EDGE 2 +#define CCD_PT_FACE 3 + + +#define __CCD_PT_EL \ + int type; /*! type of element */ \ + ccd_real_t dist; /*! distance from origin */ \ + ccd_vec3_t witness; /*! witness point of projection of origin */ \ + ccd_list_t list; /*! list of elements of same type */ + +/** + * General polytope element. + * Could be vertex, edge or triangle. + */ +struct _ccd_pt_el_t { + __CCD_PT_EL +}; +typedef struct _ccd_pt_el_t ccd_pt_el_t; + +struct _ccd_pt_edge_t; +struct _ccd_pt_face_t; + +/** + * Polytope's vertex. + */ +struct _ccd_pt_vertex_t { + __CCD_PT_EL + + int id; + ccd_support_t v; + ccd_list_t edges; //!< List of edges +}; +typedef struct _ccd_pt_vertex_t ccd_pt_vertex_t; + +/** + * Polytope's edge. + */ +struct _ccd_pt_edge_t { + __CCD_PT_EL + + ccd_pt_vertex_t *vertex[2]; //!< Reference to vertices + struct _ccd_pt_face_t *faces[2]; //!< Reference to faces + + ccd_list_t vertex_list[2]; //!< List items in vertices' lists +}; +typedef struct _ccd_pt_edge_t ccd_pt_edge_t; + +/** + * Polytope's triangle faces. + */ +struct _ccd_pt_face_t { + __CCD_PT_EL + + ccd_pt_edge_t *edge[3]; //!< Reference to surrounding edges +}; +typedef struct _ccd_pt_face_t ccd_pt_face_t; + + +/** + * Struct containing polytope. + */ +struct _ccd_pt_t { + ccd_list_t vertices; //!< List of vertices + ccd_list_t edges; //!< List of edges + ccd_list_t faces; //!< List of faces + + ccd_pt_el_t *nearest; + ccd_real_t nearest_dist; + int nearest_type; +}; +typedef struct _ccd_pt_t ccd_pt_t; + + +CCD_EXPORT void ccdPtInit(ccd_pt_t *pt); +CCD_EXPORT void ccdPtDestroy(ccd_pt_t *pt); + +/** + * Returns vertices surrounding given triangle face. + */ +_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face, + ccd_vec3_t **a, + ccd_vec3_t **b, + ccd_vec3_t **c); +_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face, + ccd_pt_vertex_t **a, + ccd_pt_vertex_t **b, + ccd_pt_vertex_t **c); +_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f, + ccd_pt_edge_t **a, + ccd_pt_edge_t **b, + ccd_pt_edge_t **c); + +_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e, + ccd_vec3_t **a, + ccd_vec3_t **b); +_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e, + ccd_pt_vertex_t **a, + ccd_pt_vertex_t **b); +_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e, + ccd_pt_face_t **f1, + ccd_pt_face_t **f2); + + +/** + * Adds vertex to polytope and returns pointer to newly created vertex. + */ +CCD_EXPORT ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v); +_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt, + ccd_real_t x, ccd_real_t y, ccd_real_t z); + +/** + * Adds edge to polytope. + */ +CCD_EXPORT ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1, + ccd_pt_vertex_t *v2); + +/** + * Adds face to polytope. + */ +CCD_EXPORT ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1, + ccd_pt_edge_t *e2, + ccd_pt_edge_t *e3); + +/** + * Deletes vertex from polytope. + * Returns 0 on success, -1 otherwise. + */ +_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *); +_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *); +_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *); + + +/** + * Recompute distances from origin for all elements in pt. + */ +CCD_EXPORT void ccdPtRecomputeDistances(ccd_pt_t *pt); + +/** + * Returns nearest element to origin. + */ +CCD_EXPORT ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt); + + +CCD_EXPORT void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn); +CCD_EXPORT void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *); + + +/**** INLINES ****/ +_ccd_inline ccd_pt_vertex_t *ccdPtAddVertexCoords(ccd_pt_t *pt, + ccd_real_t x, ccd_real_t y, ccd_real_t z) +{ + ccd_support_t s; + ccdVec3Set(&s.v, x, y, z); + return ccdPtAddVertex(pt, &s); +} + +_ccd_inline int ccdPtDelVertex(ccd_pt_t *pt, ccd_pt_vertex_t *v) +{ + // test if any edge is connected to this vertex + if (!ccdListEmpty(&v->edges)) + return -1; + + // delete vertex from main list + ccdListDel(&v->list); + + if ((void *)pt->nearest == (void *)v){ + pt->nearest = NULL; + } + + free(v); + return 0; +} + +_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *e) +{ + // text if any face is connected to this edge (faces[] is always + // aligned to lower indices) + if (e->faces[0] != NULL) + return -1; + + // disconnect edge from lists of edges in vertex struct + ccdListDel(&e->vertex_list[0]); + ccdListDel(&e->vertex_list[1]); + + // disconnect edge from main list + ccdListDel(&e->list); + + if ((void *)pt->nearest == (void *)e){ + pt->nearest = NULL; + } + + free(e); + return 0; +} + +_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *f) +{ + ccd_pt_edge_t *e; + size_t i; + + // remove face from edges' recerence lists + for (i = 0; i < 3; i++){ + e = f->edge[i]; + if (e->faces[0] == f){ + e->faces[0] = e->faces[1]; + } + e->faces[1] = NULL; + } + + // remove face from list of all faces + ccdListDel(&f->list); + + if ((void *)pt->nearest == (void *)f){ + pt->nearest = NULL; + } + + free(f); + return 0; +} + +_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face, + ccd_vec3_t **a, + ccd_vec3_t **b, + ccd_vec3_t **c) +{ + *a = &face->edge[0]->vertex[0]->v.v; + *b = &face->edge[0]->vertex[1]->v.v; + + if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0] + && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){ + *c = &face->edge[1]->vertex[0]->v.v; + }else{ + *c = &face->edge[1]->vertex[1]->v.v; + } +} + +_ccd_inline void ccdPtFaceVertices(const ccd_pt_face_t *face, + ccd_pt_vertex_t **a, + ccd_pt_vertex_t **b, + ccd_pt_vertex_t **c) +{ + *a = face->edge[0]->vertex[0]; + *b = face->edge[0]->vertex[1]; + + if (face->edge[1]->vertex[0] != face->edge[0]->vertex[0] + && face->edge[1]->vertex[0] != face->edge[0]->vertex[1]){ + *c = face->edge[1]->vertex[0]; + }else{ + *c = face->edge[1]->vertex[1]; + } +} + +_ccd_inline void ccdPtFaceEdges(const ccd_pt_face_t *f, + ccd_pt_edge_t **a, + ccd_pt_edge_t **b, + ccd_pt_edge_t **c) +{ + *a = f->edge[0]; + *b = f->edge[1]; + *c = f->edge[2]; +} + +_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e, + ccd_vec3_t **a, + ccd_vec3_t **b) +{ + *a = &e->vertex[0]->v.v; + *b = &e->vertex[1]->v.v; +} + +_ccd_inline void ccdPtEdgeVertices(const ccd_pt_edge_t *e, + ccd_pt_vertex_t **a, + ccd_pt_vertex_t **b) +{ + *a = e->vertex[0]; + *b = e->vertex[1]; +} + +_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e, + ccd_pt_face_t **f1, + ccd_pt_face_t **f2) +{ + *f1 = e->faces[0]; + *f2 = e->faces[1]; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_POLYTOPE_H__ */ diff --git a/3rdparty/libccd/src/simplex.h b/3rdparty/libccd/src/simplex.h new file mode 100644 index 0000000..8ae09c7 --- /dev/null +++ b/3rdparty/libccd/src/simplex.h @@ -0,0 +1,104 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_SIMPLEX_H__ +#define __CCD_SIMPLEX_H__ + +#include +#include "support.h" + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_simplex_t { + ccd_support_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _ccd_simplex_t ccd_simplex_t; + + +_ccd_inline void ccdSimplexInit(ccd_simplex_t *s); +_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s); +_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s); +_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx); +_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx); + +_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v); +_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a); +_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size); +_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2); + + +/**** INLINES ****/ + +_ccd_inline void ccdSimplexInit(ccd_simplex_t *s) +{ + s->last = -1; +} + +_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s) +{ + return s->last + 1; +} + +_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s) +{ + return ccdSimplexPoint(s, s->last); +} + +_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + ccdSupportCopy(s->ps + s->last, v); +} + +_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a) +{ + ccdSupportCopy(s->ps + pos, a); +} + +_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size) +{ + s->last = size - 1; +} + +_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2) +{ + ccd_support_t supp; + + ccdSupportCopy(&supp, &s->ps[pos1]); + ccdSupportCopy(&s->ps[pos1], &s->ps[pos2]); + ccdSupportCopy(&s->ps[pos2], &supp); +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_SIMPLEX_H__ */ diff --git a/3rdparty/libccd/src/support.c b/3rdparty/libccd/src/support.c new file mode 100644 index 0000000..6c7f1cc --- /dev/null +++ b/3rdparty/libccd/src/support.c @@ -0,0 +1,34 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include "support.h" + +void __ccdSupport(const void *obj1, const void *obj2, + const ccd_vec3_t *_dir, const ccd_t *ccd, + ccd_support_t *supp) +{ + ccd_vec3_t dir; + + ccdVec3Copy(&dir, _dir); + + ccd->support1(obj1, &dir, &supp->v1); + + ccdVec3Scale(&dir, -CCD_ONE); + ccd->support2(obj2, &dir, &supp->v2); + + ccdVec3Sub2(&supp->v, &supp->v1, &supp->v2); +} diff --git a/3rdparty/libccd/src/support.h b/3rdparty/libccd/src/support.h new file mode 100644 index 0000000..3fcc384 --- /dev/null +++ b/3rdparty/libccd/src/support.h @@ -0,0 +1,55 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_SUPPORT_H__ +#define __CCD_SUPPORT_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_support_t { + ccd_vec3_t v; //!< Support point in minkowski sum + ccd_vec3_t v1; //!< Support point in obj1 + ccd_vec3_t v2; //!< Support point in obj2 +}; +typedef struct _ccd_support_t ccd_support_t; + +_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s); + +/** + * Computes support point of obj1 and obj2 in direction dir. + * Support point is returned via supp. + */ +CCD_EXPORT void __ccdSupport(const void *obj1, const void *obj2, + const ccd_vec3_t *dir, const ccd_t *ccd, + ccd_support_t *supp); + + +/**** INLINES ****/ +_ccd_inline void ccdSupportCopy(ccd_support_t *d, const ccd_support_t *s) +{ + *d = *s; +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_SUPPORT_H__ */ diff --git a/3rdparty/libccd/src/testsuites/.gitignore b/3rdparty/libccd/src/testsuites/.gitignore new file mode 100644 index 0000000..3d94fdc --- /dev/null +++ b/3rdparty/libccd/src/testsuites/.gitignore @@ -0,0 +1,6 @@ +regressions/tmp.* +bench-out/* +*.o +test +bench +bench2 diff --git a/3rdparty/libccd/src/testsuites/CMakeLists.txt b/3rdparty/libccd/src/testsuites/CMakeLists.txt new file mode 100644 index 0000000..392cf5e --- /dev/null +++ b/3rdparty/libccd/src/testsuites/CMakeLists.txt @@ -0,0 +1,53 @@ +add_subdirectory(cu) + +set(MAIN_SOURCES + main.c + common.c + common.h + support.c + support.h + vec3.c + vec3.h + polytope.c + polytope.h + boxbox.c + boxbox.h + spheresphere.c + spheresphere.h + cylcyl.c + cylcyl.h + boxcyl.c + boxcyl.h + mpr_boxbox.c + mpr_boxbox.h + mpr_cylcyl.c + mpr_cylcyl.h + mpr_boxcyl.c + mpr_boxcyl.h + support.c + support.h) + +add_executable(main ${MAIN_SOURCES}) +target_link_libraries(main ccd cu) +add_test(NAME main COMMAND main) + + +if(NOT APPLE) + set(BENCH_SOURCES + bench.c + support.c + support.h) + + add_executable(bench ${BENCH_SOURCES}) + target_link_libraries(bench ccd cu) + add_test(NAME bench COMMAND bench) + + set(BENCH2_SOURCES + bench2.c + support.c + support.h) + + add_executable(bench2 ${BENCH2_SOURCES}) + target_link_libraries(bench2 ccd cu) + add_test(NAME bench2 COMMAND bench2) +endif() diff --git a/3rdparty/libccd/src/testsuites/Makefile b/3rdparty/libccd/src/testsuites/Makefile new file mode 100644 index 0000000..3f3d9d0 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/Makefile @@ -0,0 +1,61 @@ +# force some options +DEBUG = yes + +-include ../Makefile.include +CFLAGS += -I./ -I../ -Icu/ +LDFLAGS += -L./ -Lcu/ -lcu -lrt -lm -L../ -lccd + +CHECK_REG=cu/check-regressions +CHECK_TS ?= + +OBJS = common.o support.o vec3.o polytope.o boxbox.o spheresphere.o \ + cylcyl.o boxcyl.o mpr_boxbox.o mpr_cylcyl.o mpr_boxcyl.o +BENCH_OBJS = bench-boxbox.o + + +all: test bench bench2 + +test: cu $(OBJS) main.c + $(CC) $(CFLAGS) -o $@ main.c $(OBJS) $(LDFLAGS) + +bench: cu bench.c support.o + $(CC) $(CFLAGS) -o $@ bench.c support.o $(LDFLAGS) +bench2: cu bench2.c support.o + $(CC) $(CFLAGS) -o $@ bench2.c support.o $(LDFLAGS) + +%.o: %.c %.h + $(CC) $(CFLAGS) -c -o $@ $< +%.o: %.c + $(CC) $(CFLAGS) -c -o $@ $< + +check: all + @echo "" + @echo "----------------------------------------"; + ./test $(CHECK_TS) + @echo "----------------------------------------"; + @echo "Checking regressions:"; + $(PYTHON) $(CHECK_REG) regressions + @echo "" + +check-valgrind: all + valgrind -q --leak-check=full --show-reachable=yes --trace-children=yes \ + --error-limit=no \ + ./test $(CHECK_TS) + +check-valgrind-gen-suppressions: all + valgrind -q --leak-check=full --show-reachable=yes --trace-children=yes \ + --gen-suppressions=all --log-file=out --error-limit=no \ + ./test $(CHECK_TS) + +cu: + $(MAKE) ENABLE_TIMER=yes -C cu/ + +clean: + rm -f *.o + rm -f objs/*.o + rm -f test bench bench2 + rm -f tmp.* + rm -f regressions/tmp.* + +.PHONY: all clean check check-valgrind cu + diff --git a/3rdparty/libccd/src/testsuites/Makefile.am b/3rdparty/libccd/src/testsuites/Makefile.am new file mode 100644 index 0000000..733fed3 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/Makefile.am @@ -0,0 +1,28 @@ +SUBDIRS = cu + +AM_CPPFLAGS = -I $(srcdir)/.. -I $(builddir)/.. -I $(srcdir)/cu + +LDADD = $(builddir)/cu/libcu.la $(builddir)/../libccd.la + + +check_PROGRAMS = test bench bench2 + +test_SOURCES = main.c \ + common.c common.h \ + support.c support.h \ + vec3.c vec3.h \ + polytope.c polytope.h \ + boxbox.c boxbox.h \ + spheresphere.c spheresphere.h \ + cylcyl.c cylcyl.h \ + boxcyl.c boxcyl.h \ + mpr_boxbox.c mpr_boxbox.h \ + mpr_cylcyl.c mpr_cylcyl.h \ + mpr_boxcyl.c mpr_boxcyl.h + +bench_SOURCES = bench.c \ + support.c support.h + +bench2_SOURCES = bench2.c \ + support.c support.h + diff --git a/3rdparty/libccd/src/testsuites/bench.c b/3rdparty/libccd/src/testsuites/bench.c new file mode 100644 index 0000000..8ca770c --- /dev/null +++ b/3rdparty/libccd/src/testsuites/bench.c @@ -0,0 +1,256 @@ +#define CU_ENABLE_TIMER +#include +#include +#include +#include +#include "support.h" + +TEST_SUITES { + TEST_SUITES_CLOSURE +}; + +static int bench_num = 1; +static size_t cycles = 10000; + +static void runBench(const void *o1, const void *o2, const ccd_t *ccd) +{ + ccd_real_t depth; + ccd_vec3_t dir, pos; + size_t i; + const struct timespec *timer; + + cuTimerStart(); + for (i = 0; i < cycles; i++){ + ccdGJKPenetration(o1, o2, ccd, &depth, &dir, &pos); + } + timer = cuTimerStop(); + fprintf(stdout, "%02d: %ld %ld\n", bench_num, + (long)timer->tv_sec, (long)timer->tv_nsec); + fflush(stdout); + + bench_num++; +} + +static void boxbox(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + ccd_vec3_t axis; + ccd_quat_t rot; + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + bench_num = 1; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0., 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 1., 1.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.2; box2.y = 0.5; box2.z = 1.; + box2.x = box2.y = box2.z = 1.; + + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 0., 0.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -1.3, 0., 0.); + + ccdVec3Set(&box2.pos, 0., 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + + fprintf(stdout, "\n----\n\n"); +} + +void cylcyl(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_CYL(cyl1); + CCD_CYL(cyl2); + ccd_vec3_t axis; + + cyl1.radius = 0.35; + cyl1.height = 0.5; + cyl2.radius = 0.5; + cyl2.height = 1.; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0., 0., 0.); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0.567, 1.2, 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, -4.567, 1.2, 0.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + fprintf(stdout, "\n----\n\n"); +} + +void boxcyl(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + ccd_vec3_t axis; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + fprintf(stdout, "\n----\n\n"); +} + +int main(int argc, char *argv[]) +{ + if (argc > 1){ + cycles = atol(argv[1]); + } + + fprintf(stdout, "Cycles: %u\n", (unsigned int)cycles); + fprintf(stdout, "\n"); + + boxbox(); + cylcyl(); + boxcyl(); + + return 0; +} diff --git a/3rdparty/libccd/src/testsuites/bench2.c b/3rdparty/libccd/src/testsuites/bench2.c new file mode 100644 index 0000000..465acf3 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/bench2.c @@ -0,0 +1,262 @@ +#define CU_ENABLE_TIMER +#include +#include +#include +#include +#include "support.h" + +TEST_SUITES { + TEST_SUITES_CLOSURE +}; + +static int bench_num = 1; +static size_t cycles = 10000; + +static void runBench(const void *o1, const void *o2, const ccd_t *ccd) +{ + ccd_real_t depth; + ccd_vec3_t dir, pos; + size_t i; + const struct timespec *timer; + + cuTimerStart(); + for (i = 0; i < cycles; i++){ + ccdMPRPenetration(o1, o2, ccd, &depth, &dir, &pos); + } + timer = cuTimerStop(); + fprintf(stdout, "%02d: %ld %ld\n", bench_num, + (long)timer->tv_sec, (long)timer->tv_nsec); + fflush(stdout); + + bench_num++; +} + +static void boxbox(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + ccd_vec3_t axis; + ccd_quat_t rot; + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + bench_num = 1; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0., 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 1., 1.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.2; box2.y = 0.5; box2.z = 1.; + box2.x = box2.y = box2.z = 1.; + + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 0., 0.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -1.3, 0., 0.); + + ccdVec3Set(&box2.pos, 0., 0., 0.); + runBench(&box1, &box2, &ccd); + runBench(&box2, &box1, &ccd); + + + fprintf(stdout, "\n----\n\n"); +} + +void cylcyl(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_CYL(cyl1); + CCD_CYL(cyl2); + ccd_vec3_t axis; + + cyl1.radius = 0.35; + cyl1.height = 0.5; + cyl2.radius = 0.5; + cyl2.height = 1.; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0., 0., 0.); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, 0.567, 1.2, 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + ccdVec3Set(&axis, -4.567, 1.2, 0.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + runBench(&cyl1, &cyl2, &ccd); + runBench(&cyl2, &cyl1, &ccd); + + fprintf(stdout, "\n----\n\n"); +} + +void boxcyl(void) +{ + fprintf(stdout, "%s:\n", __func__); + + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + ccd_vec3_t axis; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + runBench(&box, &cyl, &ccd); + runBench(&cyl, &box, &ccd); + + fprintf(stdout, "\n----\n\n"); +} + +int main(int argc, char *argv[]) +{ + if (argc > 1){ + cycles = atol(argv[1]); + } + + fprintf(stdout, "Cycles: %u\n", (unsigned int)cycles); + fprintf(stdout, "\n"); + + boxbox(); + cylcyl(); + boxcyl(); + + return 0; +} diff --git a/3rdparty/libccd/src/testsuites/boxbox.c b/3rdparty/libccd/src/testsuites/boxbox.c new file mode 100644 index 0000000..314d134 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/boxbox.c @@ -0,0 +1,466 @@ +#include +#include + +#include +#include "support.h" +#include "../dbg.h" +#include "common.h" + + +TEST(boxboxSetUp) +{ +} + +TEST(boxboxTearDown) +{ +} + +TEST(boxboxAlignedX) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + //ccd.max_iterations = 20; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } + + + box1.x = 0.1; + box1.y = 0.2; + box1.z = 0.1; + box2.x = 0.2; + box2.y = 0.1; + box2.z = 0.2; + + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.01; + } + + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., -0.1, 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } +} + +TEST(boxboxAlignedY) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, 0., -5., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[1] += 0.1; + } +} + +TEST(boxboxAlignedZ) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, 0., 0., -5.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[2] += 0.1; + } +} + + +TEST(boxboxRot) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t axis; + ccd_real_t angle; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., 0.5, 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i < 33 || i > 67){ + assertFalse(res); + }else if (i != 33 && i != 67){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } + + box1.x = 1; + box1.y = 1; + box1.z = 1; + box2.x = 1; + box2.y = 1; + box2.z = 1; + + ccdVec3Set(&box1.pos, -1.01, 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + + ccdVec3Set(&axis, 0., 1., 0.); + angle = 0.; + for (i = 0; i < 30; i++){ + res = ccdGJKIntersect(&box1, &box2, &ccd); + + if (i != 0 && i != 10 && i != 20){ + assertTrue(res); + }else{ + assertFalse(res); + } + + angle += M_PI / 20.; + ccdQuatSetAngleAxis(&box1.quat, angle, &axis); + } + +} + + + +static void pConf(ccd_box_t *box1, ccd_box_t *box2, const ccd_vec3_t *v) +{ + fprintf(stdout, "# box1.pos: [%lf %lf %lf]\n", + ccdVec3X(&box1->pos), ccdVec3Y(&box1->pos), ccdVec3Z(&box1->pos)); + fprintf(stdout, "# box1->quat: [%lf %lf %lf %lf]\n", + box1->quat.q[0], box1->quat.q[1], box1->quat.q[2], box1->quat.q[3]); + fprintf(stdout, "# box2->pos: [%lf %lf %lf]\n", + ccdVec3X(&box2->pos), ccdVec3Y(&box2->pos), ccdVec3Z(&box2->pos)); + fprintf(stdout, "# box2->quat: [%lf %lf %lf %lf]\n", + box2->quat.q[0], box2->quat.q[1], box2->quat.q[2], box2->quat.q[3]); + fprintf(stdout, "# sep: [%lf %lf %lf]\n", + ccdVec3X(v), ccdVec3Y(v), ccdVec3Z(v)); + fprintf(stdout, "#\n"); +} + +TEST(boxboxSeparate) +{ + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t sep, expsep, expsep2, axis; + + fprintf(stderr, "\n\n\n---- boxboxSeparate ----\n\n\n"); + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.2); + res = ccdGJKIntersect(&box1, &box2, &ccd); + assertTrue(res); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0.25, 0., 0.); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + ccdVec3Scale(&sep, -1.); + ccdVec3Add(&box1.pos, &sep); + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., 0.); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., -0.25); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0., 0., 0.); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., 1.); + ccdVec3Set(&expsep2, 0., 0., -1.); + assertTrue(ccdVec3Eq(&sep, &expsep) || ccdVec3Eq(&sep, &expsep2)); + + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + pConf(&box1, &box2, &sep); + + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + pConf(&box1, &box2, &sep); +} + + +#define TOSVT() \ + svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&box2.pos, &dir); \ + svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos) + +TEST(boxboxPenetration) +{ + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t axis; + ccd_quat_t rot; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + fprintf(stderr, "\n\n\n---- boxboxPenetration ----\n\n\n"); + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + ccdVec3Set(&box2.pos, 0.1, 0., 0.); + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); <<< + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0.1, 0., 0.1); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&box2.pos, 0.1, 0., 0.); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 1., 1.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 7"); + //TOSVT(); <<< + + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.2; box2.y = 0.5; box2.z = 1.; + box2.x = box2.y = box2.z = 1.; + + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 0., 0.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -1.3, 0., 0.); + + ccdVec3Set(&box2.pos, 0., 0., 0.); + + res = ccdGJKPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 8"); + //TOSVT(); +} diff --git a/3rdparty/libccd/src/testsuites/boxbox.h b/3rdparty/libccd/src/testsuites/boxbox.h new file mode 100644 index 0000000..8127c7c --- /dev/null +++ b/3rdparty/libccd/src/testsuites/boxbox.h @@ -0,0 +1,32 @@ +#ifndef BOX_BOX +#define BOX_BOX + +#include + +TEST(boxboxSetUp); +TEST(boxboxTearDown); + +TEST(boxboxAlignedX); +TEST(boxboxAlignedY); +TEST(boxboxAlignedZ); + +TEST(boxboxRot); + +TEST(boxboxSeparate); +TEST(boxboxPenetration); + +TEST_SUITE(TSBoxBox) { + TEST_ADD(boxboxSetUp), + + TEST_ADD(boxboxAlignedX), + TEST_ADD(boxboxAlignedY), + TEST_ADD(boxboxAlignedZ), + TEST_ADD(boxboxRot), + TEST_ADD(boxboxSeparate), + TEST_ADD(boxboxPenetration), + + TEST_ADD(boxboxTearDown), + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/boxcyl.c b/3rdparty/libccd/src/testsuites/boxcyl.c new file mode 100644 index 0000000..f948e90 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/boxcyl.c @@ -0,0 +1,162 @@ +#include +#include +#include "common.h" +#include "support.h" + + +#define TOSVT() \ + svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&cyl.pos, &dir); \ + svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos) + + +TEST(boxcylIntersect) +{ + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + int res; + ccd_vec3_t axis; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + ccdVec3Set(&cyl.pos, 0.1, 0., 0.); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + res = ccdGJKIntersect(&box, &cyl, &ccd); + assertTrue(res); +} + + +TEST(boxcylPenEPA) +{ + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + int res; + ccd_vec3_t axis; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + ccdVec3Set(&cyl.pos, 0.1, 0., 0.); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); <<< + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 7"); + //TOSVT(); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + res = ccdGJKPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 8"); + //TOSVT(); +} + diff --git a/3rdparty/libccd/src/testsuites/boxcyl.h b/3rdparty/libccd/src/testsuites/boxcyl.h new file mode 100644 index 0000000..3d348d9 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/boxcyl.h @@ -0,0 +1,16 @@ +#ifndef TEST_BOXCYL_H +#define TEST_BOXCYL_H + +#include + +TEST(boxcylIntersect); +TEST(boxcylPenEPA); + +TEST_SUITE(TSBoxCyl){ + TEST_ADD(boxcylIntersect), + TEST_ADD(boxcylPenEPA), + + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/common.c b/3rdparty/libccd/src/testsuites/common.c new file mode 100644 index 0000000..e0ddd2d --- /dev/null +++ b/3rdparty/libccd/src/testsuites/common.c @@ -0,0 +1,174 @@ +#include +#include +#include "common.h" +#include "support.h" + +static void svtCyl(ccd_cyl_t *c, FILE *out, const char *color, const char *name) +{ + ccd_vec3_t v[32]; + ccd_quat_t rot; + ccd_vec3_t axis, vpos, vpos2; + ccd_real_t angle, x, y; + int i; + + ccdVec3Set(&axis, 0., 0., 1.); + ccdVec3Set(&vpos, 0., c->radius, 0.); + angle = 0.; + for (i = 0; i < 16; i++){ + angle = (ccd_real_t)i * (2. * M_PI / 16.); + + ccdQuatSetAngleAxis(&rot, angle, &axis); + ccdVec3Copy(&vpos2, &vpos); + ccdQuatRotVec(&vpos2, &rot); + x = ccdVec3X(&vpos2); + y = ccdVec3Y(&vpos2); + + ccdVec3Set(&v[i], x, y, c->height / 2.); + ccdVec3Set(&v[i + 16], x, y, -c->height / 2.); + } + + for (i = 0; i < 32; i++){ + ccdQuatRotVec(&v[i], &c->quat); + ccdVec3Add(&v[i], &c->pos); + } + + fprintf(out, "-----\n"); + if (name) + fprintf(out, "Name: %s\n", name); + + fprintf(out, "Face color: %s\n", color); + fprintf(out, "Edge color: %s\n", color); + fprintf(out, "Point color: %s\n", color); + fprintf(out, "Points:\n"); + for (i = 0; i < 32; i++){ + fprintf(out, "%lf %lf %lf\n", ccdVec3X(&v[i]), ccdVec3Y(&v[i]), ccdVec3Z(&v[i])); + } + + fprintf(out, "Edges:\n"); + fprintf(out, "0 16\n"); + fprintf(out, "0 31\n"); + for (i = 1; i < 16; i++){ + fprintf(out, "0 %d\n", i); + fprintf(out, "16 %d\n", i + 16); + if (i != 0){ + fprintf(out, "%d %d\n", i - 1, i); + fprintf(out, "%d %d\n", i + 16 - 1, i + 16); + } + + fprintf(out, "%d %d\n", i, i + 16); + fprintf(out, "%d %d\n", i, i + 16 - 1); + } + + fprintf(out, "Faces:\n"); + for (i = 2; i < 16; i++){ + fprintf(out, "0 %d %d\n", i, i -1); + fprintf(out, "16 %d %d\n", i + 16, i + 16 -1); + + } + fprintf(out, "0 16 31\n"); + fprintf(out, "0 31 15\n"); + for (i = 1; i < 16; i++){ + fprintf(out, "%d %d %d\n", i, i + 16, i + 16 - 1); + fprintf(out, "%d %d %d\n", i, i + 16 - 1, i - 1); + } + fprintf(out, "-----\n"); +} + +static void svtBox(ccd_box_t *b, FILE *out, const char *color, const char *name) +{ + ccd_vec3_t v[8]; + size_t i; + + ccdVec3Set(&v[0], b->x * 0.5, b->y * 0.5, b->z * 0.5); + ccdVec3Set(&v[1], b->x * 0.5, b->y * -0.5, b->z * 0.5); + ccdVec3Set(&v[2], b->x * 0.5, b->y * 0.5, b->z * -0.5); + ccdVec3Set(&v[3], b->x * 0.5, b->y * -0.5, b->z * -0.5); + ccdVec3Set(&v[4], b->x * -0.5, b->y * 0.5, b->z * 0.5); + ccdVec3Set(&v[5], b->x * -0.5, b->y * -0.5, b->z * 0.5); + ccdVec3Set(&v[6], b->x * -0.5, b->y * 0.5, b->z * -0.5); + ccdVec3Set(&v[7], b->x * -0.5, b->y * -0.5, b->z * -0.5); + + for (i = 0; i < 8; i++){ + ccdQuatRotVec(&v[i], &b->quat); + ccdVec3Add(&v[i], &b->pos); + } + + fprintf(out, "-----\n"); + if (name) + fprintf(out, "Name: %s\n", name); + fprintf(out, "Face color: %s\n", color); + fprintf(out, "Edge color: %s\n", color); + fprintf(out, "Point color: %s\n", color); + fprintf(out, "Points:\n"); + for (i = 0; i < 8; i++){ + fprintf(out, "%lf %lf %lf\n", ccdVec3X(&v[i]), ccdVec3Y(&v[i]), ccdVec3Z(&v[i])); + } + + fprintf(out, "Edges:\n"); + fprintf(out, "0 1\n 0 2\n2 3\n3 1\n1 2\n6 2\n1 7\n1 5\n"); + fprintf(out, "5 0\n0 4\n4 2\n6 4\n6 5\n5 7\n6 7\n7 2\n7 3\n4 5\n"); + + fprintf(out, "Faces:\n"); + fprintf(out, "0 2 1\n1 2 3\n6 2 4\n4 2 0\n4 0 5\n5 0 1\n"); + fprintf(out, "5 1 7\n7 1 3\n6 4 5\n6 5 7\n2 6 7\n2 7 3\n"); + fprintf(out, "-----\n"); +} + + +void svtObj(void *_o, FILE *out, const char *color, const char *name) +{ + ccd_obj_t *o = (ccd_obj_t *)_o; + + if (o->type == CCD_OBJ_CYL){ + svtCyl((ccd_cyl_t *)o, out, color, name); + }else if (o->type == CCD_OBJ_BOX){ + svtBox((ccd_box_t *)o, out, color, name); + } +} + +void svtObjPen(void *o1, void *o2, + FILE *out, const char *name, + ccd_real_t depth, const ccd_vec3_t *dir, const ccd_vec3_t *pos) +{ + ccd_vec3_t sep; + char oname[500]; + + ccdVec3Copy(&sep, dir); + ccdVec3Scale(&sep, depth); + ccdVec3Add(&sep, pos); + + fprintf(out, "------\n"); + if (name) + fprintf(out, "Name: %s\n", name); + fprintf(out, "Point color: 0.1 0.1 0.9\n"); + fprintf(out, "Points:\n%lf %lf %lf\n", ccdVec3X(pos), ccdVec3Y(pos), ccdVec3Z(pos)); + fprintf(out, "------\n"); + fprintf(out, "Point color: 0.1 0.9 0.9\n"); + fprintf(out, "Edge color: 0.1 0.9 0.9\n"); + fprintf(out, "Points:\n%lf %lf %lf\n", ccdVec3X(pos), ccdVec3Y(pos), ccdVec3Z(pos)); + fprintf(out, "%lf %lf %lf\n", ccdVec3X(&sep), ccdVec3Y(&sep), ccdVec3Z(&sep)); + fprintf(out, "Edges: 0 1\n"); + + oname[0] = 0x0; + if (name) + sprintf(oname, "%s o1", name); + svtObj(o1, out, "0.9 0.1 0.1", oname); + + oname[0] = 0x0; + if (name) + sprintf(oname, "%s o1", name); + svtObj(o2, out, "0.1 0.9 0.1", oname); +} + + +void recPen(ccd_real_t depth, const ccd_vec3_t *dir, const ccd_vec3_t *pos, + FILE *out, const char *note) +{ + if (!note) + note = ""; + + fprintf(out, "# %s: depth: %lf\n", note, depth); + fprintf(out, "# %s: dir: [%lf %lf %lf]\n", note, ccdVec3X(dir), ccdVec3Y(dir), ccdVec3Z(dir)); + fprintf(out, "# %s: pos: [%lf %lf %lf]\n", note, ccdVec3X(pos), ccdVec3Y(pos), ccdVec3Z(pos)); + fprintf(out, "#\n"); +} diff --git a/3rdparty/libccd/src/testsuites/common.h b/3rdparty/libccd/src/testsuites/common.h new file mode 100644 index 0000000..a4de4c2 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/common.h @@ -0,0 +1,14 @@ +#ifndef TEST_COMMON +#define TEST_COMMON + +#include +#include + +void svtObj(void *o, FILE *out, const char *color, const char *name); +void svtObjPen(void *o1, void *o2, + FILE *out, const char *name, + ccd_real_t depth, const ccd_vec3_t *dir, const ccd_vec3_t *pos); +void recPen(ccd_real_t depth, const ccd_vec3_t *dir, const ccd_vec3_t *pos, + FILE *out, const char *note); + +#endif diff --git a/3rdparty/libccd/src/testsuites/cu/.dir b/3rdparty/libccd/src/testsuites/cu/.dir new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/cu/.gitignore b/3rdparty/libccd/src/testsuites/cu/.gitignore new file mode 100644 index 0000000..3d94568 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/.gitignore @@ -0,0 +1,6 @@ +*~ +*.o +*.a +tmp.* +test + diff --git a/3rdparty/libccd/src/testsuites/cu/CMakeLists.txt b/3rdparty/libccd/src/testsuites/cu/CMakeLists.txt new file mode 100644 index 0000000..7a4ccf0 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/CMakeLists.txt @@ -0,0 +1,17 @@ +set(CU_SOURCES + cu.c + cu.h) + +add_library(cu STATIC ${CU_SOURCES}) + +if(NOT APPLE) + target_compile_definitions(cu PUBLIC CU_ENABLE_TIMER) + find_library(LIBRT_LIBRARY NAMES rt) + if(NOT LIBRT_LIBRARY) + message(FATAL_ERROR "Could NOT find required library librt") + endif() + target_link_libraries(cu "${LIBRT_LIBRARY}") +endif() + +get_filename_component(CU_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" DIRECTORY) +target_include_directories(cu PUBLIC "${CU_INCLUDE_DIR}") diff --git a/3rdparty/libccd/src/testsuites/cu/COPYING b/3rdparty/libccd/src/testsuites/cu/COPYING new file mode 100644 index 0000000..94a9ed0 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/COPYING @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/3rdparty/libccd/src/testsuites/cu/COPYING.LESSER b/3rdparty/libccd/src/testsuites/cu/COPYING.LESSER new file mode 100644 index 0000000..fc8a5de --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/COPYING.LESSER @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/3rdparty/libccd/src/testsuites/cu/Makefile b/3rdparty/libccd/src/testsuites/cu/Makefile new file mode 100644 index 0000000..6b84abd --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/Makefile @@ -0,0 +1,49 @@ +CC ?= gcc +CFLAGS = -g -Wall -pedantic + +ENABLE_TIMER ?= no + +ifeq '$(ENABLE_TIMER)' 'yes' + CFLAGS += -DCU_ENABLE_TIMER +endif + +TARGETS = libcu.a + +TEST_OBJS = test.o test2.o + +all: $(TARGETS) + +libcu.a: cu.o + ar cr $@ $^ + ranlib $@ +cu.o: cu.c cu.h + $(CC) $(CFLAGS) -c -o $@ $< + +test: $(TEST_OBJS) libcu.a + $(CC) $(CFLAGS) -o $@ $(TEST_OBJS) -L./ -lcu +test-segfault: test-segfault.c libcu.a + $(CC) $(CFLAGS) -o $@ $^ -L./ -lcu + +%.o: %.c + $(CC) $(CFLAGS) -c -o $@ $< + +check: test test-segfault + mkdir -p regressions + touch regressions/testSuiteName{,2}.{out,err} + touch regressions/testSuiteTest2.{out,err} + -./test + -cd regressions && ../check-regressions + @echo "" + @echo "======= SEGFAULT: =========" + @echo "" + -./test-segfault + +clean: + rm -f *.o + rm -f test + rm -f test-segfault + rm -f $(TARGETS) + rm -f tmp.* + rm -rf regressions + +.PHONY: all clean check diff --git a/3rdparty/libccd/src/testsuites/cu/Makefile.am b/3rdparty/libccd/src/testsuites/cu/Makefile.am new file mode 100644 index 0000000..66d6daf --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/Makefile.am @@ -0,0 +1,6 @@ +AM_CPPFLAGS = -DCU_ENABLE_TIMER + +check_LTLIBRARIES = libcu.la + +libcu_la_SOURCES = cu.c cu.h + diff --git a/3rdparty/libccd/src/testsuites/cu/check-regressions b/3rdparty/libccd/src/testsuites/cu/check-regressions new file mode 100755 index 0000000..07aec7d --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/check-regressions @@ -0,0 +1,413 @@ +#!/usr/bin/python +## +# CU - C unit testing framework +# --------------------------------- +# Copyright (c)2007,2008 Daniel Fiser +# +# +# This file is part of CU. +# +# CU is free software; you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as +# published by the Free Software Foundation; either version 3 of +# the License, or (at your option) any later version. +# +# CU is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . +# + +from subprocess import Popen, PIPE +import os +import re +import sys +import math +from getopt import gnu_getopt, GetoptError + +EPS = 0.6 +BASE_DIR = "." +MAX_DIFF_LINES = 20 +EXACT = False + +PROGRESS_ON = True +MSG_BASE = "" + +class Hunk: + """ This class represents one hunk from diff. """ + + def __init__(self): + self.added = [] + self.deleted = [] + self.lines = [] + + # to identify lines with floating point numbers + self.re_is_num = re.compile("^.*[0-9].*$") + + # pattern to match floating point number + self.num_pattern = r"-?(?:(?:[0-9]+(?:\.[0-9]*)?)|(?:\.[0-9]+))(?:[eE]-?[0-9]+)?" + self.re_num = re.compile(self.num_pattern) + + def numLines(self): + return len(self.lines) + def numLinesAdded(self): + return len(self.added) + def numLinesDeleted(self): + return len(self.deleted) + + def addLineAdded(self, line): + self.added.append(line) + def addLineDeleted(self, line): + self.deleted.append(line) + def addLine(self, line): + self.lines.append(line) + + def getLines(self): + return self.lines + def getLinesAdded(self): + return self.added + def getLinesDeleted(self): + return self.deleted + + def __eq(self, num1, num2): + """ Returns True if num1 equals to num2 with respect to EPS + (defined above) """ + return math.fabs(num1 - num2) < EPS + + def checkFloats(self): + """ This method try to check if only difference between added and + deleted lines of this hunk is different precission of floating + point numbers + """ + + # If number of added and deleted lines differs, then there is more + # differences that precission of floating point numbers + if self.numLinesAdded() != self.numLinesDeleted(): + return False + + for i in xrange(0, self.numLinesAdded()): + # if any line does not contain number - return False because + # there must be more differences than in numbers + if not self.re_is_num.match(self.added[i]) \ + or not self.re_is_num.match(self.deleted[i]): + return False + + line1 = self.added[i] + line2 = self.deleted[i] + + # Extract all floating point numbers from each line + nums1 = self.re_num.findall(line1) + nums2 = self.re_num.findall(line2) + # and remove all empty strings + nums1 = filter(lambda x: len(x) > 0, nums1) + nums2 = filter(lambda x: len(x) > 0, nums2) + + # if length of list nums1 does not equal to length of nums2 + # return False + if len(nums1) != len(nums2): + return False + + # iterate trough all numbers + for j in xrange(0, len(nums1)): + # if numbers do not equal to each other return False + if not self.__eq(float(nums1[j]), float(nums2[j])): + return False + + # compare the rest of lines + line1 = self.re_num.sub("", line1) + line2 = self.re_num.sub("", line2) + if line1 != line2: + return False + + # If it does not fail anywhere, added and deleted lines must be + # same + return True + + +class Diff: + """ Represents whole diff. """ + + def __init__(self): + self.hunks = [] + self.lines = 0 + self.omitted_lines = 0 + + def addHunk(self, hunk): + self.hunks.append(hunk) + self.lines += hunk.numLines() + + def numLines(self): + return self.lines + def numOmittedLines(self): + return self.omitted_lines + + def getHunks(self): + return self.hunks + def numHunks(self): + return len(self.hunks) + + def checkFloats(self): + """ Will call method checkFloats on each hunk """ + hks = self.hunks[:] + self.hunks = [] + self.lines = 0 + for h in hks: + if not h.checkFloats(): + self.hunks.append(h) + self.lines += h.numLines() + else: + self.omitted_lines += h.numLines() + + + +class Parser: + def __init__(self, fin): + self.fin = fin + self.line = "" + self.diff = Diff() + self.cur_hunk = None + + # to recognize beginning of hunk: + self.re_hunk = re.compile(r"^[0-9]*(,[0-9]*){0,1}[a-zA-Z]?[0-9]*(,[0-9]*){0,1}$") + + self.re_added = re.compile(r"^> (.*)$") + self.re_deleted = re.compile(r"^< (.*)$") + + def __readNextLine(self): + self.line = self.fin.readline() + if len(self.line) == 0: + return False + return True + + def parse(self): + global PROGRESS_ON + global MSG_BASE + + num_lines = 0 + while self.__readNextLine(): + # beggining of hunk + if self.re_hunk.match(self.line): + if self.cur_hunk is not None: + self.diff.addHunk(self.cur_hunk) + self.cur_hunk = Hunk() + self.cur_hunk.addLine(self.line) + + # line added + match = self.re_added.match(self.line) + if match is not None: + self.cur_hunk.addLine(self.line) + self.cur_hunk.addLineAdded(match.group(1)) + + # line deleted + match = self.re_deleted.match(self.line) + if match is not None: + self.cur_hunk.addLine(self.line) + self.cur_hunk.addLineDeleted(match.group(1)) + + num_lines += 1 + + if PROGRESS_ON and num_lines % 50 == 0: + print MSG_BASE, "[ %08d ]" % num_lines, "\r", + sys.stdout.flush() + + # last push to list of hunks + if self.cur_hunk is not None: + self.diff.addHunk(self.cur_hunk) + + if PROGRESS_ON: + print MSG_BASE, " ", "\r", + sys.stdout.flush() + + def getDiff(self): + return self.diff + + +def regressionFilesInDir(): + """ Returns sorted list of pairs of filenames where first name in pair + is tmp. file and second corresponding file with saved regressions. + """ + + re_tmp_out_file = re.compile(r"tmp\.(.*\.out)") + re_tmp_err_file = re.compile(r"tmp\.(.*\.err)") + files = [] + + all_files = os.listdir(".") + all_files.sort() + for file in all_files: + res = re_tmp_out_file.match(file) + if res is not None: + fname = res.group(1) + tmp = [file, ""] + for file2 in all_files: + if file2 == fname: + tmp = [file, file2,] + break + files.append(tmp) + + res = re_tmp_err_file.match(file) + if res is not None: + fname = res.group(1) + tmp = [file, ""] + for file2 in all_files: + if file2 == fname: + tmp = [file, file2,] + break + files.append(tmp) + + return files + + +def MSG(str = "", wait = False): + if wait: + print str, + else: + print str +def MSGOK(prestr = "", str = "", poststr = ""): + print prestr, "\033[0;32m" + str + "\033[0;0m", poststr +def MSGFAIL(prestr = "", str = "", poststr = ""): + print prestr, "\033[0;31m" + str + "\033[0;0m", poststr +def MSGINFO(prestr = "", str = "", poststr = ""): + print prestr, "\033[0;33m" + str + "\033[0;0m", poststr +def dumpLines(lines, prefix = "", wait = False, max_lines = -1): + line_num = 0 + if wait: + for line in lines: + print prefix, line, + line_num += 1 + if max_lines >= 0 and line_num > max_lines: + break + else: + for line in lines: + print prefix, line + line_num += 1 + if max_lines >= 0 and line_num > max_lines: + break + +def main(files): + global MSG_BASE + + # As first compute length of columns + len1 = 0 + len2 = 0 + for filenames in files: + if len(filenames[0]) > len1: + len1 = len(filenames[0]) + if len(filenames[1]) > len2: + len2 = len(filenames[1]) + + for filenames in files: + if len(filenames[1]) == 0: + MSGFAIL("", "===", "Can't compare %s %s, bacause %s does not exist!" % \ + (filenames[0], filenames[0][4:], filenames[0][4:])) + continue + + cmd = ["diff", filenames[0], filenames[1]] + MSG_BASE = "Comparing %s and %s" % \ + (filenames[0].ljust(len1) ,filenames[1].ljust(len2)) + if not PROGRESS_ON: + print MSG_BASE, + sys.stdout.flush() + + pipe = Popen(cmd, stdout=PIPE) + parser = Parser(pipe.stdout) + parser.parse() + diff = parser.getDiff() + if not EXACT: + diff.checkFloats() + + if PROGRESS_ON: + print MSG_BASE, + + if diff.numHunks() == 0: + MSGOK(" [", "OK", "]") + if diff.numOmittedLines() > 0: + MSGINFO(" -->", str(diff.numOmittedLines()) + " lines from diff omitted") + else: + MSGFAIL(" [", "FAILED", "]") + if diff.numOmittedLines() > 0: + MSGINFO(" -->", str(diff.numOmittedLines()) + " lines from diff omitted") + MSGINFO(" -->", "Diff has " + str(diff.numLines()) + " lines") + + if diff.numLines() <= MAX_DIFF_LINES: + MSGINFO(" -->", "Diff:") + for h in diff.getHunks(): + dumpLines(h.getLines(), " |", True) + else: + MSGINFO(" -->", "Printing only first " + str(MAX_DIFF_LINES) + " lines:") + lines = [] + for h in diff.getHunks(): + lines += h.getLines() + if len(lines) > MAX_DIFF_LINES: + break; + dumpLines(lines, " |", True, MAX_DIFF_LINES) + +def usage(): + print "Usage: " + sys.argv[0] + " [ OPTIONS ] [ directory, [ directory, [ ... ] ] ]" + print "" + print " OPTIONS:" + print " --help / -h none Print this help" + print " --exact / -e none Switch do exact comparasion of files" + print " --not-exact / -n none Switch do non exact comparasion of files (default behaviour)" + print " --max-diff-lines int Maximum of lines of diff which can be printed (default " + str(MAX_DIFF_LINES) + ")" + print " --eps float Precision of floating point numbers (epsilon) (default " + str(EPS) + ")" + print " --no-progress none Turn off progress bar" + print " --progress none Turn on progress bar (default)" + print "" + print " This program is able to compare files with regressions generated by CU testsuites." + print " You can specify directories which are to be searched for regression files." + print " In non exact copmarasion mode (which is default), this program tries to compare" + print " floating point numbers in files with respect to specified precision (see --eps) and" + print " those lines which differ only in precission of floating point numbers are omitted." + print "" + sys.exit(-1) + + + +# Init: + +# Set up base dir +BASE_DIR = os.getcwd() + +# Parse command line options: +optlist, args = gnu_getopt(sys.argv[1:], + "hen", + ["help", "max-diff-lines=", "eps=", \ + "exact", "not-exact", \ + "no-progress", "progress"]) +for opt in optlist: + if opt[0] == "--help" or opt[0] == "-h": + usage() + if opt[0] == "--exact" or opt[0] == "-e": + EXACT = True + if opt[0] == "--not-exact" or opt[0] == "-n": + EXACT = False + if opt[0] == "--max-diff-lines": + MAX_DIFF_LINES = int(opt[1]) + if opt[0] == "--eps": + EPS = float(opt[1]) + if opt[0] == "--no-progress": + PROGRESS_ON = False + if opt[0] == "--progress": + PROGRESS_ON = True + +if len(args) == 0: + files = regressionFilesInDir() + main(files) +else: + for dir in args: + os.chdir(BASE_DIR) + + MSGINFO() + MSGINFO("", "Processing directory '" + dir + "':") + MSGINFO() + try: + os.chdir(dir) + except: + MSGFAIL(" -->", "Directory '" + dir + "' does not exist.") + files = regressionFilesInDir() + main(files) + +sys.exit(0) diff --git a/3rdparty/libccd/src/testsuites/cu/cu.c b/3rdparty/libccd/src/testsuites/cu/cu.c new file mode 100644 index 0000000..7449b4b --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/cu.c @@ -0,0 +1,387 @@ +/*** + * CU - C unit testing framework + * --------------------------------- + * Copyright (c)2007,2008,2009 Daniel Fiser + * + * + * This file is part of CU. + * + * CU is free software; you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation; either version 3 of + * the License, or (at your option) any later version. + * + * CU is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include + +#include "cu.h" + +/** Declared here, because I didn't find header file where it is declared */ +char *strsignal(int sig); + +const char *cu_current_test; +const char *cu_current_test_suite; +int cu_success_test_suites = 0; +int cu_fail_test_suites = 0; +int cu_success_tests = 0; +int cu_fail_tests = 0; +int cu_success_checks = 0; +int cu_fail_checks = 0; + +char cu_out_prefix[CU_OUT_PREFIX_LENGTH+1] = ""; + + +/* globally used file descriptor for reading/writing messages */ +int fd; + +/* indicate if test was failed */ +int test_failed; + +/* codes of messages */ +#define CHECK_FAILED '0' +#define CHECK_SUCCEED '1' +#define TEST_FAILED '2' +#define TEST_SUCCEED '3' +#define TEST_SUITE_FAILED '4' +#define TEST_SUITE_SUCCEED '5' +#define END '6' +#define TEST_NAME '7' + +/* predefined messages */ +#define MSG_CHECK_SUCCEED write(fd, "1\n", 2) +#define MSG_TEST_FAILED write(fd, "2\n", 2) +#define MSG_TEST_SUCCEED write(fd, "3\n", 2) +#define MSG_TEST_SUITE_FAILED write(fd, "4\n", 2) +#define MSG_TEST_SUITE_SUCCEED write(fd, "5\n", 2) +#define MSG_END write(fd, "6\n", 2) + +/* length of buffers */ +#define BUF_LEN 1000 +#define MSGBUF_LEN 300 + + +static void redirect_out_err(const char *testName); +static void close_out_err(void); +static void run_test_suite(const char *ts_name, cu_test_suite_t *ts); +static void receive_messages(void); + +static void cu_run_fork(const char *ts_name, cu_test_suite_t *test_suite); +static void cu_print_results(void); + +void cu_run(int argc, char *argv[]) +{ + cu_test_suites_t *tss; + int i; + char found = 0; + + if (argc > 1){ + for (i=1; i < argc; i++){ + tss = cu_test_suites; + while (tss->name != NULL && tss->test_suite != NULL){ + if (strcmp(argv[i], tss->name) == 0){ + found = 1; + cu_run_fork(tss->name, tss->test_suite); + break; + } + tss++; + } + + if (tss->name == NULL || tss->test_suite == NULL){ + fprintf(stderr, "ERROR: Could not find test suite '%s'\n", argv[i]); + } + } + + if (found == 1) + cu_print_results(); + + }else{ + tss = cu_test_suites; + while (tss->name != NULL && tss->test_suite != NULL){ + cu_run_fork(tss->name, tss->test_suite); + tss++; + } + cu_print_results(); + } + + +} + +static void cu_run_fork(const char *ts_name, cu_test_suite_t *ts) +{ + int pipefd[2]; + int pid; + int status; + + if (pipe(pipefd) == -1){ + perror("Pipe error"); + exit(-1); + } + + fprintf(stdout, " -> %s [IN PROGESS]\n", ts_name); + fflush(stdout); + + pid = fork(); + if (pid < 0){ + perror("Fork error"); + exit(-1); + } + + if (pid == 0){ + /* close read end of pipe */ + close(pipefd[0]); + + fd = pipefd[1]; + + /* run testsuite, messages go to fd */ + run_test_suite(ts_name, ts); + + MSG_END; + close(fd); + + /* stop process where running testsuite */ + exit(0); + }else{ + /* close write end of pipe */ + close(pipefd[1]); + + fd = pipefd[0]; + + /* receive and interpret all messages */ + receive_messages(); + + /* wait for children */ + wait(&status); + if (!WIFEXITED(status)){ /* if child process ends up abnormaly */ + if (WIFSIGNALED(status)){ + fprintf(stdout, "Test suite was terminated by signal %d (%s).\n", + WTERMSIG(status), strsignal(WTERMSIG(status))); + }else{ + fprintf(stdout, "Test suite terminated abnormaly!\n"); + } + + /* mark this test suite as failed, because was terminated + * prematurely */ + cu_fail_test_suites++; + } + + close(fd); + + fprintf(stdout, " -> %s [DONE]\n\n", ts_name); + fflush(stdout); + } + +} + +static void run_test_suite(const char *ts_name, cu_test_suite_t *ts) +{ + int test_suite_failed = 0; + char buffer[MSGBUF_LEN]; + int len; + + /* set up current test suite name for later messaging... */ + cu_current_test_suite = ts_name; + + /* redirect stdout and stderr */ + redirect_out_err(cu_current_test_suite); + + while (ts->name != NULL && ts->func != NULL){ + test_failed = 0; + + /* set up name of test for later messaging */ + cu_current_test = ts->name; + + /* send message what test is currently running */ + len = snprintf(buffer, MSGBUF_LEN, "%c --> Running %s...\n", + TEST_NAME, cu_current_test); + write(fd, buffer, len); + + /* run test */ + (*(ts->func))(); + + if (test_failed){ + MSG_TEST_FAILED; + test_suite_failed = 1; + }else{ + MSG_TEST_SUCCEED; + } + + ts++; /* next test in test suite */ + } + + if (test_suite_failed){ + MSG_TEST_SUITE_FAILED; + }else{ + MSG_TEST_SUITE_SUCCEED; + } + + /* close redirected stdout and stderr */ + close_out_err(); +} + + +static void receive_messages(void) +{ + char buf[BUF_LEN]; /* buffer */ + int buf_len; /* how many chars stored in buf */ + char bufout[MSGBUF_LEN]; /* buffer which can be printed out */ + int bufout_len; + int state = 0; /* 0 - waiting for code, 1 - copy msg to stdout */ + int i; + int end = 0; /* end of messages? */ + + bufout_len = 0; + while((buf_len = read(fd, buf, BUF_LEN)) > 0 && !end){ + for (i=0; i < buf_len; i++){ + + /* Prepare message for printing out */ + if (state == 1 || state == 2){ + if (bufout_len < MSGBUF_LEN) + bufout[bufout_len++] = buf[i]; + } + + /* reset state on '\n' in msg */ + if (buf[i] == '\n'){ + /* copy messages out */ + if (state == 1) + write(1, bufout, bufout_len); + if (state == 2) + write(2, bufout, bufout_len); + + state = 0; + bufout_len = 0; + continue; + } + + if (state == 0){ + if (buf[i] == CHECK_FAILED){ + cu_fail_checks++; + state = 2; + }else if (buf[i] == TEST_NAME){ + state = 1; + }else if (buf[i] == CHECK_SUCCEED){ + cu_success_checks++; + }else if (buf[i] == TEST_FAILED){ + cu_fail_tests++; + }else if (buf[i] == TEST_SUCCEED){ + cu_success_tests++; + }else if (buf[i] == TEST_SUITE_FAILED){ + cu_fail_test_suites++; + }else if (buf[i] == TEST_SUITE_SUCCEED){ + cu_success_test_suites++; + }else if (buf[i] == END){ + end = 1; + break; + } + } + } + } +} + +void cu_success_assertation(void) +{ + MSG_CHECK_SUCCEED; +} + +void cu_fail_assertation(const char *file, int line, const char *msg) +{ + char buf[MSGBUF_LEN]; + int len; + + len = snprintf(buf, MSGBUF_LEN, "%c%s:%d (%s::%s) :: %s\n", + CHECK_FAILED, + file, line, cu_current_test_suite, cu_current_test, msg); + write(fd, buf, len); + + /* enable test_failed flag */ + test_failed = 1; +} + +static void cu_print_results(void) +{ + fprintf(stdout, "\n"); + fprintf(stdout, "==================================================\n"); + fprintf(stdout, "| | failed | succeed | total |\n"); + fprintf(stdout, "|------------------------------------------------|\n"); + fprintf(stdout, "| assertations: | %6d | %7d | %5d |\n", + cu_fail_checks, cu_success_checks, + cu_success_checks+cu_fail_checks); + fprintf(stdout, "| tests: | %6d | %7d | %5d |\n", + cu_fail_tests, cu_success_tests, + cu_success_tests+cu_fail_tests); + fprintf(stdout, "| tests suites: | %6d | %7d | %5d |\n", + cu_fail_test_suites, cu_success_test_suites, + cu_success_test_suites+cu_fail_test_suites); + fprintf(stdout, "==================================================\n"); +} + +void cu_set_out_prefix(const char *str) +{ + strncpy(cu_out_prefix, str, CU_OUT_PREFIX_LENGTH); +} + +static void redirect_out_err(const char *test_name) +{ + char buf[100]; + + snprintf(buf, 99, "%stmp.%s.out", cu_out_prefix, test_name); + if (freopen(buf, "w", stdout) == NULL){ + perror("Redirecting of stdout failed"); + exit(-1); + } + + snprintf(buf, 99, "%stmp.%s.err", cu_out_prefix, test_name); + if (freopen(buf, "w", stderr) == NULL){ + perror("Redirecting of stderr failed"); + exit(-1); + } +} + +static void close_out_err(void) +{ + fclose(stdout); + fclose(stderr); +} + + +#ifdef CU_ENABLE_TIMER +/* global variables for timer functions */ +struct timespec __cu_timer; +static struct timespec __cu_timer_start, __cu_timer_stop; + +const struct timespec *cuTimer(void) +{ + return &__cu_timer; +} + +void cuTimerStart(void) +{ + clock_gettime(CLOCK_MONOTONIC, &__cu_timer_start); +} + +const struct timespec *cuTimerStop(void) +{ + clock_gettime(CLOCK_MONOTONIC, &__cu_timer_stop); + + /* store into t difference between time_start and time_end */ + if (__cu_timer_stop.tv_nsec > __cu_timer_start.tv_nsec){ + __cu_timer.tv_nsec = __cu_timer_stop.tv_nsec - __cu_timer_start.tv_nsec; + __cu_timer.tv_sec = __cu_timer_stop.tv_sec - __cu_timer_start.tv_sec; + }else{ + __cu_timer.tv_nsec = __cu_timer_stop.tv_nsec + 1000000000L - __cu_timer_start.tv_nsec; + __cu_timer.tv_sec = __cu_timer_stop.tv_sec - 1 - __cu_timer_start.tv_sec; + } + + return &__cu_timer; +} +#endif /* CU_ENABLE_TIMER */ diff --git a/3rdparty/libccd/src/testsuites/cu/cu.h b/3rdparty/libccd/src/testsuites/cu/cu.h new file mode 100644 index 0000000..06574cf --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/cu.h @@ -0,0 +1,164 @@ +/*** + * CU - C unit testing framework + * --------------------------------- + * Copyright (c)2007,2008,2009 Daniel Fiser + * + * + * This file is part of CU. + * + * CU is free software; you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation; either version 3 of + * the License, or (at your option) any later version. + * + * CU is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef _CU_H_ +#define _CU_H_ + +#ifdef CU_ENABLE_TIMER +# include +#endif /* CU_ENABLE_TIMER */ + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/***** PUBLIC API *****/ +/** + * Define test + */ +#define TEST(name) \ + void name(void) + +/** + * Define testsuite + */ +#define TEST_SUITE(name) \ + cu_test_suite_t test_suite_##name[] = +/** + * Must be on the end of list of tests. + */ +#define TEST_SUITE_CLOSURE \ + { NULL, NULL } + +#define TEST_SUITES \ + cu_test_suites_t cu_test_suites[] = +#define TEST_SUITES_CLOSURE \ + { NULL, NULL } +#define TEST_SUITE_ADD(name) \ + { #name, test_suite_##name } + +/** + * Add test to testsuite + */ +#define TEST_ADD(name) \ + { #name, name } + +#define CU_RUN(argc, argv) \ + cu_run(argc, argv) + +/** + * Set prefix for files printed out. Must contain trailing /. + */ +#define CU_SET_OUT_PREFIX(str) \ + cu_set_out_prefix(str) + +/** + * Assertations + * Assertations with suffix 'M' (e.g. assertTrueM) is variation of macro + * where is possible to specify error message. + */ +#define assertTrueM(a, message) \ + if (a){ \ + cu_success_assertation(); \ + }else{ \ + cu_fail_assertation(__FILE__, __LINE__, message); \ + } +#define assertTrue(a) \ + assertTrueM((a), #a " is not true") + +#define assertFalseM(a, message) \ + assertTrueM(!(a), message) +#define assertFalse(a) \ + assertFalseM((a), #a " is not false") + +#define assertEqualsM(a,b,message) \ + assertTrueM((a) == (b), message) +#define assertEquals(a,b) \ + assertEqualsM((a), (b), #a " not equals " #b) + +#define assertNotEqualsM(a,b,message) \ + assertTrueM((a) != (b), message) +#define assertNotEquals(a,b) \ + assertNotEqualsM((a), (b), #a " equals " #b) +/***** PUBLIC API END *****/ + + +#include + +#define CU_MAX_NAME_LENGTH 30 + +typedef void (*cu_test_func_t)(void); +typedef struct _cu_test_suite_t { + const char *name; + cu_test_func_t func; +} cu_test_suite_t; +typedef struct _cu_test_suites_t { + const char *name; + cu_test_suite_t *test_suite; +} cu_test_suites_t; + +extern cu_test_suites_t cu_test_suites[]; + +extern const char *cu_current_test; +extern const char *cu_current_test_suite; + +extern int cu_success_test_suites; +extern int cu_fail_test_suites; +extern int cu_success_tests; +extern int cu_fail_tests; +extern int cu_success_checks; +extern int cu_fail_checks; + +#define CU_OUT_PREFIX_LENGTH 30 +extern char cu_out_prefix[CU_OUT_PREFIX_LENGTH+1]; + +void cu_run(int argc, char *argv[]); +void cu_success_assertation(void); +void cu_fail_assertation(const char *file, int line, const char *msg); +void cu_set_out_prefix(const char *str); + +/** Timer **/ +#ifdef CU_ENABLE_TIMER +extern struct timespec __cu_timer; + +/** + * Returns value of timer. (as timespec struct) + */ +const struct timespec *cuTimer(void); + +/** + * Starts timer. + */ +void cuTimerStart(void); + +/** + * Stops timer and record elapsed time from last call of cuTimerStart(). + * Returns current value of timer. + */ +const struct timespec *cuTimerStop(void); +#endif /* CU_ENABLE_TIMER */ + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif diff --git a/3rdparty/libccd/src/testsuites/cu/latest.sh b/3rdparty/libccd/src/testsuites/cu/latest.sh new file mode 100755 index 0000000..28481ad --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cu/latest.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +repo=http://git.danfis.cz/cu.git +files="COPYING \ + COPYING.LESSER \ + cu.h \ + cu.c \ + Makefile \ + check-regressions \ + .gitignore \ + " +rm -rf cu +git clone $repo +for file in $files; do + mv cu/"$file" . +done; +rm -rf cu diff --git a/3rdparty/libccd/src/testsuites/cylcyl.c b/3rdparty/libccd/src/testsuites/cylcyl.c new file mode 100644 index 0000000..6cd2124 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cylcyl.c @@ -0,0 +1,180 @@ +#include +#include +#include +#include "support.h" +#include "common.h" + + +TEST(cylcylSetUp) +{ +} + +TEST(cylcylTearDown) +{ +} + + +TEST(cylcylAlignedX) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, -5., 0., 0.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&c1, &c2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[0] += 0.1; + } +} + +TEST(cylcylAlignedY) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, 0., -5., 0.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&c1, &c2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[1] += 0.1; + } +} + +TEST(cylcylAlignedZ) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, 0., 0., -5.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&c1, &c2, &ccd); + + if (i < 43 || i > 57){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[2] += 0.1; + } +} + +#define TOSVT() \ + svtObjPen(&cyl1, &cyl2, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&cyl2.pos, &dir); \ + svtObjPen(&cyl1, &cyl2, stdout, "Pen 1", depth, &dir, &pos) + +TEST(cylcylPenetrationEPA) +{ + ccd_t ccd; + CCD_CYL(cyl1); + CCD_CYL(cyl2); + int res; + ccd_vec3_t axis; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + fprintf(stderr, "\n\n\n---- cylcylPenetration ----\n\n\n"); + + cyl1.radius = 0.35; + cyl1.height = 0.5; + cyl2.radius = 0.5; + cyl2.height = 1.; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + ccdVec3Set(&cyl2.pos, 0., 0., 0.3); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); <<< + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0., 0., 0.); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + ccdVec3Set(&axis, 0.567, 1.2, 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + ccdVec3Set(&axis, -4.567, 1.2, 0.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + res = ccdGJKPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); +} diff --git a/3rdparty/libccd/src/testsuites/cylcyl.h b/3rdparty/libccd/src/testsuites/cylcyl.h new file mode 100644 index 0000000..8cbbe07 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/cylcyl.h @@ -0,0 +1,29 @@ +#ifndef CYL_CYL +#define CYL_CYL + +#include + +TEST(cylcylSetUp); +TEST(cylcylTearDown); + +TEST(cylcylAlignedX); +TEST(cylcylAlignedY); +TEST(cylcylAlignedZ); + +TEST(cylcylPenetrationEPA); + +TEST_SUITE(TSCylCyl) { + TEST_ADD(cylcylSetUp), + + TEST_ADD(cylcylAlignedX), + TEST_ADD(cylcylAlignedY), + TEST_ADD(cylcylAlignedZ), + + TEST_ADD(cylcylPenetrationEPA), + + TEST_ADD(cylcylTearDown), + TEST_SUITE_CLOSURE +}; + +#endif + diff --git a/3rdparty/libccd/src/testsuites/main.c b/3rdparty/libccd/src/testsuites/main.c new file mode 100644 index 0000000..a4585b0 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/main.c @@ -0,0 +1,32 @@ +#include "vec3.h" +#include "polytope.h" +#include "boxbox.h" +#include "spheresphere.h" +#include "cylcyl.h" +#include "boxcyl.h" + +#include "mpr_boxbox.h" +#include "mpr_cylcyl.h" +#include "mpr_boxcyl.h" + +TEST_SUITES { + TEST_SUITE_ADD(TSVec3), + TEST_SUITE_ADD(TSPt), + TEST_SUITE_ADD(TSBoxBox), + TEST_SUITE_ADD(TSSphereSphere), + TEST_SUITE_ADD(TSCylCyl), + TEST_SUITE_ADD(TSBoxCyl), + + TEST_SUITE_ADD(TSMPRBoxBox), + TEST_SUITE_ADD(TSMPRCylCyl), + TEST_SUITE_ADD(TSMPRBoxCyl), + + TEST_SUITES_CLOSURE +}; +int main(int argc, char *argv[]) +{ + CU_SET_OUT_PREFIX("regressions/"); + CU_RUN(argc, argv); + + return 0; +} diff --git a/3rdparty/libccd/src/testsuites/mpr_boxbox.c b/3rdparty/libccd/src/testsuites/mpr_boxbox.c new file mode 100644 index 0000000..22f0a31 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_boxbox.c @@ -0,0 +1,502 @@ +#include +#include + +#include +#include +#include "../dbg.h" +#include "support.h" +#include "common.h" + + +TEST(mprBoxboxAlignedX) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } + + box1.x = 0.1; + box1.y = 0.2; + box1.z = 0.1; + box2.x = 0.2; + box2.y = 0.1; + box2.z = 0.2; + + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.01; + } + + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., -0.1, 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } +} + +TEST(mprBoxboxAlignedY) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, 0., -5., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[1] += 0.1; + } +} + +TEST(mprBoxboxAlignedZ) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, 0., 0., -5.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i < 35 || i > 65){ + assertFalse(res); + }else if (i != 35 && i != 65){ + assertTrue(res); + } + + box1.pos.v[2] += 0.1; + } +} + + +TEST(mprBoxboxRot) +{ + size_t i; + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t axis; + ccd_real_t angle; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + box1.x = 1; + box1.y = 2; + box1.z = 1; + box2.x = 2; + box2.y = 1; + box2.z = 2; + + ccdVec3Set(&box1.pos, -5., 0.5, 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i < 33 || i > 67){ + assertFalse(res); + }else if (i != 33 && i != 67){ + assertTrue(res); + } + + box1.pos.v[0] += 0.1; + } + + box1.x = 1; + box1.y = 1; + box1.z = 1; + box2.x = 1; + box2.y = 1; + box2.z = 1; + + ccdVec3Set(&box1.pos, -1.01, 0., 0.); + ccdVec3Set(&box2.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + + ccdVec3Set(&axis, 0., 1., 0.); + angle = 0.; + for (i = 0; i < 30; i++){ + res = ccdMPRIntersect(&box1, &box2, &ccd); + + if (i != 0 && i != 10 && i != 20){ + assertTrue(res); + }else{ + assertFalse(res); + } + + angle += M_PI / 20.; + ccdQuatSetAngleAxis(&box1.quat, angle, &axis); + } + +} + + + +static void pConf(ccd_box_t *box1, ccd_box_t *box2, const ccd_vec3_t *v) +{ + fprintf(stdout, "# box1.pos: [%lf %lf %lf]\n", + ccdVec3X(&box1->pos), ccdVec3Y(&box1->pos), ccdVec3Z(&box1->pos)); + fprintf(stdout, "# box1->quat: [%lf %lf %lf %lf]\n", + box1->quat.q[0], box1->quat.q[1], box1->quat.q[2], box1->quat.q[3]); + fprintf(stdout, "# box2->pos: [%lf %lf %lf]\n", + ccdVec3X(&box2->pos), ccdVec3Y(&box2->pos), ccdVec3Z(&box2->pos)); + fprintf(stdout, "# box2->quat: [%lf %lf %lf %lf]\n", + box2->quat.q[0], box2->quat.q[1], box2->quat.q[2], box2->quat.q[3]); + fprintf(stdout, "# sep: [%lf %lf %lf]\n", + ccdVec3X(v), ccdVec3Y(v), ccdVec3Z(v)); + fprintf(stdout, "#\n"); +} + +TEST(mprBoxboxSeparate) +{ + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t sep, expsep, expsep2, axis; + + fprintf(stderr, "\n\n\n---- boxboxSeparate ----\n\n\n"); + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.2); + res = ccdMPRIntersect(&box1, &box2, &ccd); + assertTrue(res); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0.25, 0., 0.); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + ccdVec3Scale(&sep, -1.); + ccdVec3Add(&box1.pos, &sep); + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., 0.); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., -0.25); + assertTrue(ccdVec3Eq(&sep, &expsep)); + + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0., 0., 0.); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + ccdVec3Set(&expsep, 0., 0., 1.); + ccdVec3Set(&expsep2, 0., 0., -1.); + assertTrue(ccdVec3Eq(&sep, &expsep) || ccdVec3Eq(&sep, &expsep2)); + + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + pConf(&box1, &box2, &sep); + + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdGJKSeparate(&box1, &box2, &ccd, &sep); + assertTrue(res == 0); + pConf(&box1, &box2, &sep); +} + + +#define TOSVT() \ + svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&box2.pos, &dir); \ + svtObjPen(&box1, &box2, stdout, "Pen 1", depth, &dir, &pos) + +TEST(mprBoxboxPenetration) +{ + ccd_t ccd; + CCD_BOX(box1); + CCD_BOX(box2); + int res; + ccd_vec3_t axis; + ccd_quat_t rot; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + fprintf(stderr, "\n\n\n---- boxboxPenetration ----\n\n\n"); + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; + box2.y = 1.; + box2.z = 1.5; + + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + /* + ccdVec3Set(&box2.pos, 0., 0., 0.); + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + TOSVT(); + */ + + ccdVec3Set(&box2.pos, 0.1, 0., 0.); + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + + ccdVec3Set(&box1.pos, -0.3, 0.5, 1.); + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, 0.1, 0., 0.1); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0., 0.); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.5, 0.); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = box2.y = box2.z = 1.; + ccdVec3Set(&box2.pos, 0.1, 0., 0.); + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 1., 1.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 7"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.2; box2.y = 0.5; box2.z = 1.; + box2.x = box2.y = box2.z = 1.; + + ccdVec3Set(&axis, 0., 0., 1.); + ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis); + ccdVec3Set(&axis, 1., 0., 0.); + ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis); + ccdQuatMul(&box1.quat, &rot); + ccdVec3Set(&box1.pos, -1.3, 0., 0.); + + ccdVec3Set(&box2.pos, 0., 0., 0.); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 8"); + //TOSVT(); + + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; box2.y = 0.5; box2.z = .5; + ccdVec3Set(&box1.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdVec3Set(&box2.pos, 0., 0.73, 0.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 9"); + //TOSVT(); + + box1.x = box1.y = box1.z = 1.; + box2.x = 0.5; box2.y = 0.5; box2.z = .5; + ccdVec3Set(&box1.pos, 0., 0., 0.); + ccdQuatSet(&box1.quat, 0., 0., 0., 1.); + ccdVec3Set(&box2.pos, 0.3, 0.738, 0.); + ccdQuatSet(&box2.quat, 0., 0., 0., 1.); + + res = ccdMPRPenetration(&box1, &box2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 10"); + //TOSVT(); +} diff --git a/3rdparty/libccd/src/testsuites/mpr_boxbox.h b/3rdparty/libccd/src/testsuites/mpr_boxbox.h new file mode 100644 index 0000000..2f29ad7 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_boxbox.h @@ -0,0 +1,26 @@ +#ifndef MPR_BOX_BOX +#define MPR_BOX_BOX + +#include + +TEST(mprBoxboxAlignedX); +TEST(mprBoxboxAlignedY); +TEST(mprBoxboxAlignedZ); + +TEST(mprBoxboxRot); + +TEST(mprBoxboxSeparate); +TEST(mprBoxboxPenetration); + +TEST_SUITE(TSMPRBoxBox) { + TEST_ADD(mprBoxboxAlignedX), + TEST_ADD(mprBoxboxAlignedY), + TEST_ADD(mprBoxboxAlignedZ), + TEST_ADD(mprBoxboxRot), + TEST_ADD(mprBoxboxSeparate), + TEST_ADD(mprBoxboxPenetration), + + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/mpr_boxcyl.c b/3rdparty/libccd/src/testsuites/mpr_boxcyl.c new file mode 100644 index 0000000..6920df7 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_boxcyl.c @@ -0,0 +1,165 @@ +#include +#include +#include "common.h" +#include "support.h" + +#define TOSVT() \ + svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&cyl.pos, &dir); \ + svtObjPen(&box, &cyl, stdout, "Pen 1", depth, &dir, &pos) + +TEST(mprBoxcylIntersect) +{ + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + int res; + ccd_vec3_t axis; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + ccdVec3Set(&cyl.pos, 0.1, 0., 0.); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + res = ccdMPRIntersect(&box, &cyl, &ccd); + assertTrue(res); +} + + + +TEST(mprBoxcylPen) +{ + ccd_t ccd; + CCD_BOX(box); + CCD_CYL(cyl); + int res; + ccd_vec3_t axis; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + box.x = 0.5; + box.y = 1.; + box.z = 1.5; + cyl.radius = 0.4; + cyl.height = 0.7; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + ccdVec3Set(&cyl.pos, 0.1, 0., 0.); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + ccdVec3Set(&cyl.pos, .6, 0., 0.); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + ccdVec3Set(&axis, 0., 1., 0.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl.pos, .6, 0.6, 0.5); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + ccdVec3Set(&axis, 0.67, 1.1, 0.12); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .6, 0., 0.5); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 7"); + //TOSVT(); + + ccdVec3Set(&axis, -0.1, 2.2, -1.); + ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis); + ccdVec3Set(&cyl.pos, .6, 0., 0.5); + ccdVec3Set(&axis, 1., 1., 0.); + ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis); + ccdVec3Set(&box.pos, .9, 0.8, 0.5); + res = ccdMPRPenetration(&box, &cyl, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 8"); + //TOSVT(); +} + diff --git a/3rdparty/libccd/src/testsuites/mpr_boxcyl.h b/3rdparty/libccd/src/testsuites/mpr_boxcyl.h new file mode 100644 index 0000000..86f14e6 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_boxcyl.h @@ -0,0 +1,16 @@ +#ifndef MPR_TEST_BOXCYL_H +#define MPR_TEST_BOXCYL_H + +#include + +TEST(mprBoxcylIntersect); +TEST(mprBoxcylPen); + +TEST_SUITE(TSMPRBoxCyl){ + TEST_ADD(mprBoxcylIntersect), + TEST_ADD(mprBoxcylPen), + + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/mpr_cylcyl.c b/3rdparty/libccd/src/testsuites/mpr_cylcyl.c new file mode 100644 index 0000000..ec0a3bc --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_cylcyl.c @@ -0,0 +1,179 @@ +#include +#include +#include +#include "support.h" +#include "common.h" + + +TEST(mprCylcylAlignedX) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, -5., 0., 0.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&c1, &c2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[0] += 0.1; + } +} + +TEST(mprCylcylAlignedY) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, 0., -5., 0.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&c1, &c2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[1] += 0.1; + } +} + +TEST(mprCylcylAlignedZ) +{ + ccd_t ccd; + CCD_CYL(c1); + CCD_CYL(c2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + c1.radius = 0.35; + c1.height = 0.5; + c2.radius = 0.5; + c2.height = 1.; + + ccdVec3Set(&c1.pos, 0., 0., -5.); + for (i = 0; i < 100; i++){ + res = ccdMPRIntersect(&c1, &c2, &ccd); + + if (i < 43 || i > 57){ + assertFalse(res); + }else{ + assertTrue(res); + } + + c1.pos.v[2] += 0.1; + } +} + +#define TOSVT() \ + svtObjPen(&cyl1, &cyl2, stdout, "Pen 1", depth, &dir, &pos); \ + ccdVec3Scale(&dir, depth); \ + ccdVec3Add(&cyl2.pos, &dir); \ + svtObjPen(&cyl1, &cyl2, stdout, "Pen 1", depth, &dir, &pos) + +TEST(mprCylcylPenetration) +{ + ccd_t ccd; + CCD_CYL(cyl1); + CCD_CYL(cyl2); + int res; + ccd_vec3_t axis; + ccd_real_t depth; + ccd_vec3_t dir, pos; + + fprintf(stderr, "\n\n\n---- mprCylcylPenetration ----\n\n\n"); + + cyl1.radius = 0.35; + cyl1.height = 0.5; + cyl2.radius = 0.5; + cyl2.height = 1.; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + ccd.center1 = ccdObjCenter; + ccd.center2 = ccdObjCenter; + + ccdVec3Set(&cyl2.pos, 0., 0., 0.3); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 1"); + //TOSVT(); + + ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 2"); + //TOSVT(); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0., 0., 0.); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 3"); + //TOSVT(); + + ccdVec3Set(&axis, 0., 1., 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 4"); + //TOSVT(); + + ccdVec3Set(&axis, 0.567, 1.2, 1.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 5"); + //TOSVT(); + + ccdVec3Set(&axis, -4.567, 1.2, 0.); + ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis); + ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2); + res = ccdMPRPenetration(&cyl1, &cyl2, &ccd, &depth, &dir, &pos); + assertTrue(res == 0); + recPen(depth, &dir, &pos, stdout, "Pen 6"); + //TOSVT(); +} diff --git a/3rdparty/libccd/src/testsuites/mpr_cylcyl.h b/3rdparty/libccd/src/testsuites/mpr_cylcyl.h new file mode 100644 index 0000000..2d2162b --- /dev/null +++ b/3rdparty/libccd/src/testsuites/mpr_cylcyl.h @@ -0,0 +1,23 @@ +#ifndef MPR_CYL_CYL +#define MPR_CYL_CYL + +#include + +TEST(mprCylcylAlignedX); +TEST(mprCylcylAlignedY); +TEST(mprCylcylAlignedZ); + +TEST(mprCylcylPenetration); + +TEST_SUITE(TSMPRCylCyl) { + TEST_ADD(mprCylcylAlignedX), + TEST_ADD(mprCylcylAlignedY), + TEST_ADD(mprCylcylAlignedZ), + + TEST_ADD(mprCylcylPenetration), + + TEST_SUITE_CLOSURE +}; + +#endif + diff --git a/3rdparty/libccd/src/testsuites/polytope.c b/3rdparty/libccd/src/testsuites/polytope.c new file mode 100644 index 0000000..fc52e10 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/polytope.c @@ -0,0 +1,396 @@ +//#undef NDEBUG +#include +#include "../polytope.h" +#include "../dbg.h" + +TEST(ptSetUp) +{ +} + +TEST(ptTearDown) +{ +} + + +TEST(ptCreate1) +{ + ccd_pt_t pt; + ccd_pt_vertex_t *v[3]; + ccd_pt_edge_t *e[3]; + ccd_pt_face_t *f; + ccd_vec3_t u; + int res, i; + + DBG2("------"); + + ccdPtInit(&pt); + ccdPtDestroy(&pt); + + + ccdPtInit(&pt); + + ccdVec3Set(&u, -1., -1., 0.); + v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.); + assertTrue(ccdVec3Eq(&u, &v[0]->v.v)); + + ccdVec3Set(&u, 1., 0., 0.); + v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.); + assertTrue(ccdVec3Eq(&u, &v[1]->v.v)); + + ccdVec3Set(&u, 0., 0., 1.); + v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.); + assertTrue(ccdVec3Eq(&u, &v[2]->v.v)); + + for (i = 0; i < 3; i++){ + assertTrue(ccdEq(v[i]->dist, ccdVec3Len2(&v[i]->v.v))); + } + + + e[0] = ccdPtAddEdge(&pt, v[0], v[1]); + e[1] = ccdPtAddEdge(&pt, v[1], v[2]); + e[2] = ccdPtAddEdge(&pt, v[2], v[0]); + for (i = 0; i < 3; i++){ + DBG("e[%d]->dist: %lf", i, e[i]->dist); + DBG_VEC3(&e[i]->witness, " ->witness: "); + } + + f = ccdPtAddFace(&pt, e[0], e[1], e[2]); + DBG("f->dist: %lf", f->dist); + DBG_VEC3(&f->witness, " ->witness: "); + + for (i = 0; i < 3; i++){ + res = ccdPtDelVertex(&pt, v[i]); + assertFalse(res == 0); + res = ccdPtDelEdge(&pt, e[i]); + assertFalse(res == 0); + } + + ccdPtDelFace(&pt, f); + for (i = 0; i < 3; i++){ + res = ccdPtDelVertex(&pt, v[i]); + assertFalse(res == 0); + } + for (i = 0; i < 3; i++){ + res = ccdPtDelEdge(&pt, e[i]); + assertTrue(res == 0); + } + for (i = 0; i < 3; i++){ + res = ccdPtDelVertex(&pt, v[i]); + assertTrue(res == 0); + } + + v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.); + v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.); + v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.); + + e[0] = ccdPtAddEdge(&pt, v[0], v[1]); + e[1] = ccdPtAddEdge(&pt, v[1], v[2]); + e[2] = ccdPtAddEdge(&pt, v[2], v[0]); + + f = ccdPtAddFace(&pt, e[0], e[1], e[2]); + + ccdPtDestroy(&pt); +} + +TEST(ptCreate2) +{ + ccd_pt_t pt; + ccd_pt_vertex_t *v[4]; + ccd_pt_edge_t *e[6]; + ccd_pt_face_t *f[4]; + ccd_vec3_t u; + int res, i; + + DBG2("------"); + + ccdPtInit(&pt); + + ccdVec3Set(&u, -1., -1., 0.); + v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.); + assertTrue(ccdVec3Eq(&u, &v[0]->v.v)); + + ccdVec3Set(&u, 1., 0., 0.); + v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.); + assertTrue(ccdVec3Eq(&u, &v[1]->v.v)); + + ccdVec3Set(&u, 0., 0., 1.); + v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.); + assertTrue(ccdVec3Eq(&u, &v[2]->v.v)); + + ccdVec3Set(&u, 0., 1., 0.); + v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.); + assertTrue(ccdVec3Eq(&u, &v[3]->v.v)); + + for (i = 0; i < 4; i++){ + assertTrue(ccdEq(v[i]->dist, ccdVec3Len2(&v[i]->v.v))); + } + for (i = 0; i < 4; i++){ + DBG("v[%d]->dist: %lf", i, v[i]->dist); + DBG_VEC3(&v[i]->witness, " ->witness: "); + } + + e[0] = ccdPtAddEdge(&pt, v[0], v[1]); + e[1] = ccdPtAddEdge(&pt, v[1], v[2]); + e[2] = ccdPtAddEdge(&pt, v[2], v[0]); + e[3] = ccdPtAddEdge(&pt, v[3], v[0]); + e[4] = ccdPtAddEdge(&pt, v[3], v[1]); + e[5] = ccdPtAddEdge(&pt, v[3], v[2]); + for (i = 0; i < 6; i++){ + DBG("e[%d]->dist: %lf", i, e[i]->dist); + DBG_VEC3(&e[i]->witness, " ->witness: "); + } + + f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]); + f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]); + f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]); + f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]); + for (i = 0; i < 4; i++){ + DBG("f[%d]->dist: %lf", i, f[i]->dist); + DBG_VEC3(&f[i]->witness, " ->witness: "); + } + + for (i = 0; i < 4; i++){ + res = ccdPtDelVertex(&pt, v[i]); + assertFalse(res == 0); + } + for (i = 0; i < 6; i++){ + res = ccdPtDelEdge(&pt, e[i]); + assertFalse(res == 0); + } + + res = ccdPtDelFace(&pt, f[0]); + for (i = 0; i < 6; i++){ + res = ccdPtDelEdge(&pt, e[i]); + assertFalse(res == 0); + } + + res = ccdPtDelFace(&pt, f[1]); + assertTrue(ccdPtDelEdge(&pt, e[0]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[1]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[2]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[3]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[4]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[5]) == 0); + for (i = 0; i < 4; i++){ + res = ccdPtDelVertex(&pt, v[i]); + assertFalse(res == 0); + } + + res = ccdPtDelFace(&pt, f[2]); + assertTrue(ccdPtDelEdge(&pt, e[1]) == 0); + assertTrue(ccdPtDelEdge(&pt, e[4]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[2]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[3]) == 0); + assertFalse(ccdPtDelEdge(&pt, e[5]) == 0); + + assertTrue(ccdPtDelVertex(&pt, v[1]) == 0); + assertFalse(ccdPtDelVertex(&pt, v[0]) == 0); + assertFalse(ccdPtDelVertex(&pt, v[2]) == 0); + assertFalse(ccdPtDelVertex(&pt, v[3]) == 0); + + res = ccdPtDelFace(&pt, f[3]); + assertTrue(ccdPtDelEdge(&pt, e[2]) == 0); + assertTrue(ccdPtDelEdge(&pt, e[3]) == 0); + assertTrue(ccdPtDelEdge(&pt, e[5]) == 0); + + assertTrue(ccdPtDelVertex(&pt, v[0]) == 0); + assertTrue(ccdPtDelVertex(&pt, v[2]) == 0); + assertTrue(ccdPtDelVertex(&pt, v[3]) == 0); + + + v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.); + v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.); + v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.); + v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.); + + e[0] = ccdPtAddEdge(&pt, v[0], v[1]); + e[1] = ccdPtAddEdge(&pt, v[1], v[2]); + e[2] = ccdPtAddEdge(&pt, v[2], v[0]); + e[3] = ccdPtAddEdge(&pt, v[3], v[0]); + e[4] = ccdPtAddEdge(&pt, v[3], v[1]); + e[5] = ccdPtAddEdge(&pt, v[3], v[2]); + + f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]); + f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]); + f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]); + f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]); + + ccdPtDestroy(&pt); +} + +TEST(ptNearest) +{ + ccd_pt_t pt; + ccd_pt_vertex_t *v[4]; + ccd_pt_edge_t *e[6]; + ccd_pt_face_t *f[4]; + ccd_pt_el_t *nearest; + + DBG2("------"); + + ccdPtInit(&pt); + + v[0] = ccdPtAddVertexCoords(&pt, -1., -1., 0.); + v[1] = ccdPtAddVertexCoords(&pt, 1., 0., 0.); + v[2] = ccdPtAddVertexCoords(&pt, 0., 0., 1.); + v[3] = ccdPtAddVertexCoords(&pt, 0., 1., 0.); + + e[0] = ccdPtAddEdge(&pt, v[0], v[1]); + e[1] = ccdPtAddEdge(&pt, v[1], v[2]); + e[2] = ccdPtAddEdge(&pt, v[2], v[0]); + e[3] = ccdPtAddEdge(&pt, v[3], v[0]); + e[4] = ccdPtAddEdge(&pt, v[3], v[1]); + e[5] = ccdPtAddEdge(&pt, v[3], v[2]); + + f[0] = ccdPtAddFace(&pt, e[0], e[1], e[2]); + f[1] = ccdPtAddFace(&pt, e[3], e[4], e[0]); + f[2] = ccdPtAddFace(&pt, e[4], e[5], e[1]); + f[3] = ccdPtAddFace(&pt, e[5], e[3], e[2]); + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_FACE); + assertEquals(nearest, (ccd_pt_el_t *)f[1]); + assertTrue(ccdPtDelFace(&pt, f[1]) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_FACE); + assertTrue(nearest == (ccd_pt_el_t *)f[0] + || nearest == (ccd_pt_el_t *)f[3]); + assertTrue(ccdPtDelFace(&pt, (ccd_pt_face_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_FACE); + assertTrue(nearest == (ccd_pt_el_t *)f[0] + || nearest == (ccd_pt_el_t *)f[3]); + assertTrue(ccdPtDelFace(&pt, (ccd_pt_face_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[0] + || nearest == (ccd_pt_el_t *)e[3]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[0] + || nearest == (ccd_pt_el_t *)e[3]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_FACE); + assertEquals(nearest, (ccd_pt_el_t *)f[2]); + assertTrue(ccdPtDelFace(&pt, f[2]) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[1] + || nearest == (ccd_pt_el_t *)e[4] + || nearest == (ccd_pt_el_t *)e[5]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[1] + || nearest == (ccd_pt_el_t *)e[4] + || nearest == (ccd_pt_el_t *)e[5]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[1] + || nearest == (ccd_pt_el_t *)e[4] + || nearest == (ccd_pt_el_t *)e[5]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_EDGE); + assertTrue(nearest == (ccd_pt_el_t *)e[2]); + assertTrue(ccdPtDelEdge(&pt, (ccd_pt_edge_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_VERTEX); + assertTrue(nearest == (ccd_pt_el_t *)v[1] + || nearest == (ccd_pt_el_t *)v[2] + || nearest == (ccd_pt_el_t *)v[3]); + assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_VERTEX); + assertTrue(nearest == (ccd_pt_el_t *)v[1] + || nearest == (ccd_pt_el_t *)v[2] + || nearest == (ccd_pt_el_t *)v[3]); + assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_VERTEX); + assertTrue(nearest == (ccd_pt_el_t *)v[1] + || nearest == (ccd_pt_el_t *)v[2] + || nearest == (ccd_pt_el_t *)v[3]); + assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + //DBG("nearest->type: %d", nearest->type); + //DBG(" ->dist: %lf", nearest->dist); + //DBG_VEC3(&nearest->witness, " ->witness: "); + assertEquals(nearest->type, CCD_PT_VERTEX); + assertTrue(nearest == (ccd_pt_el_t *)v[0]); + assertTrue(ccdPtDelVertex(&pt, (ccd_pt_vertex_t *)nearest) == 0); + + + nearest = ccdPtNearest(&pt); + assertTrue(nearest == NULL); + + ccdPtDestroy(&pt); +} diff --git a/3rdparty/libccd/src/testsuites/polytope.h b/3rdparty/libccd/src/testsuites/polytope.h new file mode 100644 index 0000000..cf31546 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/polytope.h @@ -0,0 +1,24 @@ +#ifndef TEST_POLYTOPE_H +#define TEST_POLYTOPE_H + +#include + +TEST(ptSetUp); +TEST(ptTearDown); + +TEST(ptCreate1); +TEST(ptCreate2); +TEST(ptNearest); + +TEST_SUITE(TSPt) { + TEST_ADD(ptSetUp), + + TEST_ADD(ptCreate1), + TEST_ADD(ptCreate2), + TEST_ADD(ptNearest), + + TEST_ADD(ptTearDown), + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/regressions/.dir b/3rdparty/libccd/src/testsuites/regressions/.dir new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.err b/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.err new file mode 100644 index 0000000..a589c58 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.err @@ -0,0 +1,12 @@ + + + +---- boxboxSeparate ---- + + + + + +---- boxboxPenetration ---- + + diff --git a/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.out b/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.out new file mode 100644 index 0000000..27bb231 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSBoxBox.out @@ -0,0 +1,44 @@ +# box1.pos: [-0.500000 0.000000 0.000000] +# box1->quat: [0.000000 0.000000 0.382683 0.923880] +# box2->pos: [0.000000 0.000000 0.000000] +# box2->quat: [0.000000 0.000000 0.000000 1.000000] +# sep: [0.707107 0.000000 0.000000] +# +# box1.pos: [-0.500000 0.100000 0.400000] +# box1->quat: [0.000000 0.270598 0.270598 0.923880] +# box2->pos: [0.000000 0.000000 0.000000] +# box2->quat: [0.000000 0.000000 0.000000 1.000000] +# sep: [0.633939 0.000000 -0.371353] +# +# Pen 1: depth: 0.650000 +# Pen 1: dir: [1.000000 0.000000 0.000000] +# Pen 1: pos: [0.096875 0.000000 0.000000] +# +# Pen 2: depth: 0.250000 +# Pen 2: dir: [-0.000000 0.000000 -1.000000] +# Pen 2: pos: [-0.058333 0.250000 0.583333] +# +# Pen 3: depth: 0.900000 +# Pen 3: dir: [0.000000 0.000000 -1.000000] +# Pen 3: pos: [0.111506 0.000000 0.050000] +# +# Pen 4: depth: 0.607107 +# Pen 4: dir: [1.000000 0.000000 0.000000] +# Pen 4: pos: [-0.153585 0.000000 0.000000] +# +# Pen 5: depth: 0.429289 +# Pen 5: dir: [0.707107 -0.707107 0.000000] +# Pen 5: pos: [-0.167157 0.379289 0.000000] +# +# Pen 6: depth: 0.648412 +# Pen 6: dir: [0.862856 0.000000 -0.505449] +# Pen 6: pos: [-0.148223 0.055362 0.319638] +# +# Pen 7: depth: 0.622622 +# Pen 7: dir: [1.000000 0.000000 -0.000000] +# Pen 7: pos: [-0.095997 0.063593 0.067678] +# +# Pen 8: depth: 0.053553 +# Pen 8: dir: [1.000000 0.000000 0.000000] +# Pen 8: pos: [-0.523223 -0.073223 0.020711] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSBoxCyl.err b/3rdparty/libccd/src/testsuites/regressions/TSBoxCyl.err new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSBoxCyl.out b/3rdparty/libccd/src/testsuites/regressions/TSBoxCyl.out new file mode 100644 index 0000000..d257ec1 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSBoxCyl.out @@ -0,0 +1,32 @@ +# Pen 1: depth: 0.549996 +# Pen 1: dir: [0.999992 -0.003902 0.000000] +# Pen 1: pos: [0.020284 0.000000 0.000000] +# +# Pen 2: depth: 0.050000 +# Pen 2: dir: [0.999992 -0.003902 0.000000] +# Pen 2: pos: [0.253480 0.000000 0.025000] +# +# Pen 3: depth: 0.030994 +# Pen 3: dir: [0.950248 0.311493 0.000000] +# Pen 3: pos: [0.246546 0.420744 0.000000] +# +# Pen 4: depth: 0.033436 +# Pen 4: dir: [0.976101 0.217308 0.001900] +# Pen 4: pos: [0.243648 0.480401 0.450000] +# +# Pen 5: depth: 0.142160 +# Pen 5: dir: [0.968442 0.249235 0.001146] +# Pen 5: pos: [0.190887 0.421462 0.605496] +# +# Pen 6: depth: 0.179282 +# Pen 6: dir: [0.999995 0.001057 0.002913] +# Pen 6: pos: [0.176026 0.036944 0.488189] +# +# Pen 7: depth: 0.750000 +# Pen 7: dir: [-0.853795 -0.143509 -0.500438] +# Pen 7: pos: [0.572744 0.014828 0.562324] +# +# Pen 8: depth: 0.142666 +# Pen 8: dir: [-0.475515 -0.841074 0.257839] +# Pen 8: pos: [0.824886 0.230213 0.463136] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.err b/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.err new file mode 100644 index 0000000..6407d50 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.err @@ -0,0 +1,6 @@ + + + +---- cylcylPenetration ---- + + diff --git a/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.out b/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.out new file mode 100644 index 0000000..b97fbc5 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSCylCyl.out @@ -0,0 +1,24 @@ +# Pen 1: depth: 0.750000 +# Pen 1: dir: [0.000000 0.000000 1.000000] +# Pen 1: pos: [0.004079 -0.012238 0.009615] +# +# Pen 2: depth: 0.531931 +# Pen 2: dir: [-0.926428 -0.376463 -0.002666] +# Pen 2: pos: [0.218566 0.072232 0.025000] +# +# Pen 3: depth: 0.645740 +# Pen 3: dir: [-0.500000 -0.146447 -0.853553] +# Pen 3: pos: [0.177594 0.070484 0.186987] +# +# Pen 4: depth: 0.104445 +# Pen 4: dir: [-0.482095 0.866317 0.130685] +# Pen 4: pos: [0.123724 0.348390 0.269312] +# +# Pen 5: depth: 0.093082 +# Pen 5: dir: [0.034600 -0.999228 -0.018627] +# Pen 5: pos: [0.311257 -0.203923 -0.064270] +# +# Pen 6: depth: 0.198749 +# Pen 6: dir: [0.411370 -0.911372 0.013223] +# Pen 6: pos: [0.405836 -0.130066 0.121441] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.err b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.err new file mode 100644 index 0000000..a589c58 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.err @@ -0,0 +1,12 @@ + + + +---- boxboxSeparate ---- + + + + + +---- boxboxPenetration ---- + + diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.out b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.out new file mode 100644 index 0000000..700839b --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxBox.out @@ -0,0 +1,52 @@ +# box1.pos: [-0.500000 0.000000 0.000000] +# box1->quat: [0.000000 0.000000 0.382683 0.923880] +# box2->pos: [0.000000 0.000000 0.000000] +# box2->quat: [0.000000 0.000000 0.000000 1.000000] +# sep: [0.707107 0.000000 0.000000] +# +# box1.pos: [-0.500000 0.100000 0.400000] +# box1->quat: [0.000000 0.270598 0.270598 0.923880] +# box2->pos: [0.000000 0.000000 0.000000] +# box2->quat: [0.000000 0.000000 0.000000 1.000000] +# sep: [0.633939 0.000000 -0.371353] +# +# Pen 1: depth: 0.650000 +# Pen 1: dir: [1.000000 0.000000 0.000000] +# Pen 1: pos: [0.175000 0.000000 0.000000] +# +# Pen 2: depth: 0.250000 +# Pen 2: dir: [-0.000000 0.000000 -1.000000] +# Pen 2: pos: [-0.033333 0.250000 0.600000] +# +# Pen 3: depth: 0.900000 +# Pen 3: dir: [0.000000 0.000000 -1.000000] +# Pen 3: pos: [0.100000 0.000000 0.050000] +# +# Pen 4: depth: 0.607107 +# Pen 4: dir: [1.000000 0.000000 0.000000] +# Pen 4: pos: [-0.096447 0.000000 0.000000] +# +# Pen 5: depth: 0.429289 +# Pen 5: dir: [0.707107 -0.707107 0.000000] +# Pen 5: pos: [-0.222183 0.322183 0.000000] +# +# Pen 6: depth: 0.648412 +# Pen 6: dir: [0.862856 0.000000 -0.505449] +# Pen 6: pos: [-0.163060 0.012676 0.263060] +# +# Pen 7: depth: 0.622928 +# Pen 7: dir: [0.999509 0.028016 -0.014008] +# Pen 7: pos: [-0.145374 0.170833 0.176732] +# +# Pen 8: depth: 0.053553 +# Pen 8: dir: [1.000000 0.000000 0.000000] +# Pen 8: pos: [-0.480217 -0.140652 0.000000] +# +# Pen 9: depth: 0.020000 +# Pen 9: dir: [0.000000 1.000000 0.000000] +# Pen 9: pos: [0.000000 0.490000 0.000000] +# +# Pen 10: depth: 0.012000 +# Pen 10: dir: [-0.000000 1.000000 0.000000] +# Pen 10: pos: [0.200000 0.492000 0.000000] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxCyl.err b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxCyl.err new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxCyl.out b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxCyl.out new file mode 100644 index 0000000..775d86c --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSMPRBoxCyl.out @@ -0,0 +1,32 @@ +# Pen 1: depth: 0.550000 +# Pen 1: dir: [1.000000 0.000000 0.000000] +# Pen 1: pos: [-0.025000 0.000000 0.000000] +# +# Pen 2: depth: 0.050000 +# Pen 2: dir: [1.000000 0.000000 0.000000] +# Pen 2: pos: [0.225000 0.000000 0.000000] +# +# Pen 3: depth: 0.038532 +# Pen 3: dir: [0.788956 0.614450 0.000000] +# Pen 3: pos: [0.238587 0.477175 0.000000] +# +# Pen 4: depth: 0.038654 +# Pen 4: dir: [0.779134 0.626832 -0.005696] +# Pen 4: pos: [0.238603 0.477206 0.340909] +# +# Pen 5: depth: 0.166653 +# Pen 5: dir: [0.734126 0.679013 -0.000000] +# Pen 5: pos: [0.208320 0.416640 0.595113] +# +# Pen 6: depth: 0.180673 +# Pen 6: dir: [1.000000 0.000003 -0.000000] +# Pen 6: pos: [0.192142 0.009404 0.479162] +# +# Pen 7: depth: 1.321922 +# Pen 7: dir: [-0.897996 -0.063457 0.435403] +# Pen 7: pos: [0.531929 -0.046446 0.867546] +# +# Pen 8: depth: 0.142813 +# Pen 8: dir: [-0.476782 -0.840534 0.257259] +# Pen 8: pos: [0.776128 0.285646 0.436629] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.err b/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.err new file mode 100644 index 0000000..8980d14 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.err @@ -0,0 +1,6 @@ + + + +---- mprCylcylPenetration ---- + + diff --git a/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.out b/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.out new file mode 100644 index 0000000..866b27d --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSMPRCylCyl.out @@ -0,0 +1,24 @@ +# Pen 1: depth: 0.450000 +# Pen 1: dir: [0.000000 0.000000 1.000000] +# Pen 1: pos: [0.000000 0.000000 0.025000] +# +# Pen 2: depth: 0.533732 +# Pen 2: dir: [-0.952492 -0.304562 0.000000] +# Pen 2: pos: [0.176471 0.058824 0.166667] +# +# Pen 3: depth: 0.720933 +# Pen 3: dir: [-0.947406 -0.320033 0.000085] +# Pen 3: pos: [0.198747 0.066309 0.050800] +# +# Pen 4: depth: 0.106076 +# Pen 4: dir: [-0.524820 0.835278 0.163936] +# Pen 4: pos: [0.138692 0.362418 0.320024] +# +# Pen 5: depth: 0.103863 +# Pen 5: dir: [0.291494 -0.956567 -0.003314] +# Pen 5: pos: [0.337721 -0.209314 -0.094587] +# +# Pen 6: depth: 0.202625 +# Pen 6: dir: [0.347225 -0.937782 -0.000000] +# Pen 6: pos: [0.399554 -0.164780 0.199941] +# diff --git a/3rdparty/libccd/src/testsuites/regressions/TSPt.err b/3rdparty/libccd/src/testsuites/regressions/TSPt.err new file mode 100644 index 0000000..4fd6c93 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/regressions/TSPt.err @@ -0,0 +1,39 @@ +ptCreate1 :: ------ +ptCreate1 :: e[0]->dist: 0.200000 +ptCreate1 :: ->witness: [0.200000 -0.400000 0.000000] +ptCreate1 :: e[1]->dist: 0.500000 +ptCreate1 :: ->witness: [0.500000 0.000000 0.500000] +ptCreate1 :: e[2]->dist: 0.666667 +ptCreate1 :: ->witness: [-0.333333 -0.333333 0.666667] +ptCreate1 :: f->dist: 0.166667 +ptCreate1 :: ->witness: [0.166667 -0.333333 0.166667] +ptCreate2 :: ------ +ptCreate2 :: v[0]->dist: 2.000000 +ptCreate2 :: ->witness: [-1.000000 -1.000000 0.000000] +ptCreate2 :: v[1]->dist: 1.000000 +ptCreate2 :: ->witness: [1.000000 0.000000 0.000000] +ptCreate2 :: v[2]->dist: 1.000000 +ptCreate2 :: ->witness: [0.000000 0.000000 1.000000] +ptCreate2 :: v[3]->dist: 1.000000 +ptCreate2 :: ->witness: [0.000000 1.000000 0.000000] +ptCreate2 :: e[0]->dist: 0.200000 +ptCreate2 :: ->witness: [0.200000 -0.400000 0.000000] +ptCreate2 :: e[1]->dist: 0.500000 +ptCreate2 :: ->witness: [0.500000 0.000000 0.500000] +ptCreate2 :: e[2]->dist: 0.666667 +ptCreate2 :: ->witness: [-0.333333 -0.333333 0.666667] +ptCreate2 :: e[3]->dist: 0.200000 +ptCreate2 :: ->witness: [-0.400000 0.200000 0.000000] +ptCreate2 :: e[4]->dist: 0.500000 +ptCreate2 :: ->witness: [0.500000 0.500000 0.000000] +ptCreate2 :: e[5]->dist: 0.500000 +ptCreate2 :: ->witness: [0.000000 0.500000 0.500000] +ptCreate2 :: f[0]->dist: 0.166667 +ptCreate2 :: ->witness: [0.166667 -0.333333 0.166667] +ptCreate2 :: f[1]->dist: 0.000000 +ptCreate2 :: ->witness: [0.000000 0.000000 0.000000] +ptCreate2 :: f[2]->dist: 0.333333 +ptCreate2 :: ->witness: [0.333333 0.333333 0.333333] +ptCreate2 :: f[3]->dist: 0.166667 +ptCreate2 :: ->witness: [-0.333333 0.166667 0.166667] +ptNearest :: ------ diff --git a/3rdparty/libccd/src/testsuites/regressions/TSPt.out b/3rdparty/libccd/src/testsuites/regressions/TSPt.out new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSSphereSphere.err b/3rdparty/libccd/src/testsuites/regressions/TSSphereSphere.err new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSSphereSphere.out b/3rdparty/libccd/src/testsuites/regressions/TSSphereSphere.out new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSVec3.err b/3rdparty/libccd/src/testsuites/regressions/TSVec3.err new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/regressions/TSVec3.out b/3rdparty/libccd/src/testsuites/regressions/TSVec3.out new file mode 100644 index 0000000..e69de29 diff --git a/3rdparty/libccd/src/testsuites/spheresphere.c b/3rdparty/libccd/src/testsuites/spheresphere.c new file mode 100644 index 0000000..88fca93 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/spheresphere.c @@ -0,0 +1,99 @@ +#include +#include +#include +#include "support.h" + +TEST(spheresphereSetUp) +{ +} + +TEST(spheresphereTearDown) +{ +} + +TEST(spheresphereAlignedX) +{ + ccd_t ccd; + CCD_SPHERE(s1); + CCD_SPHERE(s2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + s1.radius = 0.35; + s2.radius = .5; + + ccdVec3Set(&s1.pos, -5., 0., 0.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&s1, &s2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + s1.pos.v[0] += 0.1; + } +} + +TEST(spheresphereAlignedY) +{ + ccd_t ccd; + CCD_SPHERE(s1); + CCD_SPHERE(s2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + s1.radius = 0.35; + s2.radius = .5; + + ccdVec3Set(&s1.pos, 0., -5., 0.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&s1, &s2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + s1.pos.v[1] += 0.1; + } +} + +TEST(spheresphereAlignedZ) +{ + ccd_t ccd; + CCD_SPHERE(s1); + CCD_SPHERE(s2); + size_t i; + int res; + + CCD_INIT(&ccd); + ccd.support1 = ccdSupport; + ccd.support2 = ccdSupport; + + s1.radius = 0.35; + s2.radius = .5; + + ccdVec3Set(&s1.pos, 0., 0., -5.); + for (i = 0; i < 100; i++){ + res = ccdGJKIntersect(&s1, &s2, &ccd); + + if (i < 42 || i > 58){ + assertFalse(res); + }else{ + assertTrue(res); + } + + s1.pos.v[2] += 0.1; + } +} diff --git a/3rdparty/libccd/src/testsuites/spheresphere.h b/3rdparty/libccd/src/testsuites/spheresphere.h new file mode 100644 index 0000000..b032215 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/spheresphere.h @@ -0,0 +1,24 @@ +#ifndef SPHERE_SPHERE +#define SPHERE_SPHERE + +#include + +TEST(spheresphereSetUp); +TEST(spheresphereTearDown); + +TEST(spheresphereAlignedX); +TEST(spheresphereAlignedY); +TEST(spheresphereAlignedZ); + +TEST_SUITE(TSSphereSphere) { + TEST_ADD(spheresphereSetUp), + + TEST_ADD(spheresphereAlignedX), + TEST_ADD(spheresphereAlignedY), + TEST_ADD(spheresphereAlignedZ), + + TEST_ADD(spheresphereTearDown), + TEST_SUITE_CLOSURE +}; + +#endif diff --git a/3rdparty/libccd/src/testsuites/support.c b/3rdparty/libccd/src/testsuites/support.c new file mode 100644 index 0000000..a071d30 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/support.c @@ -0,0 +1,84 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include +#include +#include "support.h" + +void ccdSupport(const void *_obj, const ccd_vec3_t *_dir, + ccd_vec3_t *v) +{ + // Support function is made according to Gino van den Bergen's paper + // A Fast and Robust CCD Implementation for Collision Detection of + // Convex Objects + + ccd_obj_t *obj = (ccd_obj_t *)_obj; + ccd_vec3_t dir; + ccd_quat_t qinv; + + ccdVec3Copy(&dir, _dir); + ccdQuatInvert2(&qinv, &obj->quat); + + ccdQuatRotVec(&dir, &qinv); + + if (obj->type == CCD_OBJ_BOX){ + ccd_box_t *box = (ccd_box_t *)obj; + ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * box->x * CCD_REAL(0.5), + ccdSign(ccdVec3Y(&dir)) * box->y * CCD_REAL(0.5), + ccdSign(ccdVec3Z(&dir)) * box->z * CCD_REAL(0.5)); + }else if (obj->type == CCD_OBJ_SPHERE){ + ccd_sphere_t *sphere = (ccd_sphere_t *)obj; + ccd_real_t len; + + len = ccdVec3Len2(&dir); + if (len - CCD_EPS > CCD_ZERO){ + ccdVec3Copy(v, &dir); + ccdVec3Scale(v, sphere->radius / CCD_SQRT(len)); + }else{ + ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO); + } + }else if (obj->type == CCD_OBJ_CYL){ + ccd_cyl_t *cyl = (ccd_cyl_t *)obj; + ccd_real_t zdist, rad; + + zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1]; + zdist = CCD_SQRT(zdist); + if (ccdIsZero(zdist)){ + ccdVec3Set(v, CCD_ZERO, CCD_ZERO, + ccdSign(ccdVec3Z(&dir)) * cyl->height * CCD_REAL(0.5)); + }else{ + rad = cyl->radius / zdist; + + ccdVec3Set(v, rad * ccdVec3X(&dir), + rad * ccdVec3Y(&dir), + ccdSign(ccdVec3Z(&dir)) * cyl->height * CCD_REAL(0.5)); + } + } + + // transform support vertex + ccdQuatRotVec(v, &obj->quat); + ccdVec3Add(v, &obj->pos); +} + +void ccdObjCenter(const void *_obj, ccd_vec3_t *center) +{ + ccd_obj_t *obj = (ccd_obj_t *)_obj; + + ccdVec3Set(center, CCD_ZERO, CCD_ZERO, CCD_ZERO); + // rotation is not needed + ccdVec3Add(center, &obj->pos); +} diff --git a/3rdparty/libccd/src/testsuites/support.h b/3rdparty/libccd/src/testsuites/support.h new file mode 100644 index 0000000..e444296 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/support.h @@ -0,0 +1,102 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +/*** + * Some support() functions for some convex shapes. + */ + +#ifndef __CCD_SUPPORT_H__ +#define __CCD_SUPPORT_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +#define CCD_OBJ_BOX 1 +#define CCD_OBJ_SPHERE 2 +#define CCD_OBJ_CYL 3 + +#define __CCD_OBJ__ \ + int type; \ + ccd_vec3_t pos; \ + ccd_quat_t quat; + +struct _ccd_obj_t { + __CCD_OBJ__ +}; +typedef struct _ccd_obj_t ccd_obj_t; + +struct _ccd_box_t { + __CCD_OBJ__ + ccd_real_t x, y, z; //!< Lengths of box's edges +}; +typedef struct _ccd_box_t ccd_box_t; + +struct _ccd_sphere_t { + __CCD_OBJ__ + ccd_real_t radius; +}; +typedef struct _ccd_sphere_t ccd_sphere_t; + +struct _ccd_cyl_t { + __CCD_OBJ__ + ccd_real_t radius; + ccd_real_t height; +}; +typedef struct _ccd_cyl_t ccd_cyl_t; + + +#define CCD_BOX(name) \ + ccd_box_t name = { .type = CCD_OBJ_BOX, \ + .pos = { .v = { 0., 0., 0. } }, \ + .quat = { .q = { 0., 0., 0., 1. } }, \ + .x = 0., \ + .y = 0., \ + .z = 0. } + +#define CCD_SPHERE(name) \ + ccd_sphere_t name = { .type = CCD_OBJ_SPHERE, \ + .pos = { .v = { 0., 0., 0. } }, \ + .quat = { .q = { 0., 0., 0., 1. } }, \ + .radius = 0. } + +#define CCD_CYL(name) \ + ccd_cyl_t name = { .type = CCD_OBJ_CYL, \ + .pos = { .v = { 0., 0., 0. } }, \ + .quat = { .q = { 0., 0., 0., 1. } }, \ + .radius = 0., \ + .height = 0. } + +/** + * Returns supporting vertex via v. + * Supporting vertex is fathest vertex from object in direction dir. + */ +void ccdSupport(const void *obj, const ccd_vec3_t *dir, + ccd_vec3_t *v); + +/** + * Returns center of object. + */ +void ccdObjCenter(const void *obj, ccd_vec3_t *center); + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_SUPPORT_H__ */ diff --git a/3rdparty/libccd/src/testsuites/vec3.c b/3rdparty/libccd/src/testsuites/vec3.c new file mode 100644 index 0000000..c28fad9 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/vec3.c @@ -0,0 +1,280 @@ +#include +#include +#include + +TEST(vec3SetUp) +{ +} + +TEST(vec3TearDown) +{ +} + + +TEST(vec3PointSegmentDist) +{ + ccd_vec3_t P, a, b, w, ew; + ccd_real_t dist; + + ccdVec3Set(&a, 0., 0., 0.); + ccdVec3Set(&b, 1., 0., 0.); + + // extereme w == a + ccdVec3Set(&P, -1., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 1.)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -0.5, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.5 * 0.5)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -0.1, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, .1 * .1)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -1., 1., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 2.)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -0.5, 0.5, 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.5)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -0.1, -1., 2.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 5.01)); + assertTrue(ccdVec3Eq(&w, &a)); + + + // extereme w == b + ccdVec3Set(&P, 2., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 1.)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 1.5, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.5 * 0.5)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 1.1, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, .1 * .1)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 1., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 2., 1., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 2.)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 1.5, 0.5, 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.5)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 1.1, -1., 2.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 5.01)); + assertTrue(ccdVec3Eq(&w, &b)); + + // inside segment + ccdVec3Set(&P, .5, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &P)); + + ccdVec3Set(&P, .9, 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &P)); + + ccdVec3Set(&P, .5, 1., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 1.)); + ccdVec3Set(&ew, 0.5, 0., 0.); + assertTrue(ccdVec3Eq(&w, &ew)); + + ccdVec3Set(&P, .5, 1., 1.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 2.)); + ccdVec3Set(&ew, 0.5, 0., 0.); + assertTrue(ccdVec3Eq(&w, &ew)); + + + + ccdVec3Set(&a, -.5, 2., 1.); + ccdVec3Set(&b, 1., 1.5, 0.5); + + // extereme w == a + ccdVec3Set(&P, -10., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 9.5 * 9.5 + 2. * 2. + 1.)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, -10., 9.2, 3.4); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 9.5 * 9.5 + 7.2 * 7.2 + 2.4 * 2.4)); + assertTrue(ccdVec3Eq(&w, &a)); + + // extereme w == b + ccdVec3Set(&P, 10., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 9. * 9. + 1.5 * 1.5 + 0.5 * 0.5)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, 10., 9.2, 3.4); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 9. * 9. + 7.7 * 7.7 + 2.9 * 2.9)); + assertTrue(ccdVec3Eq(&w, &b)); + + // inside ab + ccdVec3Set(&a, -.1, 1., 1.); + ccdVec3Set(&b, 1., 1., 1.); + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w); + assertTrue(ccdEq(dist, 2.)); + ccdVec3Set(&ew, 0., 1., 1.); + assertTrue(ccdVec3Eq(&w, &ew)); +} + + +TEST(vec3PointTriDist) +{ + ccd_vec3_t P, a, b, c, w, P0; + ccd_real_t dist; + + ccdVec3Set(&a, -1., 0., 0.); + ccdVec3Set(&b, 0., 1., 1.); + ccdVec3Set(&c, -1., 0., 1.); + + ccdVec3Set(&P, -1., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &a)); + + ccdVec3Set(&P, 0., 1., 1.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &b)); + + ccdVec3Set(&P, -1., 0., 1.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &c)); + + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, NULL); + assertTrue(ccdEq(dist, 2./3.)); + + + // region 4 + ccdVec3Set(&P, -2., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a))); + assertTrue(ccdVec3Eq(&w, &a)); + ccdVec3Set(&P, -2., 0.2, -1.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a))); + assertTrue(ccdVec3Eq(&w, &a)); + + // region 2 + ccdVec3Set(&P, -1.3, 0., 1.2); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c))); + assertTrue(ccdVec3Eq(&w, &c)); + ccdVec3Set(&P, -1.2, 0.2, 1.1); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c))); + assertTrue(ccdVec3Eq(&w, &c)); + + // region 6 + ccdVec3Set(&P, 0.3, 1., 1.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b))); + assertTrue(ccdVec3Eq(&w, &b)); + ccdVec3Set(&P, .1, 1., 1.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b))); + assertTrue(ccdVec3Eq(&w, &b)); + + // region 1 + ccdVec3Set(&P, 0., 1., 2.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 1.)); + assertTrue(ccdVec3Eq(&w, &b)); + ccdVec3Set(&P, -1., 0., 2.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 1.)); + assertTrue(ccdVec3Eq(&w, &c)); + ccdVec3Set(&P, -0.5, 0.5, 2.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 1.)); + ccdVec3Set(&P0, -0.5, 0.5, 1.); + assertTrue(ccdVec3Eq(&w, &P0)); + + // region 3 + ccdVec3Set(&P, -2., -1., 0.7); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 2.)); + ccdVec3Set(&P0, -1., 0., 0.7); + assertTrue(ccdVec3Eq(&w, &P0)); + + // region 5 + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 2./3.)); + ccdVec3Set(&P0, -2./3., 1./3., 1./3.); + assertTrue(ccdVec3Eq(&w, &P0)); + + // region 0 + ccdVec3Set(&P, -0.5, 0.5, 0.5); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &P)); + ccdVec3Set(&P, -0.5, 0.5, 0.7); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &P)); + ccdVec3Set(&P, -0.5, 0.5, 0.9); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.)); + assertTrue(ccdVec3Eq(&w, &P)); + + ccdVec3Set(&P, 0., 0., 0.5); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.5)); + ccdVec3Set(&P0, -.5, .5, .5); + assertTrue(ccdVec3Eq(&w, &P0)); + + ccdVec3Set(&a, -1., 0., 0.); + ccdVec3Set(&b, 0., 1., -1.); + ccdVec3Set(&c, 0., 1., 1.); + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(ccdEq(dist, 0.5)); + ccdVec3Set(&P0, -.5, .5, 0.); + assertTrue(ccdVec3Eq(&w, &P0)); + //fprintf(stderr, "dist: %lf\n", dist); + + ccdVec3Set(&a, -0.36715889, 0.288464308, 0.000100158155); + ccdVec3Set(&b, -0.0222680569, 0.0171524286, -2.88337469e-06); + ccdVec3Set(&c, 3.0792e-01, -2.4249e-01, 3.8363e-05); + ccdVec3Set(&P, 0., 0., 0.); + dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w); + assertTrue(dist < 0.0000001); +} diff --git a/3rdparty/libccd/src/testsuites/vec3.h b/3rdparty/libccd/src/testsuites/vec3.h new file mode 100644 index 0000000..2055947 --- /dev/null +++ b/3rdparty/libccd/src/testsuites/vec3.h @@ -0,0 +1,20 @@ +#ifndef TEST_VEC3_H +#define TEST_VEC3_H + +#include + +TEST(vec3SetUp); +TEST(vec3TearDown); +TEST(vec3PointSegmentDist); +TEST(vec3PointTriDist); + +TEST_SUITE(TSVec3) { + TEST_ADD(vec3SetUp), + + TEST_ADD(vec3PointSegmentDist), + TEST_ADD(vec3PointTriDist), + + TEST_ADD(vec3TearDown), + TEST_SUITE_CLOSURE +}; +#endif diff --git a/3rdparty/libccd/src/vec3.c b/3rdparty/libccd/src/vec3.c new file mode 100644 index 0000000..003c946 --- /dev/null +++ b/3rdparty/libccd/src/vec3.c @@ -0,0 +1,217 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#include +#include +#include "dbg.h" + +static CCD_VEC3(__ccd_vec3_origin, CCD_ZERO, CCD_ZERO, CCD_ZERO); +ccd_vec3_t *ccd_vec3_origin = &__ccd_vec3_origin; + +static ccd_vec3_t points_on_sphere[] = { + CCD_VEC3_STATIC(CCD_REAL( 0.000000), CCD_REAL(-0.000000), CCD_REAL(-1.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.723608), CCD_REAL(-0.525725), CCD_REAL(-0.447219)), + CCD_VEC3_STATIC(CCD_REAL(-0.276388), CCD_REAL(-0.850649), CCD_REAL(-0.447219)), + CCD_VEC3_STATIC(CCD_REAL(-0.894426), CCD_REAL(-0.000000), CCD_REAL(-0.447216)), + CCD_VEC3_STATIC(CCD_REAL(-0.276388), CCD_REAL( 0.850649), CCD_REAL(-0.447220)), + CCD_VEC3_STATIC(CCD_REAL( 0.723608), CCD_REAL( 0.525725), CCD_REAL(-0.447219)), + CCD_VEC3_STATIC(CCD_REAL( 0.276388), CCD_REAL(-0.850649), CCD_REAL( 0.447220)), + CCD_VEC3_STATIC(CCD_REAL(-0.723608), CCD_REAL(-0.525725), CCD_REAL( 0.447219)), + CCD_VEC3_STATIC(CCD_REAL(-0.723608), CCD_REAL( 0.525725), CCD_REAL( 0.447219)), + CCD_VEC3_STATIC(CCD_REAL( 0.276388), CCD_REAL( 0.850649), CCD_REAL( 0.447219)), + CCD_VEC3_STATIC(CCD_REAL( 0.894426), CCD_REAL( 0.000000), CCD_REAL( 0.447216)), + CCD_VEC3_STATIC(CCD_REAL(-0.000000), CCD_REAL( 0.000000), CCD_REAL( 1.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.425323), CCD_REAL(-0.309011), CCD_REAL(-0.850654)), + CCD_VEC3_STATIC(CCD_REAL(-0.162456), CCD_REAL(-0.499995), CCD_REAL(-0.850654)), + CCD_VEC3_STATIC(CCD_REAL( 0.262869), CCD_REAL(-0.809012), CCD_REAL(-0.525738)), + CCD_VEC3_STATIC(CCD_REAL( 0.425323), CCD_REAL( 0.309011), CCD_REAL(-0.850654)), + CCD_VEC3_STATIC(CCD_REAL( 0.850648), CCD_REAL(-0.000000), CCD_REAL(-0.525736)), + CCD_VEC3_STATIC(CCD_REAL(-0.525730), CCD_REAL(-0.000000), CCD_REAL(-0.850652)), + CCD_VEC3_STATIC(CCD_REAL(-0.688190), CCD_REAL(-0.499997), CCD_REAL(-0.525736)), + CCD_VEC3_STATIC(CCD_REAL(-0.162456), CCD_REAL( 0.499995), CCD_REAL(-0.850654)), + CCD_VEC3_STATIC(CCD_REAL(-0.688190), CCD_REAL( 0.499997), CCD_REAL(-0.525736)), + CCD_VEC3_STATIC(CCD_REAL( 0.262869), CCD_REAL( 0.809012), CCD_REAL(-0.525738)), + CCD_VEC3_STATIC(CCD_REAL( 0.951058), CCD_REAL( 0.309013), CCD_REAL( 0.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.951058), CCD_REAL(-0.309013), CCD_REAL( 0.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.587786), CCD_REAL(-0.809017), CCD_REAL( 0.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.000000), CCD_REAL(-1.000000), CCD_REAL( 0.000000)), + CCD_VEC3_STATIC(CCD_REAL(-0.587786), CCD_REAL(-0.809017), CCD_REAL( 0.000000)), + CCD_VEC3_STATIC(CCD_REAL(-0.951058), CCD_REAL(-0.309013), CCD_REAL(-0.000000)), + CCD_VEC3_STATIC(CCD_REAL(-0.951058), CCD_REAL( 0.309013), CCD_REAL(-0.000000)), + CCD_VEC3_STATIC(CCD_REAL(-0.587786), CCD_REAL( 0.809017), CCD_REAL(-0.000000)), + CCD_VEC3_STATIC(CCD_REAL(-0.000000), CCD_REAL( 1.000000), CCD_REAL(-0.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.587786), CCD_REAL( 0.809017), CCD_REAL(-0.000000)), + CCD_VEC3_STATIC(CCD_REAL( 0.688190), CCD_REAL(-0.499997), CCD_REAL( 0.525736)), + CCD_VEC3_STATIC(CCD_REAL(-0.262869), CCD_REAL(-0.809012), CCD_REAL( 0.525738)), + CCD_VEC3_STATIC(CCD_REAL(-0.850648), CCD_REAL( 0.000000), CCD_REAL( 0.525736)), + CCD_VEC3_STATIC(CCD_REAL(-0.262869), CCD_REAL( 0.809012), CCD_REAL( 0.525738)), + CCD_VEC3_STATIC(CCD_REAL( 0.688190), CCD_REAL( 0.499997), CCD_REAL( 0.525736)), + CCD_VEC3_STATIC(CCD_REAL( 0.525730), CCD_REAL( 0.000000), CCD_REAL( 0.850652)), + CCD_VEC3_STATIC(CCD_REAL( 0.162456), CCD_REAL(-0.499995), CCD_REAL( 0.850654)), + CCD_VEC3_STATIC(CCD_REAL(-0.425323), CCD_REAL(-0.309011), CCD_REAL( 0.850654)), + CCD_VEC3_STATIC(CCD_REAL(-0.425323), CCD_REAL( 0.309011), CCD_REAL( 0.850654)), + CCD_VEC3_STATIC(CCD_REAL( 0.162456), CCD_REAL( 0.499995), CCD_REAL( 0.850654)) +}; +ccd_vec3_t *ccd_points_on_sphere = points_on_sphere; +size_t ccd_points_on_sphere_len = sizeof(points_on_sphere) / sizeof(ccd_vec3_t); + + +_ccd_inline ccd_real_t __ccdVec3PointSegmentDist2(const ccd_vec3_t *P, + const ccd_vec3_t *x0, + const ccd_vec3_t *b, + ccd_vec3_t *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + ccd_real_t dist, t; + ccd_vec3_t d, a; + + // direction of segment + ccdVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + ccdVec3Sub2(&a, x0, P); + + t = -CCD_REAL(1.) * ccdVec3Dot(&a, &d); + t /= ccdVec3Len2(&d); + + if (t < CCD_ZERO || ccdIsZero(t)){ + dist = ccdVec3Dist2(x0, P); + if (witness) + ccdVec3Copy(witness, x0); + }else if (t > CCD_ONE || ccdEq(t, CCD_ONE)){ + dist = ccdVec3Dist2(b, P); + if (witness) + ccdVec3Copy(witness, b); + }else{ + if (witness){ + ccdVec3Copy(witness, &d); + ccdVec3Scale(witness, t); + ccdVec3Add(witness, x0); + dist = ccdVec3Dist2(witness, P); + }else{ + // recycling variables + ccdVec3Scale(&d, t); + ccdVec3Add(&d, &a); + dist = ccdVec3Len2(&d); + } + } + + return dist; +} + +ccd_real_t ccdVec3PointSegmentDist2(const ccd_vec3_t *P, + const ccd_vec3_t *x0, const ccd_vec3_t *b, + ccd_vec3_t *witness) +{ + return __ccdVec3PointSegmentDist2(P, x0, b, witness); +} + +ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P, + const ccd_vec3_t *x0, const ccd_vec3_t *B, + const ccd_vec3_t *C, + ccd_vec3_t *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + ccd_vec3_t d1, d2, a; + ccd_real_t u, v, w, p, q, r, d; + ccd_real_t s, t, dist, dist2; + ccd_vec3_t witness2; + + ccdVec3Sub2(&d1, B, x0); + ccdVec3Sub2(&d2, C, x0); + ccdVec3Sub2(&a, x0, P); + + u = ccdVec3Dot(&a, &a); + v = ccdVec3Dot(&d1, &d1); + w = ccdVec3Dot(&d2, &d2); + p = ccdVec3Dot(&a, &d1); + q = ccdVec3Dot(&a, &d2); + r = ccdVec3Dot(&d1, &d2); + + d = w * v - r * r; + if (ccdIsZero(d)){ + // To avoid division by zero for zero (or near zero) area triangles + s = t = -1.; + }else{ + s = (q * r - w * p) / d; + t = (-s * r - q) / w; + } + + if ((ccdIsZero(s) || s > CCD_ZERO) + && (ccdEq(s, CCD_ONE) || s < CCD_ONE) + && (ccdIsZero(t) || t > CCD_ZERO) + && (ccdEq(t, CCD_ONE) || t < CCD_ONE) + && (ccdEq(t + s, CCD_ONE) || t + s < CCD_ONE)){ + + if (witness){ + ccdVec3Scale(&d1, s); + ccdVec3Scale(&d2, t); + ccdVec3Copy(witness, x0); + ccdVec3Add(witness, &d1); + ccdVec3Add(witness, &d2); + + dist = ccdVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += CCD_REAL(2.) * s * t * r; + dist += CCD_REAL(2.) * s * p; + dist += CCD_REAL(2.) * t * q; + dist += u; + } + }else{ + dist = __ccdVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = __ccdVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + ccdVec3Copy(witness, &witness2); + } + + dist2 = __ccdVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + ccdVec3Copy(witness, &witness2); + } + } + + return dist; +} diff --git a/3rdparty/rbdl/.clang-format b/3rdparty/rbdl/.clang-format new file mode 100644 index 0000000..1d44113 --- /dev/null +++ b/3rdparty/rbdl/.clang-format @@ -0,0 +1,13 @@ +--- +BasedOnStyle: Google +AlignAfterOpenBracket: AlwaysBreak +AlignOperands: 'true' +AllowAllArgumentsOnNextLine: 'false' +AllowAllParametersOfDeclarationOnNextLine: 'false' +BinPackArguments: 'false' +BinPackParameters: 'false' +BreakBeforeBinaryOperators: NonAssignment +ExperimentalAutoDetectBinPacking: 'false' +ReflowComments: 'false' + +... diff --git a/3rdparty/rbdl/.editorconfig b/3rdparty/rbdl/.editorconfig new file mode 100644 index 0000000..daf9d70 --- /dev/null +++ b/3rdparty/rbdl/.editorconfig @@ -0,0 +1,12 @@ +root = true + +[*] +end_of_line = lf +indent_style = space +indent_size = 2 +charset = utf-8 +insert_final_newline = true +trim_trailing_whitespace = true + +[Makefile] +indent_style = tab diff --git a/3rdparty/rbdl/.gitignore b/3rdparty/rbdl/.gitignore new file mode 100644 index 0000000..abd1bb0 --- /dev/null +++ b/3rdparty/rbdl/.gitignore @@ -0,0 +1,40 @@ +syntax: glob + +doc/html/* +doc/notes/*.aux +doc/notes/*.pdf + +*Debug/* +*Release/* +*RelWithDebInfo/* +*build/* +*build2/* + +.*.swp +CMakeFiles/* +cmake_install.cmake +CMakeCache.txt +Makefile +tags +*.log + +*.orig +*.user + +src/rbdl_config.h + +# autogenerated by python/wrappergen.py +python/rbdl.pyx + +examples/simple/CMakeCache.txt +examples/simple/example +examples/simple/librbdl.so + +examples/luamodel/example_luamodel +examples/luamodel/CMakeCache.txt + +*.vcxproj +*.vcxproj.filters +*.suo +*.db +*.sln diff --git a/3rdparty/rbdl/.hgignore b/3rdparty/rbdl/.hgignore new file mode 100644 index 0000000..5fd14da --- /dev/null +++ b/3rdparty/rbdl/.hgignore @@ -0,0 +1,38 @@ +syntax: glob + +doc/html/* +doc/notes/*.aux +doc/notes/*.pdf + +*Debug/* +*Release/* +*RelWithDebInfo/* +*build*/* + +.*.swp +CMakeFiles/* +cmake_install.cmake +CMakeCache.txt +Makefile +tags +*.log + +*.user + +src/rbdl_config.h + +# autogenerated by python/wrappergen.py +python/rbdl.pyx + +examples/simple/CMakeCache.txt +examples/simple/example +examples/simple/librbdl.so + +examples/luamodel/example_luamodel +examples/luamodel/CMakeCache.txt + +*.vcxproj +*.vcxproj.filters +*.suo +*.db +*.sln diff --git a/3rdparty/rbdl/.travis.yml b/3rdparty/rbdl/.travis.yml new file mode 100644 index 0000000..0faca46 --- /dev/null +++ b/3rdparty/rbdl/.travis.yml @@ -0,0 +1,43 @@ +language: cpp + +os: linux + +dist: trusty + +addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - cmake-data + - cmake + - libunittest++-dev + - libeigen3-dev + +jobs: + include: + - name: "SimpleMath - gcc" + env: CMAKE_DEFINES="-DRBDL_USE_SIMPLE_MATH=ON -DRBDL_BUILD_TESTS=ON -DRBDL_BUILD_ADDON_BENCHMARK=ON" + compiler: gcc + - name: "SimpleMath - clang" + env: CMAKE_DEFINES="-DRBDL_USE_SIMPLE_MATH=ON -DRBDL_BUILD_TESTS=ON -DRBDL_BUILD_ADDON_BENCHMARK=ON" + compiler: clang + - name: "EigenMath - gcc" + env: CMAKE_DEFINES="-DRBDL_USE_SIMPLE_MATH=OFF -DRBDL_BUILD_TESTS=ON -DRBDL_BUILD_ADDON_BENCHMARK=ON" + compiler: gcc + - name: "EigenMath - clang" + env: CMAKE_DEFINES="-DRBDL_USE_SIMPLE_MATH=OFF -DRBDL_BUILD_TESTS=ON -DRBDL_BUILD_ADDON_BENCHMARK=ON" + compiler: clang + +before_script: + - mkdir build + - cd build + - cmake -L -DCMAKE_CXX_FLAGS="-Wall -Wpedantic -Wextra -Werror" $CMAKE_DEFINES .. + +script: + # Compile RBDL + - make + # Run unit tests + - ./tests/runtests + # Run benchmarks + - ./addons/benchmark/benchmark diff --git a/3rdparty/rbdl/CMake/FindCython.cmake b/3rdparty/rbdl/CMake/FindCython.cmake new file mode 100644 index 0000000..04aed1f --- /dev/null +++ b/3rdparty/rbdl/CMake/FindCython.cmake @@ -0,0 +1,44 @@ +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_EXECUTABLE +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if( PYTHONINTERP_FOUND ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + HINTS ${_python_path} + ) +else() + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + ) +endif() + + +include( FindPackageHandleStandardArgs ) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE ) + +mark_as_advanced( CYTHON_EXECUTABLE ) diff --git a/3rdparty/rbdl/CMake/FindEigen3.cmake b/3rdparty/rbdl/CMake/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/CMake/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/3rdparty/rbdl/CMake/FindIPOPT.cmake b/3rdparty/rbdl/CMake/FindIPOPT.cmake new file mode 100644 index 0000000..3a272f8 --- /dev/null +++ b/3rdparty/rbdl/CMake/FindIPOPT.cmake @@ -0,0 +1,44 @@ +SET (IPOPT_FOUND FALSE) + + +UNSET( IPOPT_INCLUDE_DIR CACHE) +UNSET( IPOPT_LIBRARY CACHE) + +IF(CUSTOM_IPOPT_PATH) + + FIND_PATH (IPOPT_INCLUDE_DIR coin/IpTNLP.hpp + PATHS + ${CUSTOM_IPOPT_PATH}/include + NO_DEFAULT_PATH + ) + + FIND_LIBRARY (IPOPT_LIBRARY ipopt + PATHS + ${CUSTOM_IPOPT_PATH}/lib + NO_DEFAULT_PATH + ) +ENDIF(CUSTOM_IPOPT_PATH) + +IF (IPOPT_INCLUDE_DIR AND IPOPT_LIBRARY) + SET (IPOPT_FOUND TRUE) +ELSE(IPOPT_INCLUDE_DIR AND IPOPT_LIBRARY) + IF(IPOPT_FIND_REQUIRED) + MESSAGE (SEND_ERROR " Could not find IPOPT.") + MESSAGE (SEND_ERROR " Try setting CUSTOM_IPOPT_PATH in FindIPOPT.cmake force CMake to use the desired directory.") + ELSE(IPOPT_FIND_REQUIRED) + MESSAGE (STATUS " Could not find IPOPT.") + MESSAGE (STATUS " Try setting CUSTOM_IPOPT_PATH in FindIPOPT.cmake force CMake to use the desired directory.") + ENDIF(IPOPT_FIND_REQUIRED) +ENDIF (IPOPT_INCLUDE_DIR AND IPOPT_LIBRARY) + +IF (IPOPT_FOUND) + IF (NOT IPOPT_FIND_QUIETLY) + MESSAGE(STATUS "Found IPOPT: ${IPOPT_LIBRARY}") + ENDIF (NOT IPOPT_FIND_QUIETLY) + +ENDIF (IPOPT_FOUND) + +MARK_AS_ADVANCED ( + IPOPT_INCLUDE_DIR + IPOPT_LIBRARY + ) diff --git a/3rdparty/rbdl/CMake/FindNumPy2.cmake b/3rdparty/rbdl/CMake/FindNumPy2.cmake new file mode 100644 index 0000000..32a68e9 --- /dev/null +++ b/3rdparty/rbdl/CMake/FindNumPy2.cmake @@ -0,0 +1,34 @@ +# Find the Python NumPy package +# PYTHON_NUMPY_2_INCLUDE_DIR +# PYTHON_NUMPY_2_FOUND +# will be set by this script + +cmake_minimum_required(VERSION 2.6) + +set(PYTHON_EXECUTABLE "python2") + +if (PYTHON_EXECUTABLE) + # Find out the include path + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.get_include(), end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_path) + # And the version + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.__version__, end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_version) +elseif(__numpy_out) + message(STATUS "Python executable not found.") +endif(PYTHON_EXECUTABLE) + +find_path(PYTHON_NUMPY_2_INCLUDE_DIR numpy/arrayobject.h + HINTS "${__numpy_path}" "${PYTHON_INCLUDE_PATH}" NO_DEFAULT_PATH) + +if(PYTHON_NUMPY_2_INCLUDE_DIR) + set(PYTHON_NUMPY_2_FOUND 1 CACHE INTERNAL "Python numpy found") +endif(PYTHON_NUMPY_2_INCLUDE_DIR) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(NumPy REQUIRED_VARS PYTHON_NUMPY_2_INCLUDE_DIR +VERSION_VAR __numpy_version) diff --git a/3rdparty/rbdl/CMake/FindNumPy3.cmake b/3rdparty/rbdl/CMake/FindNumPy3.cmake new file mode 100644 index 0000000..15a3725 --- /dev/null +++ b/3rdparty/rbdl/CMake/FindNumPy3.cmake @@ -0,0 +1,34 @@ +# Find the Python NumPy package +# PYTHON_NUMPY_3_INCLUDE_DIR +# PYTHON_NUMPY_3_FOUND +# will be set by this script + +cmake_minimum_required(VERSION 2.6) + +set(PYTHON_EXECUTABLE "python3") + +if (PYTHON_EXECUTABLE) + # Find out the include path + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.get_include(), end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_path) + # And the version + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.__version__, end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_version) +elseif(__numpy_out) + message(STATUS "Python executable not found.") +endif(PYTHON_EXECUTABLE) + +find_path(PYTHON_NUMPY_3_INCLUDE_DIR numpy/arrayobject.h + HINTS "${__numpy_path}" "${PYTHON_INCLUDE_PATH}" NO_DEFAULT_PATH) + +if(PYTHON_NUMPY_3_INCLUDE_DIR) + set(PYTHON_NUMPY_3_FOUND 1 CACHE INTERNAL "Python numpy found") +endif(PYTHON_NUMPY_3_INCLUDE_DIR) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(NumPy REQUIRED_VARS PYTHON_NUMPY_3_INCLUDE_DIR +VERSION_VAR __numpy_version) diff --git a/3rdparty/rbdl/CMake/FindUnitTest++.cmake b/3rdparty/rbdl/CMake/FindUnitTest++.cmake new file mode 100644 index 0000000..44f7c90 --- /dev/null +++ b/3rdparty/rbdl/CMake/FindUnitTest++.cmake @@ -0,0 +1,42 @@ +# - Try to find UnitTest++ +# +# + +SET (UNITTEST++_FOUND FALSE) + +FIND_PATH (UNITTEST++_INCLUDE_DIR UnitTest++.h + /usr/include/unittest++ + /usr/include/UnitTest++ + /usr/local/include/unittest++ + /usr/local/include/UnitTest++ + /opt/local/include/unittest++ + $ENV{UNITTESTXX_PATH}/src + $ENV{UNITTESTXX_INCLUDE_PATH} + ) + +FIND_LIBRARY (UNITTEST++_LIBRARY NAMES UnitTest++ PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + $ENV{UNITTESTXX_PATH} + ENV{UNITTESTXX_LIBRARY_PATH} + ) + +IF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY) + SET (UNITTEST++_FOUND TRUE) +ENDIF (UNITTEST++_INCLUDE_DIR AND UNITTEST++_LIBRARY) + +IF (UNITTEST++_FOUND) + IF (NOT UnitTest++_FIND_QUIETLY) + MESSAGE(STATUS "Found UnitTest++: ${UNITTEST++_LIBRARY}") + ENDIF (NOT UnitTest++_FIND_QUIETLY) +ELSE (UNITTEST++_FOUND) + IF (UnitTest++_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find UnitTest++") + ENDIF (UnitTest++_FIND_REQUIRED) +ENDIF (UNITTEST++_FOUND) + +MARK_AS_ADVANCED ( + UNITTEST++_INCLUDE_DIR + UNITTEST++_LIBRARY + ) diff --git a/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake b/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake new file mode 100644 index 0000000..69e638d --- /dev/null +++ b/3rdparty/rbdl/CMake/ReplicatePythonSourceTree.cmake @@ -0,0 +1,4 @@ +# Note: when executed in the build dir, then CMAKE_CURRENT_SOURCE_DIR is the +# build dir. +file( COPY src test bin DESTINATION "${CMAKE_ARGV3}" + FILES_MATCHING PATTERN "*.py" ) diff --git a/3rdparty/rbdl/CMake/UseCython.cmake b/3rdparty/rbdl/CMake/UseCython.cmake new file mode 100644 index 0000000..40d3ab1 --- /dev/null +++ b/3rdparty/rbdl/CMake/UseCython.cmake @@ -0,0 +1,295 @@ +# Define a function to create Cython modules. +# +# For more information on the Cython project, see http://cython.org/. +# "Cython is a language that makes writing C extensions for the Python language +# as easy as Python itself." +# +# This file defines a CMake function to build a Cython Python module. +# To use it, first include this file. +# +# include( UseCython ) +# +# Then call cython_add_module to create a module. +# +# cython_add_module( ... ) +# +# To create a standalone executable, the function +# +# cython_add_standalone_executable( [MAIN_MODULE src1] ... ) +# +# To avoid dependence on Python, set the PYTHON_LIBRARY cache variable to point +# to a static library. If a MAIN_MODULE source is specified, +# the "if __name__ == '__main__':" from that module is used as the C main() method +# for the executable. If MAIN_MODULE, the source with the same basename as +# is assumed to be the MAIN_MODULE. +# +# Where is the name of the resulting Python module and +# ... are source files to be compiled into the module, e.g. *.pyx, +# *.py, *.c, *.cxx, etc. A CMake target is created with name . This can +# be used for target_link_libraries(), etc. +# +# The sample paths set with the CMake include_directories() command will be used +# for include directories to search for *.pxd when running the Cython complire. +# +# Cache variables that effect the behavior include: +# +# CYTHON_ANNOTATE +# CYTHON_NO_DOCSTRINGS +# CYTHON_FLAGS +# +# Source file properties that effect the build process are +# +# CYTHON_IS_CXX +# +# If this is set of a *.pyx file with CMake set_source_files_properties() +# command, the file will be compiled as a C++ file. +# +# See also FindCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Configuration options. +set( CYTHON_ANNOTATE OFF + CACHE BOOL "Create an annotated .html file when compiling *.pyx." ) +set( CYTHON_NO_DOCSTRINGS OFF + CACHE BOOL "Strip docstrings from the compiled module." ) +set( CYTHON_FLAGS "" CACHE STRING + "Extra flags to the cython compiler." ) +mark_as_advanced( CYTHON_ANNOTATE CYTHON_NO_DOCSTRINGS CYTHON_FLAGS ) + +find_package( Cython REQUIRED ) +find_package( PythonLibs REQUIRED ) + +set( CYTHON_CXX_EXTENSION "cxx" ) +set( CYTHON_C_EXTENSION "c" ) + +# Create a *.c or *.cxx file from a *.pyx file. +# Input the generated file basename. The generate file will put into the variable +# placed in the "generated_file" argument. Finally all the *.py and *.pyx files. +function( compile_pyx _name generated_file ) + # Default to assuming all files are C. + set( cxx_arg "" ) + set( extension ${CYTHON_C_EXTENSION} ) + set( pyx_lang "C" ) + set( comment "Compiling Cython C source for ${_name}..." ) + + set( cython_include_directories "" ) + set( pxd_dependencies "" ) + set( c_header_dependencies "" ) + set( pyx_locations "" ) + + foreach( pyx_file ${ARGN} ) + get_filename_component( pyx_file_basename "${pyx_file}" NAME_WE ) + + # Determine if it is a C or C++ file. + get_source_file_property( property_is_cxx ${pyx_file} CYTHON_IS_CXX ) + if( ${property_is_cxx} STREQUAL "TRUE" ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython CXX source for ${_name}..." ) + endif() + + # Get the include directories. + get_source_file_property( pyx_location ${pyx_file} LOCATION ) + get_filename_component( pyx_path ${pyx_location} PATH ) + #get_directory_property( cmake_include_directories DIRECTORY ${pyx_path} INCLUDE_DIRECTORIES ) + list( APPEND cython_include_directories ${pyx_path} ) + list( APPEND pyx_locations "${pyx_location}" ) + + # Determine dependencies. + # Add the pxd file will the same name as the given pyx file. + unset( corresponding_pxd_file CACHE ) + find_file( corresponding_pxd_file ${pyx_file_basename}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} + NO_DEFAULT_PATH ) + if( corresponding_pxd_file ) + list( APPEND pxd_dependencies "${corresponding_pxd_file}" ) + endif() + + # pxd files to check for additional dependencies. + set( pxds_to_check "${pyx_file}" "${pxd_dependencies}" ) + set( pxds_checked "" ) + set( number_pxds_to_check 1 ) + while( ${number_pxds_to_check} GREATER 0 ) + foreach( pxd ${pxds_to_check} ) + list( APPEND pxds_checked "${pxd}" ) + list( REMOVE_ITEM pxds_to_check "${pxd}" ) + + # check for C header dependencies + file( STRINGS "${pxd}" extern_from_statements + REGEX "cdef[ ]+extern[ ]+from.*$" ) + foreach( statement ${extern_from_statements} ) + # Had trouble getting the quote in the regex + string( REGEX REPLACE "cdef[ ]+extern[ ]+from[ ]+[\"]([^\"]+)[\"].*" "\\1" header "${statement}" ) + unset( header_location CACHE ) + find_file( header_location ${header} PATHS ${cmake_include_directories} ) + if( header_location ) + list( FIND c_header_dependencies "${header_location}" header_idx ) + if( ${header_idx} LESS 0 ) + list( APPEND c_header_dependencies "${header_location}" ) + endif() + endif() + endforeach() + + # check for pxd dependencies + + # Look for cimport statements. + set( module_dependencies "" ) + file( STRINGS "${pxd}" cimport_statements REGEX cimport ) + foreach( statement ${cimport_statements} ) + if( ${statement} MATCHES from ) + string( REGEX REPLACE "from[ ]+([^ ]+).*" "\\1" module "${statement}" ) + else() + string( REGEX REPLACE "cimport[ ]+([^ ]+).*" "\\1" module "${statement}" ) + endif() + list( APPEND module_dependencies ${module} ) + endforeach() + list( REMOVE_DUPLICATES module_dependencies ) + # Add the module to the files to check, if appropriate. + foreach( module ${module_dependencies} ) + unset( pxd_location CACHE ) + find_file( pxd_location ${module}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} NO_DEFAULT_PATH ) + if( pxd_location ) + list( FIND pxds_checked ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( FIND pxds_to_check ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( APPEND pxds_to_check ${pxd_location} ) + list( APPEND pxd_dependencies ${pxd_location} ) + endif() # if it is not already going to be checked + endif() # if it has not already been checked + endif() # if pxd file can be found + endforeach() # for each module dependency discovered + endforeach() # for each pxd file to check + list( LENGTH pxds_to_check number_pxds_to_check ) + endwhile() + endforeach() # pyx_file + + # Set additional flags. + if( CYTHON_ANNOTATE ) + set( annotate_arg "--annotate" ) + endif() + + if( CYTHON_NO_DOCSTRINGS ) + set( no_docstrings_arg "--no-docstrings" ) + endif() + + if( "${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR + "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo" ) + set( cython_debug_arg "--gdb" ) + endif() + + if( "${PYTHONLIBS_VERSION_STRING}" MATCHES "^2." ) + set( version_arg "-2" ) + elseif( "${PYTHONLIBS_VERSION_STRING}" MATCHES "^3." ) + set( version_arg "-3" ) + else() + set( version_arg ) + endif() + + # Include directory arguments. + list( REMOVE_DUPLICATES cython_include_directories ) + set( include_directory_arg "" ) + foreach( _include_dir ${cython_include_directories} ) + set( include_directory_arg ${include_directory_arg} "-I" "${_include_dir}" ) + endforeach() + + # Determining generated file name. + set( _generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.${extension}" ) + set_source_files_properties( ${_generated_file} PROPERTIES GENERATED TRUE ) + set( ${generated_file} ${_generated_file} PARENT_SCOPE ) + + list( REMOVE_DUPLICATES pxd_dependencies ) + list( REMOVE_DUPLICATES c_header_dependencies ) + + # Add the command to run the compiler. + add_custom_command( OUTPUT ${_generated_file} + COMMAND ${CYTHON_EXECUTABLE} + ARGS ${cxx_arg} ${include_directory_arg} ${version_arg} + ${annotate_arg} ${no_docstrings_arg} ${cython_debug_arg} ${CYTHON_FLAGS} + --output-file ${_generated_file} ${pyx_locations} + DEPENDS ${pyx_locations} ${pxd_dependencies} + IMPLICIT_DEPENDS ${pyx_lang} ${c_header_dependencies} + COMMENT ${comment} + ) + + # Remove their visibility to the user. + set( corresponding_pxd_file "" CACHE INTERNAL "" ) + set( header_location "" CACHE INTERNAL "" ) + set( pxd_location "" CACHE INTERNAL "" ) +endfunction() + +# cython_add_module( src1 src2 ... srcN ) +# Build the Cython Python module. +function( cython_add_module _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + foreach( _file ${ARGN} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + list( APPEND pyx_module_sources ${_file} ) + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + compile_pyx( ${_name} generated_file ${pyx_module_sources} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + python_add_module( ${_name} ${generated_file} ${other_module_sources} ) + if( APPLE ) + set_target_properties( ${_name} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" ) + else() + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ) + endif() +endfunction() + +include( CMakeParseArguments ) +# cython_add_standalone_executable( _name [MAIN_MODULE src3.py] src1 src2 ... srcN ) +# Creates a standalone executable the given sources. +function( cython_add_standalone_executable _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + set( main_module "" ) + cmake_parse_arguments( cython_arguments "" "MAIN_MODULE" "" ${ARGN} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + foreach( _file ${cython_arguments_UNPARSED_ARGUMENTS} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + get_filename_component( _file_we ${_file} NAME_WE ) + if( "${_file_we}" STREQUAL "${_name}" ) + set( main_module "${_file}" ) + elseif( NOT "${_file}" STREQUAL "${cython_arguments_MAIN_MODULE}" ) + set( PYTHON_MODULE_${_file_we}_static_BUILD_SHARED OFF ) + compile_pyx( "${_file_we}_static" generated_file "${_file}" ) + list( APPEND pyx_module_sources "${generated_file}" ) + endif() + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + + if( cython_arguments_MAIN_MODULE ) + set( main_module ${cython_arguments_MAIN_MODULE} ) + endif() + if( NOT main_module ) + message( FATAL_ERROR "main module not found." ) + endif() + get_filename_component( main_module_we "${main_module}" NAME_WE ) + set( CYTHON_FLAGS ${CYTHON_FLAGS} --embed ) + compile_pyx( "${main_module_we}_static" generated_file ${main_module} ) + add_executable( ${_name} ${generated_file} ${pyx_module_sources} ${other_module_sources} ) + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ${pyx_module_libs} ) +endfunction() diff --git a/3rdparty/rbdl/CMakeLists.txt b/3rdparty/rbdl/CMakeLists.txt new file mode 100644 index 0000000..e4d70fc --- /dev/null +++ b/3rdparty/rbdl/CMakeLists.txt @@ -0,0 +1,232 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.15) + +SET ( RBDL_VERSION_MAJOR 2 ) +SET ( RBDL_VERSION_MINOR 6 ) +SET ( RBDL_VERSION_PATCH 0 ) +SET ( RBDL_VERSION + ${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH} +) +SET ( RBDL_SO_VERSION + ${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH} +) +SET (PROJECT_VERSION ${RBDL_VERSION}) +PROJECT (RBDL VERSION ${RBDL_VERSION}) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_BINARY_DIR}/include +) + +# SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") + +INCLUDE(GNUInstallDirs) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +# Set a default build type to 'Release' if none was specified +IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + MESSAGE(STATUS "Setting build type to 'Release' as none was specified.") + SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + SET_PROPERTY(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +ENDIF() + +# Find and use the system's Eigen3 library +FIND_PACKAGE (Eigen3 3.0.0) + +IF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + MESSAGE (WARNING "Could not find Eigen3 on your system. Install it or use the slower SimpleMath library by enabling RBDL_USE_SIMPLE_MATH.") +ENDIF (NOT EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + +IF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + INCLUDE_DIRECTORIES (SYSTEM ${EIGEN3_INCLUDE_DIR}) +ENDIF (EIGEN3_FOUND AND NOT RBDL_USE_SIMPLE_MATH) + +# Options +SET (RBDL_BUILD_STATIC_DEFAULT OFF) +IF (MSVC) + SET (RBDL_BUILD_STATIC_DEFAULT ON) +ENDIF (MSVC) + +OPTION (RBDL_BUILD_STATIC "Build statically linked library (otherwise dynamiclly linked)" ${RBDL_BUILD_STATIC_DEFAULT}) +OPTION (RBDL_BUILD_TESTS "Build the test executables" OFF) +OPTION (RBDL_ENABLE_LOGGING "Enable logging (warning: major impact on performance!)" OFF) +OPTION (RBDL_USE_SIMPLE_MATH "Use slow math instead of the fast Eigen3 library (faster compilation)" OFF) +OPTION (RBDL_STORE_VERSION "Enable storing of version information in the library (requires build from valid repository)" OFF) +OPTION (RBDL_BUILD_ADDON_URDFREADER "Build the (experimental) urdf reader" OFF) +OPTION (RBDL_BUILD_ADDON_BENCHMARK "Build the benchmarking tool" OFF) +OPTION (RBDL_BUILD_ADDON_LUAMODEL "Build the lua model reader" OFF) +OPTION (RBDL_BUILD_PYTHON_WRAPPER "Build experimental python wrapper" OFF) +OPTION (RBDL_USE_PYTHON_2 "Use python 2 instead of python 3" OFF) +OPTION (RBDL_BUILD_ADDON_GEOMETRY "Build the geometry library" OFF) +OPTION (RBDL_BUILD_ADDON_MUSCLE "Build the muscle library" OFF) +OPTION (RBDL_BUILD_ADDON_MUSCLE_FITTING "Build muscle library fitting functions (requires Ipopt)" OFF) + +SET (RBDL_BUILD_COMPILER_ID ${CMAKE_CXX_COMPILER_ID}) +SET (RBDL_BUILD_COMPILER_VERSION ${CMAKE_CXX_COMPILER_VERSION}) + +# Addons +IF (RBDL_BUILD_ADDON_URDFREADER) + ADD_SUBDIRECTORY ( addons/urdfreader ) +ENDIF (RBDL_BUILD_ADDON_URDFREADER) + +IF (RBDL_BUILD_ADDON_BENCHMARK) + ADD_SUBDIRECTORY ( addons/benchmark ) +ENDIF (RBDL_BUILD_ADDON_BENCHMARK) + +IF (RBDL_BUILD_ADDON_LUAMODEL) + ADD_SUBDIRECTORY ( addons/luamodel ) +ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + +IF(RBDL_BUILD_ADDON_MUSCLE) + SET(RBDL_BUILD_ADDON_GEOMETRY ON CACHE BOOL "Build the geometry library" FORCE) + IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + SET(CUSTOM_IPOPT_PATH "" CACHE PATH "Path to specific IPOPT Installation") + ENDIF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + ADD_SUBDIRECTORY ( addons/muscle ) + IF(RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( addons/muscle/tests ) + ENDIF(RBDL_BUILD_TESTS) +ENDIF(RBDL_BUILD_ADDON_MUSCLE) + + +IF(RBDL_BUILD_ADDON_GEOMETRY) + ADD_SUBDIRECTORY ( addons/geometry ) + IF(RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( addons/geometry/tests ) + ENDIF(RBDL_BUILD_TESTS) +ENDIF(RBDL_BUILD_ADDON_GEOMETRY) + + + +IF (RBDL_BUILD_TESTS) + ADD_SUBDIRECTORY ( tests ) +ENDIF (RBDL_BUILD_TESTS) + +# Source files for RBDL +SET ( RBDL_SOURCES + src/rbdl_version.cc + src/rbdl_mathutils.cc + src/rbdl_utils.cc + src/Constraints.cc + src/Dynamics.cc + src/Logging.cc + src/Joint.cc + src/Model.cc + src/Kinematics.cc + ) + +IF (MSVC AND NOT RBDL_BUILD_STATIC) + MESSAGE (FATAL_ERROR, "Compiling RBDL as a DLL currently not supported. Please enable RBDL_BUILD_STATIC.") +ENDIF (MSVC AND NOT RBDL_BUILD_STATIC) + +# Static / dynamic builds +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl-static STATIC ${RBDL_SOURCES} ) + IF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES PREFIX "lib") + ENDIF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl-static PROPERTIES + OUTPUT_NAME "rbdl" + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + ) + + IF (RBDL_BUILD_ADDON_LUAMODEL) + TARGET_LINK_LIBRARIES ( rbdl-static + rbdl_luamodel-static + ) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + INSTALL (TARGETS rbdl-static + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl SHARED ${RBDL_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + ) + + INSTALL (TARGETS rbdl + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +IF (RBDL_STORE_VERSION) + # Set versioning information that can be queried during runtime + 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_CURRENT_SOURCE_DIR}/include/rbdl/rbdl_config.h.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h" + ) + +# Python wrapper +IF (RBDL_BUILD_PYTHON_WRAPPER) + add_subdirectory ( python ) +ENDIF (RBDL_BUILD_PYTHON_WRAPPER) + +# Installation +FILE ( GLOB headers + ${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/*.h + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl/rbdl_config.h + ) + +INSTALL ( FILES ${headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl ) + +# Setup of SimpleMath install settings +IF (RBDL_USE_SIMPLE_MATH) + INSTALL ( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/include/rbdl/SimpleMath" + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/rbdl + ) +ENDIF (RBDL_USE_SIMPLE_MATH) + +# pkg-config +CONFIGURE_FILE ( + ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pc.cmake + ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pc @ONLY + ) +INSTALL ( + FILES ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig + ) + +# Packaging +SET(CPACK_GENERATOR "DEB") +SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Martin Felis ") +SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "the Rigid Body Dynamics Library (RBDL)") +SET(CPACK_PACKAGE_VENDOR "Martin Felis") +SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") +SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +SET(CPACK_PACKAGE_VERSION_MAJOR ${RBDL_VERSION_MAJOR}) +SET(CPACK_PACKAGE_VERSION_MINOR ${RBDL_VERSION_MINOR}) +SET(CPACK_PACKAGE_VERSION_PATCH ${RBDL_VERSION_PATCH}) +SET(CPACK_PACKAGE_INSTALL_DIRECTORY "CPACK_PACKAGE ${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}") +SET(CPACK_PACKAGE_FILE_NAME "rbdl-${CMAKE_LIBRARY_ARCHITECTURE}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}") +SET(CPACK_PACKAGE_EXECUTABLES "rbdl_luamodel_util;RBDL Lua Model Utility") + +INCLUDE(CPack) diff --git a/3rdparty/rbdl/CONTRIBUTING.md b/3rdparty/rbdl/CONTRIBUTING.md new file mode 100644 index 0000000..d1732a3 --- /dev/null +++ b/3rdparty/rbdl/CONTRIBUTING.md @@ -0,0 +1,173 @@ +# Coding style for RBDL + +This documents gives an overview of the coding style used in RBDL and also +the general goals of RBDL. + +If you are considering contributing to this library please read the whole +document. + +## General Purpose of RBDL + +RBDL implements large parts of the algorithms and methods described in +Featherstone's book Rigid Body Dynamics Algorithms. One of the main goals +of this library is to serve as an efficient implementation for the methods +described in Featherstone's book and to a lesser extent general multibody +dynamics and kinematics computations. A person who is familiar with +Featherstone's book should have an easy time to understand the code and +therefore the variable naming conventions used in the book should when +possible used in the code, e.g.: + + - the joint space inertia matrix is denoted with H + - the coriolis forces are denoted with C + +The algorithmic parts of RBDL's code try to follow the algorithmic or +mathematical notations instead of wrapping algorithms in elegant +programming patterns. + +### Aims and Non-Aims of RBDL + +This is what RBDL aims to be: + +* RBDL aims to be lean +* RBDL aims to be easily integrated into other projects +* RBDL aims to be suitable as a foundation for sophisticated dynamics packages +* RBDL gives you access to its internals and provides only a thin abstraction layer over the actual computation +* RBDL aims to be self-contained and dependant on as few libraries as possible + +And this is what RBDL is ***not*** about: + +* RBDL is ***not*** a fully fledged simulator with collision detection or fancy graphics +* RBDL does not keep you from screwing up things. + +Multibody dynamics is a complicated subject and in this codebase the +preference is mathematical and algorithmic clarity over elegant software +architecture. + +## Licensing + +RBDL is published under the very permissive zlib license that gives you a +lot of freedom in the use of full library or parts of it. The core part +of the library is solely using this license but addons may use different +licenses. + +There is no formal contributor license agreement for this project. Instead +when you submit patches or create a pull request it is assumed that you +have the rights to transfer the corresponding code to the RBDL project and +that you are okay that the code will be published as part of RBDL. + +## Data Storage + +RBDL tries to avoid dynamic allocations and prefers contiguous memory +storage such as in ```std::vector```s over possibly fragmented memory as in +```std::list``` or heap allocated tree structures. + +Where possible we use the Structure-of-Arrays (SOA) approach to store data, +e.g. the velocities v of all bodies is stored in an array (```std::vector```) +of ```SpatialVector```s in the ```Model``` structure. + +## Naming Conventions + +1. Structs and classes use CamelCase, e.g. ```ConstraintSet``` +2. Struct and class members use the lowerCamelCase convention, e.g. + ```Model::dofCount```. + Exceptions are: + 1. The member variable is a mathematical symbol in an + algorithm reference, E.g. ```S``` is commonly used to denote the joint + motion subspace, then we use the algorithm notation. For mathematical + symbols we also allow the underscore ```_``` to denote a subscript. + 2. Specializations of existing variables may be prefixed with an identifier, + followed by a underscore. E.g. ```Model::S``` is the default storage for + joint motion subspaces, however for the specialized 3-DOF joints it uses + the prefix ```multdof3_``` and are therefore stored in + ```Model::multdof3_S```. +3. Only the first letter of an acronym is using a capital letter, e.g. + degree of freedom (DOF) would be used as ```jointDofCount```, or + ```dofCount```. +4. Variables that are not member variables use the ```snake_case``` convention. + +### Examples + + struct Model { + std::vector v; // ok, v is an + std::vector S; // ok, S is commonly used in a reference algorithm + std::vector u; // ok + std::vector multdof3_u; // ok, 3-dof specialization of Model::u + + std::vector mJointIndex; // NOT OK: invalid prefix + unsigned int DOFCount; // NOT OK: only first letter of abbreviation should be in upper case + double error_tol; // NOT OK: use camelCase instead of snake_case + void CalcPositions(); // NOT OK: camelCase for member variables and function must start with lower-case name + + }; + +## Spacing and Line Width + +We use spaces to indent code and use two spaces to indent a block. Do not +use tabs. Namespaces should not be indented. + +Lines should not exceed 80 characters in width. + +Hint: in the root directory of the RBDL repository you find the file +.editorconfig which uses the correct spacing settings automatically for +many editors (see http://editorconfig.org/ for more details). + +## Error Handling + +RBDL will fail loudly and abort if an error occurs. This allows you to spot +errors early on. RBDL does not use exceptions. + +Code must compile without warnings with all compiler warnings enabled. +Please also consider checking code with static code analyzers such as +clang-analyzer (http://clang-analyzer.llvm.org/). + +## Const Correctness + +This code uses const correctness, i.e. parameters that are not expected to +change must be specified as const. Use const references whenever possible. +For some optional variables we use pointers, but when possible use +references. + +## Braces + +Use braces whenever possible. E.g. even there is only a single line of code +after an if-statement wrap it with curly braces. The opening brace starts +in the same line as the ```if``` or the ```else``` statement. + +## Documentation + +Most importantly the code should be readable to someone who is familiar +with multibody dynamics, especially with Featherstone's notation. The +documentation should mainly serve to clarify the API in terms of doxygen +comments. Within the code itself comments may be used to emphasize on ideas +behind it or to clarify sections. But in general it is best to write +readable code in the first place as comments easily become deprecated. + +The doxygen comments should be written in the header files and not in the +```.cc``` files. + +## Testing + +All code contributions must provide unit tests. RBDL uses UnitTest++ +(https://github.com/unittest-cpp/unittest-cpp) as a testing framework. Many +small tests that check single features are preferred over large tests that +test multiple things simultaneously. + +Bugfixes ideally come with a test case that reproduce the bug. + +### Working on a new feature + +The following steps are advised when working on a new feature for RBDL: + +1. Fork the official repository at https://github.com/rbdl/rbdl +2. Create a new branch for your work that branches off the official dev + branch. +3. Perform your changes in your branch. +4. When ready perform a pull request against the dev branch. + +## Debugging + +* Todo: mention logging facility +* Todo: mention SimpleMath as a fast-compiling (but slower runtime) linear +algebra package. + + diff --git a/3rdparty/rbdl/Doxyfile b/3rdparty/rbdl/Doxyfile new file mode 100644 index 0000000..42085b8 --- /dev/null +++ b/3rdparty/rbdl/Doxyfile @@ -0,0 +1,1838 @@ +# Doxyfile 1.7.6.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project +# +# All text after a hash (#) is considered a comment and will be ignored +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" ") + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "Rigid Body Dynamics Library" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./doc + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = YES + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 2 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given extension. +# Doxygen has a built-in mapping, but you can override or extend it using this +# tag. The format is ext=language, where ext is a file extension, and language +# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C, +# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make +# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C +# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions +# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = YES + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate getter +# and setter methods for a property. Setting this option to YES (the default) +# will make doxygen replace the get and set methods by a property in the +# documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = YES + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = NO + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = YES + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = YES + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# If the sources in your project are distributed over multiple directories +# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy +# in the documentation. The default is NO. + +SHOW_DIRECTORIES = NO + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. The create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = ./src \ + include/rbdl \ + doc/api_changes.txt \ + doc/example.h \ + doc/python_example.h \ + doc/Mainpage.h \ + doc/images \ + doc/luamodel_example.h \ + addons/luamodel/luamodel.h \ + addons/urdfreader/rbdl_urdfreader.h \ + addons/urdfreader/urdfreader.h \ + addons/geometry/Function.h \ + addons/geometry/SegmentedQuinticBezierToolkit.h \ + addons/geometry/SmoothSegmentedFunction.h \ + addons/muscle/Millard2016TorqueMuscle.h \ + addons/muscle/MuscleFunctionFactory.h \ + addons/muscle/TorqueMuscleFunctionFactory.h \ + addons/muscle/TorqueMuscleFittingToolkit.h \ + ./include/rbdl + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.d \ + *.java \ + *.ii \ + *.ixx \ + *.ipp \ + *.i++ \ + *.inl \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.idl \ + *.odl \ + *.cs \ + *.php \ + *.php3 \ + *.inc \ + *.m \ + *.mm \ + *.dox \ + *.py \ + *.f90 \ + *.f \ + *.vhd \ + *.vhdl + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = ./include/rbdl/SimpleMath + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = doc \ + examples/luamodel \ + examples/simple \ + examples/python + + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = doc/images \ + doc \ + doc/logo \ + + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = YES + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet. Note that doxygen will try to copy +# the style sheet file to the HTML output directory, so don't put your own +# style sheet in the HTML output directory as well, or it will be erased! + +HTML_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. For this to work a browser that supports +# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox +# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). + +HTML_DYNAMIC_SECTIONS = YES + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 3 + +# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories, +# and Class Hierarchy pages using a tree view instead of an ordered list. + +USE_INLINE_TREES = YES + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 248 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 12 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the +# mathjax.org site, so you can quickly see the result without installing +# MathJax, but it is strongly recommended to install a local copy of MathJax +# before deployment. + +MATHJAX_RELPATH = http://www.mathjax.org/mathjax + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a PHP enabled web server instead of at the web client +# using Javascript. Doxygen will generate the search PHP script and index +# file to put on the web server. The advantage of the server +# based approach is that it scales better to large projects and allows +# full text search. The disadvantages are that it is more difficult to setup +# and does not have live searching capabilities. + +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4wide + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = amsmath xr amsfonts + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = YES + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. This is useful +# if you want to understand what is going on. On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = RBDL_BUILD_ADDON_MUSCLE_FITTING + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. +# Optionally an initial location of the external documentation +# can be added for each tagfile. The format of a tag file without +# this location is as follows: +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths or +# URLs. If a location is present for each tag, the installdox tool +# does not have to be run to correct the links. +# Note that each tag file must have a unique name +# (where the name does NOT include the path) +# If a tag file is not located in the directory in which doxygen +# is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = NO + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = FreeSans + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = NO + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = NO + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES + diff --git a/3rdparty/rbdl/LICENSE b/3rdparty/rbdl/LICENSE new file mode 100644 index 0000000..e1fcd42 --- /dev/null +++ b/3rdparty/rbdl/LICENSE @@ -0,0 +1,23 @@ +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2016 Martin Felis + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/README.md b/3rdparty/rbdl/README.md new file mode 100644 index 0000000..27f993c --- /dev/null +++ b/3rdparty/rbdl/README.md @@ -0,0 +1,209 @@ +[![Build Status](https://travis-ci.org/rbdl/rbdl.svg?branch=master)](https://travis-ci.org/rbdl/rbdl) + +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2020 Martin Felis + +Introduction +============ + +RBDL is a highly efficient C++ library that contains some essential rigid +body dynamics algorithms such as the Articulated Body Algorithm (ABA) for +forward dynamics, Recursive Newton-Euler Algorithm (RNEA) for inverse +dynamics and the Composite Rigid Body Algorithm (CRBA) for the efficient +computation of the joint space inertia matrix. It further contains code for +Jacobians, forward and inverse kinematics, handling of external +constraints such as contacts and collisions, and closed-loop models. + +The code is developed by Martin Felis +at the research group [Optimization in Robotics and Biomechanics +(ORB)](http://orb.iwr.uni-heidelberg.de) of the [Interdisciplinary Center +for Scientific Computing (IWR)](http://www.iwr.uni-heidelberg.de) at +[Heidelberg University](http://www.uni-heidelberg.de). The code tightly +follows the notation used in Roy Featherstone''s book "Rigid Body Dynamics +Algorithm". + +Recent Changes +============== + * 02 May 2018: New version 2.6.0: + * Added support for closed-loop models by replacing Contacts API by a new + Constraints API. Loop constraints can be stabilized using Baumgarte + stabilization. Special thanks to Davide Corradi for this contribution! + * New constraint type CustomConstraint: a versatile interface to define + more general types of constraints (e.g. time dependent), contributed by + Matthew J. Millard. + * New joint type JointTypeHelical that can be used for screwing motions + (translations and simultaneous rotations), contributed by Stuart Anderson. + * Added support to specify external forces on bodies on constrained forward + dynamics and NonlinearEffects() (contributed by Matthew J. Millard) + * Changed Quaternion multiplication behaviour for a more standard + convention: multiplying q1 (1,0,0,0) with q2 (0,1,0,0) results now in + (0,0,1,0) instead of the previous (0,0,-1,0). + * Removed Model::SetFloatingBaseBody(). Use JointTypeFloatingBase instead. + * LuaModel: extended specification to support ConstraintSets. + * 28 April 2016: New version 2.5.0: + * Added an experimental Cython based Python wrapper of RBDL. The API is + very close to the C++ API. For a brief glimpse of the API see the file + python/test_wrapper.py. + * Matthew Millard added CustomJoints which allow to create different joint + types completely by user code. They are implemented as proxy joints for + which their behaviour is specified using virtual functions. + * Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + the joint space inertia matrix with a vector in O(n) time. + * Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + the floating base joint. + * Loading of floating base URDF models must now be specified as a third + parameter to URDFReadFromFile() and URDFReadFromString() + * Added the URDF code from Bullet3 which gets used when ROS is not found. + Otherwise use the URDF libraries found via Catkin. + * Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + that compute both linear and angular quantities + * Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + JointTypeFloatingBase instead. + * Fixed building issues when building DLL with MSVC++. + * 20 April 2016: New version 2.4.1: + * This is a bugfix release that maintains binary compatibility and only fixes + erroneous behaviour. + * critical: fixed termination criterion for InverseKinematics. The termination + criterion would be evaluated too early and thus report convergence too + early. This was reported independently by Kevin Stein, Yun Fei, and Davide + Corradi. Thanks for the reports! + * critical: fixed CompositeRigidBodyAlgorithm when using spherical joints + (thanks to SĂ©bastien BarthĂ©lĂ©my for reporting!) + * 23 February 2015: New version 2.4.0: + * Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() + and ComputeContactImpulsesRangeSpaceSparse() + * Added null-space method ForwardDynamicsContactsNullSpace() + and ComputeContactImpulsesNullSpace() + * Renamed ForwardDynamicsContactsLagrangian() to + ForwardDynamicsContactsDirect() and + ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect() + * Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis() + * Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The + functionality has been integrated into CalcCenterOfMass(). + * CalcPointJacobian() does not clear the argument of the result anymore. + Caller has to ensure that the matrix was set to zero before using this + function. + * Added optional workspace parameters for ForwardDynamicsLagrangian() to + optionally reduce memory allocations + * Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ + which are equivalent to the emulated multidof joints but faster. + * Added optional parameter to CalcCenterOfMass to compute angular momentum. + * Added CalcBodySpatialJacobian() + * Added CalcContactJacobian() + * Added NonlinearEffects() + * Added solving of linear systems using standard Householder QR + * LuaModel: Added LuaModelReadFromLuaState() + * URDFReader: Fixed various issues and using faster joints for floating + base models + * Various performance improvements + +For a complete history see doc/api_changes.txt. + +Documentation +============= + +The documentation is contained in the code and can be extracted with the +tool [doxygen](http://www.doxygen.org). + +To create the documentation simply run + + doxygen Doxyfile + +which will generate the documentation in the subdirectory ./doc/html. The +main page will then be located in ./doc/html/index.html. + +An online version of the generated documentation can be found at +[https://rbdl.github.io](https://rbdl.github.io). + +Getting RBDL +============ + +The latest stable code can be obtained from the master branch at + + https://github.com/rbdl/rbdl + +Building and Installation +========================= + +The RBDL is built using CMake +([http://www.cmake.org](http://www.cmake.org)). To compile the library in +a separate directory in Release mode use: + + mkdir build + cd build/ + cmake -D CMAKE_BUILD_TYPE=Release ../ + make + +For optimal performance it is highly recommended to install the Eigen3 +linear algebra library from +[http://eigen.tuxfamily.org](http://eigen.tuxfamily.org). RBDL also +comes with a simple, albeit much slower math library (SimpleMath) that can +be used by enabling `RBDL_USE_SIMPLE_MATH`, i.e.: + + cmake -D RBDL_USE_SIMPLE_MATH=TRUE ../ + +Python Bindings +=============== + +RBDL can also build an experimental python wrapper that works with python 3 and python 2. To do this enable the +the `RBDL_BUILD_PYTHON_WRAPPER` cmake options. This will build the wrapper for python 3, if you want to use python 2 instead +you will also have to enable the `RBDL_USE_PYTHON_2` cmake option. The result of this is an extra python directory in the build +directory. From within which you can install it using setup.py. This is done automically when using `make install` + +Citation +======== + +An overview of the theoretical and implementation details has been +published in [https://doi.org/10.1007/s10514-016-9574-0](Felis, +M.L. Auton Robot (2017) 41: 495). To cite RBDL in your academic +research you can use the following BibTeX entry: + + @Article{Felis2016, + author="Felis, Martin L.", + title="RBDL: an efficient rigid-body dynamics library using recursive algorithms", + journal="Autonomous Robots", + year="2016", + pages="1--17", + issn="1573-7527", + doi="10.1007/s10514-016-9574-0", + url="http://dx.doi.org/10.1007/s10514-016-9574-0" + } + +Licensing +========= + +The library is published under the very permissive zlib free software +license which should allow you to use the software wherever you need. + +This is the full license text (zlib license): + + RBDL - Rigid Body Dynamics Library + Copyright (c) 2011-2020 Martin Felis + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not + be misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. + +Acknowledgements +================ + +Work on this library was originally funded by the [Heidelberg Graduate +School of Mathematical and Computational Methods for the Sciences +(HGS)](http://hgs.iwr.uni-heidelberg.de/hgs.mathcomp/), and the European +FP7 projects [ECHORD](http://echord.eu) (grant number 231143) and +[Koroibot](http://koroibot.eu) (grant number 611909). diff --git a/3rdparty/rbdl/addons/benchmark/CMakeLists.txt b/3rdparty/rbdl/addons/benchmark/CMakeLists.txt new file mode 100644 index 0000000..2f394de --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/CMakeLists.txt @@ -0,0 +1,65 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +# Perform the proper linking +SET (CMAKE_SKIP_BUILD_RPATH FALSE) +SET (CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +SET (CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") +SET (CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +# Options +SET ( BENCHMARK_SOURCES + model_generator.cc + Human36Model.cc + benchmark.cc + ) + +ADD_EXECUTABLE ( benchmark ${BENCHMARK_SOURCES} ) + +SET_TARGET_PROPERTIES ( benchmark PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF +) + +IF (RBDL_BUILD_STATIC) + SET (LIBRARIES rbdl-static) + + IF (RBDL_BUILD_ADDON_LUAMODEL) + SET (LIBRARIES ${LIBRARIES} rbdl_luamodel-static) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + IF (RBDL_BUILD_ADDON_URDFREADER) + SET (LIBRARIES ${LIBRARIES} rbdl_urdfreader-static) + ENDIF (RBDL_BUILD_ADDON_URDFREADER) + + TARGET_LINK_LIBRARIES ( benchmark + rbdl-static + ${LIBRARIES} + ) +ELSE (RBDL_BUILD_STATIC) + SET (LIBRARIES rbdl) + + IF (RBDL_BUILD_ADDON_LUAMODEL) + SET (LIBRARIES ${LIBRARIES} rbdl_luamodel) + ENDIF (RBDL_BUILD_ADDON_LUAMODEL) + + IF (RBDL_BUILD_ADDON_URDFREADER) + SET (LIBRARIES ${LIBRARIES} rbdl_urdfreader) + ENDIF (RBDL_BUILD_ADDON_URDFREADER) + + TARGET_LINK_LIBRARIES ( benchmark + rbdl + ${LIBRARIES} + ) + +ENDIF (RBDL_BUILD_STATIC) diff --git a/3rdparty/rbdl/addons/benchmark/Human36Model.cc b/3rdparty/rbdl/addons/benchmark/Human36Model.cc new file mode 100644 index 0000000..565daf4 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Human36Model.cc @@ -0,0 +1,158 @@ +#include "Human36Model.h" + +#include "rbdl/rbdl.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +enum SegmentName { + SegmentPelvis = 0, + SegmentThigh, + SegmentShank, + SegmentFoot, + SegmentMiddleTrunk, + SegmentUpperTrunk, + SegmentUpperArm, + SegmentLowerArm, + SegmentHand, + SegmentHead, + SegmentNameLast +}; + +double SegmentLengths[SegmentNameLast] = { + 0.1457, + 0.4222, + 0.4403, + 0.1037, + 0.2155, + 0.2421, + 0.2817, + 0.2689, + 0.0862, + 0.2429 +}; + +double SegmentMass[SegmentNameLast] = { + 0.8154, + 10.3368, + 3.1609, + 1.001, + 16.33, + 15.96, + 1.9783, + 1.1826, + 0.4453, + 5.0662 +}; + +double SegmentCOM[SegmentNameLast][3] = { + { 0., 0., 0.0891}, + { 0., 0., -0.1729}, + { 0., 0., -0.1963}, + { 0.1254, 0., -0.0516}, + { 0., 0., 0.1185}, + { 0., 0., 0.1195}, + { 0., 0., -0.1626}, + { 0., 0., -0.1230}, + { 0., 0., -0.0680}, + { 0., 0., 1.1214} +}; + +double SegmentRadiiOfGyration[SegmentNameLast][3] = { + { 0.0897, 0.0855, 0.0803}, + { 0.1389, 0.0629, 0.1389}, + { 0.1123, 0.0454, 0.1096}, + { 0.0267, 0.0129, 0.0254}, + { 0.0970, 0.1009, 0.0825}, + { 0.1273, 0.1172, 0.0807}, + { 0.0803, 0.0758, 0.0445}, + { 0.0742, 0.0713, 0.0325}, + { 0.0541, 0.0442, 0.0346}, + { 0.0736, 0.0634, 0.0765} +}; + +Body create_body (SegmentName segment) { + Matrix3d inertia_C (Matrix3d::Zero()); + inertia_C(0,0) = pow(SegmentRadiiOfGyration[segment][0] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(1,1) = pow(SegmentRadiiOfGyration[segment][1] * SegmentLengths[segment], 2) * SegmentMass[segment]; + inertia_C(2,2) = pow(SegmentRadiiOfGyration[segment][2] * SegmentLengths[segment], 2) * SegmentMass[segment]; + + return RigidBodyDynamics::Body ( + SegmentMass[segment], + RigidBodyDynamics::Math::Vector3d ( + SegmentCOM[segment][0], + SegmentCOM[segment][1], + SegmentCOM[segment][2] + ), + inertia_C); +} + +void generate_human36model (RigidBodyDynamics::Model *model) { + Body pelvis_body = create_body (SegmentPelvis); + Body thigh_body = create_body (SegmentThigh); + Body shank_body = create_body (SegmentShank); + Body foot_body = create_body (SegmentFoot); + Body middle_trunk_body = create_body (SegmentMiddleTrunk); + Body upper_trunk_body = create_body (SegmentUpperTrunk); + Body upperarm_body = create_body (SegmentUpperArm); + Body lowerarm_body = create_body (SegmentLowerArm); + Body hand_body = create_body (SegmentHand); + Body head_body = create_body (SegmentHead); + + Joint free_flyer ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Joint rot_yxz ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_yz ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + Joint rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + Joint fixed (JointTypeFixed); + + model->gravity = Vector3d (0., 0., -9.81); + + unsigned int pelvis_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + model->AddBody (pelvis_id, Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz, thigh_body, "thigh_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz, thigh_body, "thigh_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz, middle_trunk_body, "middletrunk"); + unsigned int uppertrunk_id = model->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "upperarm_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "upperarm_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz, upperarm_body, "head"); +} diff --git a/3rdparty/rbdl/addons/benchmark/Human36Model.h b/3rdparty/rbdl/addons/benchmark/Human36Model.h new file mode 100644 index 0000000..0854b21 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Human36Model.h @@ -0,0 +1,11 @@ +#ifndef _HUMAN36MODEL_H +#define _HUMAN36MODEL_H + +namespace RigidBodyDynamics { +struct Model; +} + +void generate_human36model (RigidBodyDynamics::Model *model); + +/* _HUMAN36MODEL_H */ +#endif diff --git a/3rdparty/rbdl/addons/benchmark/SampleData.h b/3rdparty/rbdl/addons/benchmark/SampleData.h new file mode 100644 index 0000000..fcfeffc --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/SampleData.h @@ -0,0 +1,95 @@ +#ifndef _SAMPLE_DATA_H +#define _SAMPLE_DATA_H + +struct SampleData { + SampleData() : + count (0), q(NULL), qdot(NULL), qddot(NULL), tau(NULL) + {} + ~SampleData() { + deleteData(); + } + SampleData(const SampleData &data) { + count = data.count; + + q = new RigidBodyDynamics::Math::VectorNd[count]; + qdot = new RigidBodyDynamics::Math::VectorNd[count]; + qddot = new RigidBodyDynamics::Math::VectorNd[count]; + tau = new RigidBodyDynamics::Math::VectorNd[count]; + durations = data.durations; + + for (unsigned int si = 0; si < count; si++) { + q[si] = data.q[si]; + qdot[si] = data.qdot[si]; + qddot[si] = data.qddot[si]; + tau[si] = data.tau[si]; + } + } + SampleData& operator= (const SampleData &data) { + if (this != &data) { + deleteData(); + *this = SampleData (data); + } + return *this; + } + + unsigned int count; + RigidBodyDynamics::Math::VectorNd *q; + RigidBodyDynamics::Math::VectorNd *qdot; + RigidBodyDynamics::Math::VectorNd *qddot; + RigidBodyDynamics::Math::VectorNd *tau; + RigidBodyDynamics::Math::VectorNd durations; + + void deleteData() { + count = 0; + + if (q) { + delete[] q; + } + q = NULL; + + if (qdot) { + delete[] qdot; + } + qdot = NULL; + + if (qddot) { + delete[] qddot; + } + qddot = NULL; + + if (tau) { + delete[] tau; + } + tau = NULL; + + durations.resize(0); + } + + void fillRandom (int dof_count, int sample_count) { + deleteData(); + count = sample_count; + + q = new RigidBodyDynamics::Math::VectorNd[count]; + qdot = new RigidBodyDynamics::Math::VectorNd[count]; + qddot = new RigidBodyDynamics::Math::VectorNd[count]; + tau = new RigidBodyDynamics::Math::VectorNd[count]; + + for (unsigned int si = 0; si < count; si++) { + q[si].resize (dof_count); + qdot[si].resize (dof_count); + qddot[si].resize (dof_count); + tau[si].resize (dof_count); + + for (int i = 0; i < dof_count; i++) { + q[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + qdot[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + qddot[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + tau[si][i] = (static_cast(rand()) / static_cast(RAND_MAX)) * 2. -1.; + } + } + + durations = RigidBodyDynamics::Math::VectorNd::Zero(count); + } +}; + +#endif diff --git a/3rdparty/rbdl/addons/benchmark/Timer.h b/3rdparty/rbdl/addons/benchmark/Timer.h new file mode 100644 index 0000000..2656f64 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/Timer.h @@ -0,0 +1,29 @@ +#ifndef _TIMER_H +#define _TIMER_H + +#include + +struct TimerInfo { + /// time stamp when timer_start() gets called + clock_t clock_start_value; + + /// time stamp when the timer was stopped + clock_t clock_end_value; + + /// duration between clock_start_value and clock_end_value in seconds + double duration_sec; +}; + +inline void timer_start (TimerInfo *timer) { + timer->clock_start_value = clock(); +} + +inline double timer_stop (TimerInfo *timer) { + timer->clock_end_value = clock(); + + timer->duration_sec = static_cast(timer->clock_end_value - timer->clock_start_value) * 1 / CLOCKS_PER_SEC; + + return timer->duration_sec; +} + +#endif diff --git a/3rdparty/rbdl/addons/benchmark/benchmark.cc b/3rdparty/rbdl/addons/benchmark/benchmark.cc new file mode 100644 index 0000000..a423538 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/benchmark.cc @@ -0,0 +1,1011 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rbdl/rbdl.h" +#include "model_generator.h" +#include "Human36Model.h" +#include "SampleData.h" +#include "Timer.h" + +#ifdef RBDL_BUILD_ADDON_LUAMODEL +#include "../addons/luamodel/luamodel.h" +bool have_luamodel = true; +#else +bool have_luamodel = false; +#endif + +#ifdef RBDL_BUILD_ADDON_URDFREADER +#include "../addons/urdfreader/urdfreader.h" +bool have_urdfreader = true; +bool urdf_floating_base = false; +#else +bool have_urdfreader = false; +#endif + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int benchmark_sample_count = 1000; +int benchmark_model_max_depth = 5; + +bool benchmark_run_fd_aba = true; +bool benchmark_run_fd_lagrangian = true; +bool benchmark_run_id_rnea = true; +bool benchmark_run_crba = true; +bool benchmark_run_nle = true; +bool benchmark_run_calc_minv_times_tau = true; +bool benchmark_run_contacts = false; +bool benchmark_run_ik = false; + +bool json_output = false; + +string model_name; + +enum ContactsMethod { + ConstraintsMethodDirect = 0, + ConstraintsMethodRangeSpaceSparse, + ConstraintsMethodNullSpace, + ConstraintsMethodKokkevis +}; + +struct BenchmarkRun { + string model_name; + int model_dof; + string benchmark; + int sample_count; + + double duration; + double avg; + double min; + double max; +}; + +vector benchmark_runs; + +void report_section (const char* section_name) { + if (!json_output) { + cout << "= " << section_name << " =" << endl; + } +} + +void register_run(const Model &model, const SampleData &data, const char *run_name) { + BenchmarkRun run; + run.benchmark = run_name; + run.model_name = model_name; + run.model_dof = model.dof_count; + run.sample_count = data.count; + + run.duration = data.durations.sum(); + run.avg = data.durations.mean(); + run.min = data.durations.minCoeff(); + run.max = data.durations.maxCoeff(); + + benchmark_runs.push_back(run); +} + +void report_run(const Model &model, const SampleData &data, + const char *run_name) { + register_run(model, data, run_name); + + if (!json_output) { + cout << "#DOF: " << setw(3) << model.dof_count + << " #samples: " << data.count + << " duration = " << setw(10) << data.durations.sum() << "(s)" + << " (~" << setw(10) << data.durations.mean() << "(s) per call)" << endl; + } +} + +void report_constraints_run(const Model &model, const SampleData &data, + const char *run_name) { + register_run(model, data, run_name); + + if (!json_output) { + cout << model_name << ": " + << " duration = " << setw(10) << data.durations.sum() << "(s)" + << " (~" << setw(10) << data.durations.mean() << "(s) per call)" << endl; + } +} + +/** Parses /proc/cpuinfo for the CPU model name. */ +string get_cpu_model_name () { + ifstream proc_cpu ("/proc/cpuinfo", ios_base::in); + if (!proc_cpu) { + cerr << "Cannot determine cpu model: could not open /proc/cpuinfo for reading." << endl; + abort(); + } + ostringstream content; + content << proc_cpu.rdbuf(); + proc_cpu.close(); + + string content_str = content.str(); + std::size_t model_name_pos = content_str.find("model name"); + if (model_name_pos != string::npos) { + std::size_t start = content_str.find(':', model_name_pos + strlen("model name")) + 2; + std::size_t end = content_str.find('\n', model_name_pos + strlen("model name")); + + return content_str.substr(start, end - start); + } + return "unknown"; +} + +string get_utc_time_string () { + time_t current_time; + struct tm* timeinfo; + time(¤t_time); + timeinfo = gmtime(¤t_time); + char time_buf[80]; + std::size_t UNUSED(time_len) = strftime(&time_buf[0], 80, "%a %b %d %T %Y", timeinfo); + assert(time_len <= 80 & "violating buffer size for utc time conversion"); + return time_buf; +} +double run_forward_dynamics_ABA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamics (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i], + sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "ForwardDynamics"); + + return sample_data.durations.sum(); +} + +double run_forward_dynamics_lagrangian_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + MatrixNd H (MatrixNd::Zero(model->dof_count, model->dof_count)); + VectorNd C (VectorNd::Zero(model->dof_count)); + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamicsLagrangian (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i], + sample_data.qddot[i], + Math::LinearSolverPartialPivLU, + NULL, + &H, + &C + ); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "ForwardDynamicsLagrangian_PivLU"); + + return sample_data.durations.sum(); +} + +double run_inverse_dynamics_RNEA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + timer_start (&tinfo); + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + InverseDynamics (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.qddot[i], + sample_data.tau[i] + ); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "InverseDynamics"); + + return sample_data.durations.sum(); +} + +double run_CRBA_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + Math::MatrixNd H = Math::MatrixNd::Zero(model->dof_count, model->dof_count); + Math::MatrixNd identity = Math::MatrixNd::Identity(model->dof_count, model->dof_count); + Math::MatrixNd Hinv = Math::MatrixNd::Zero(model->dof_count, model->dof_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + CompositeRigidBodyAlgorithm (*model, sample_data.q[i], H, true); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "CompositeRigidBodyAlgorithm"); + + return sample_data.durations.sum(); +} + +double run_nle_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + NonlinearEffects (*model, + sample_data.q[i], + sample_data.qdot[i], + sample_data.tau[i] + ); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "NonlinearEffects"); + + return sample_data.durations.sum(); +} + +double run_calc_minv_times_tau_benchmark (Model *model, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + CalcMInvTimesTau (*model, sample_data.q[0], sample_data.tau[0], sample_data.qddot[0]); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + CalcMInvTimesTau (*model, sample_data.q[i], sample_data.tau[i], sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_run(*model, sample_data, "NonlinearEffects"); + + return sample_data.durations.sum(); +} + +double run_contacts_lagrangian_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamicsConstraintsDirect (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsDirect"); + + return sample_data.durations.sum(); +} + +double run_contacts_lagrangian_sparse_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamicsConstraintsRangeSpaceSparse (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsRangeSpaceSparse"); + + return sample_data.durations.sum(); +} + +double run_contacts_null_space (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamicsConstraintsNullSpace (*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_constraints_run(*model, sample_data, "ForwardDynamicsConstraintsNullSpace"); + + return sample_data.durations.sum(); +} + +double run_contacts_kokkevis_benchmark (Model *model, ConstraintSet *constraint_set, int sample_count) { + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + TimerInfo tinfo; + + for (int i = 0; i < sample_count; i++) { + timer_start (&tinfo); + ForwardDynamicsContactsKokkevis(*model, sample_data.q[i], sample_data.qdot[i], sample_data.tau[i], *constraint_set, sample_data.qddot[i]); + sample_data.durations[i] = timer_stop (&tinfo); + } + + report_constraints_run(*model, sample_data, "ForwardDynamicsContactsKokkevis"); + + return sample_data.durations.sum(); +} + +void contacts_benchmark (int sample_count, ContactsMethod contacts_method) { + // initialize the human model + Model *model = new Model(); + generate_human36model(model); + + // initialize the constraint sets + unsigned int foot_r = model->GetBodyId ("foot_r"); + unsigned int foot_l = model->GetBodyId ("foot_l"); + unsigned int hand_r = model->GetBodyId ("hand_r"); + unsigned int hand_l = model->GetBodyId ("hand_l"); + + ConstraintSet one_body_one_constraint; + ConstraintSet two_bodies_one_constraint; + ConstraintSet four_bodies_one_constraint; + + ConstraintSet one_body_four_constraints; + ConstraintSet two_bodies_four_constraints; + ConstraintSet four_bodies_four_constraints; + + LinearSolver linear_solver = LinearSolverPartialPivLU; + + one_body_one_constraint.linear_solver = linear_solver; + two_bodies_one_constraint.linear_solver = linear_solver; + four_bodies_one_constraint.linear_solver = linear_solver; + one_body_four_constraints.linear_solver = linear_solver; + two_bodies_four_constraints.linear_solver = linear_solver; + four_bodies_four_constraints.linear_solver = linear_solver; + + // one_body_one + one_body_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_one_constraint.Bind (*model); + + // two_bodies_one + two_bodies_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_one_constraint.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_one_constraint.Bind (*model); + + // four_bodies_one + four_bodies_one_constraint.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_one_constraint.Bind (*model); + + // one_body_four + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + one_body_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + one_body_four_constraints.Bind (*model); + + // two_bodies_four + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + two_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + two_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + two_bodies_four_constraints.Bind (*model); + + // four_bodies_four + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (foot_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (foot_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (hand_r, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + four_bodies_four_constraints.AddContactConstraint (hand_l, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + four_bodies_four_constraints.Bind (*model); + + model_name = "Human36"; + if (!json_output) { + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + cout << "= No constraints (Articulated Body Algorithm):" << endl; + run_forward_dynamics_ABA_benchmark(model, sample_count); + } + + // one body one + model_name = "Human36_1Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &one_body_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &one_body_one_constraint, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &one_body_one_constraint, sample_count); + } + + + // two_bodies_one + model_name = "Human36_2Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &two_bodies_one_constraint, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &two_bodies_one_constraint, sample_count); + } + + + + // four_bodies_one + model_name = "Human36_4Bodies1Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_one_constraint, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &four_bodies_one_constraint, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &four_bodies_one_constraint, sample_count); + } + + + // one_body_four + model_name = "Human36_1Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &one_body_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &one_body_four_constraints, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &one_body_four_constraints, sample_count); + } + + + // two_bodies_four + model_name = "Human36_2Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &two_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &two_bodies_four_constraints, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &two_bodies_four_constraints, sample_count); + } + + + + // four_bodies_four + model_name = "Human36_4Bodies4Constraints"; + if (contacts_method == ConstraintsMethodDirect) { + run_contacts_lagrangian_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodRangeSpaceSparse) { + run_contacts_lagrangian_sparse_benchmark (model, &four_bodies_four_constraints, sample_count); + } else if (contacts_method == ConstraintsMethodNullSpace) { + run_contacts_null_space (model, &four_bodies_four_constraints, sample_count); + } else { + run_contacts_kokkevis_benchmark (model, &four_bodies_four_constraints, sample_count); + } + + + delete model; + +} + +double run_single_inverse_kinematics_benchmark(Model *model, std::vector &CS, int sample_count){ + TimerInfo tinfo; + timer_start (&tinfo); + VectorNd qinit = VectorNd::Zero(model->dof_count); + VectorNd qres = VectorNd::Zero(model->dof_count); + VectorNd failures = VectorNd::Zero(model->dof_count); + + for (int i = 0; i < sample_count; i++) { + if (!InverseKinematics(*model, qinit, CS[i], qres)){ + failures[i] = 1; + } + } + double duration = timer_stop (&tinfo); + std::cout << "Success Rate: " << (1-failures.mean())*100 << "% for: "; + return duration; + +} + +double run_all_inverse_kinematics_benchmark (unsigned int sample_count){ + + //initialize the human model + Model *model = new Model(); + generate_human36model(model); + + unsigned int foot_r = model->GetBodyId ("foot_r"); + unsigned int foot_l = model->GetBodyId ("foot_l"); + unsigned int hand_r = model->GetBodyId ("hand_r"); + unsigned int hand_l = model->GetBodyId ("hand_l"); + unsigned int head = model->GetBodyId ("head"); + + Vector3d foot_r_point (1., 0., 0.); + Vector3d foot_l_point (-1., 0., 0.); + Vector3d hand_r_point (0., 1., 0.); + Vector3d hand_l_point (1., 0., 1.); + Vector3d head_point (0.,0.,-1.); + + SampleData sample_data; + sample_data.fillRandom(model->dof_count, sample_count); + + + //create constraint sets + std::vector cs_one_point; + std::vector cs_two_point_one_orientation; + std::vector cs_two_full_one_point; + std::vector cs_two_full_two_point_one_orientation; + std::vector cs_five_full; + + for (unsigned int i = 0; i < sample_count; i++){ + Vector3d foot_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_r, foot_r_point); + Vector3d foot_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], foot_l, foot_l_point); + Vector3d hand_r_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_r, hand_r_point); + Vector3d hand_l_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], hand_l, hand_l_point); + Vector3d head_position = CalcBodyToBaseCoordinates (*model, sample_data.q[i], head , head_point); + + Matrix3d foot_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_r, false); + Matrix3d foot_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], foot_l, false); + Matrix3d hand_r_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_r, false); + Matrix3d hand_l_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], hand_l, false); + Matrix3d head_orientation = CalcBodyWorldOrientation (*model, sample_data.q[i], head , false); + + //single point + InverseKinematicsConstraintSet one_point; + one_point.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + one_point.step_tol = 1e-12; + cs_one_point.push_back(one_point); + + //two point and one orientation + InverseKinematicsConstraintSet two_point_one_orientation; + two_point_one_orientation.AddPointConstraint(foot_l,foot_l_point, foot_l_position); + two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_point_one_orientation.step_tol = 1e-12; + cs_two_point_one_orientation.push_back(two_point_one_orientation); + + //two full and one point + InverseKinematicsConstraintSet two_full_one_point; + two_full_one_point.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_one_point.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_one_point.AddPointConstraint(head, head_point, head_position); + two_full_one_point.step_tol = 1e-12; + cs_two_full_one_point.push_back(two_full_one_point); + + //two full, two points and one orienation + InverseKinematicsConstraintSet two_full_two_point_one_orientation; + two_full_two_point_one_orientation.AddPointConstraint(foot_r, foot_r_point, foot_r_position); + two_full_two_point_one_orientation.AddPointConstraint(foot_l, foot_l_point, foot_l_position); + two_full_two_point_one_orientation.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + two_full_two_point_one_orientation.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + two_full_two_point_one_orientation.AddOrientationConstraint(head, head_orientation); + two_full_two_point_one_orientation.step_tol = 1e-12; + cs_two_full_two_point_one_orientation.push_back(two_full_two_point_one_orientation); + + //five points 5 orientations + InverseKinematicsConstraintSet five_full; + five_full.AddFullConstraint(foot_r, foot_r_point, foot_r_position, foot_r_orientation); + five_full.AddFullConstraint(foot_l, foot_l_point, foot_l_position, foot_l_orientation); + five_full.AddFullConstraint(hand_r, hand_r_point, hand_r_position, hand_r_orientation); + five_full.AddFullConstraint(hand_l, hand_l_point, hand_l_position, hand_l_orientation); + five_full.AddFullConstraint(head, head_point, head_position, head_orientation); + five_full.step_tol = 1e-12; + cs_five_full.push_back(five_full); + } + + cout << "= #DOF: " << setw(3) << model->dof_count << endl; + cout << "= #samples: " << sample_count << endl; + double duration; + + duration = run_single_inverse_kinematics_benchmark(model, cs_one_point, sample_count); + cout << "Constraints: 1 Body: 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_point_one_orientation, sample_count); + cout << "Constraints: 3 Bodies: 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_one_point, sample_count); + cout << "Constraints: 3 Bodies: 2 Full 1 Point : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_two_full_two_point_one_orientation, sample_count); + cout << "Constraints: 5 Bodies: 2 Full 2 Points 1 Orienation : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + + duration = run_single_inverse_kinematics_benchmark(model, cs_five_full, sample_count); + cout << "Constraints: 5 Bodies: 5 Full : " + << " duration = " << setw(10) << duration << "(s)" + << " (~" << setw(10) << duration / sample_count << "(s) per call)" << endl; + return duration; +} + +void print_usage () { +#if defined (RBDL_BUILD_ADDON_LUAMODEL) || defined (RBDL_BUILD_ADDON_URDFREADER) + cout << "Usage: benchmark [--count|-c ] [--depth|-d ] " << endl; +#else + cout << "Usage: benchmark [--count|-c ] [--depth|-d ]" << endl; +#endif + cout << "Simple benchmark tool for the Rigid Body Dynamics Library." << endl; + cout << " --count | -c : sets the number of sample states that should" << endl; + cout << " be calculated (default: 1000)" << endl; + cout << " --depth | -d : sets maximum depth for the branched test model" << endl; + cout << " which is created increased from 1 to (default: 5)." << endl; +#if defined RBDL_BUILD_ADDON_URDFREADER + cout << " --floating-base | -f : the specified URDF model is a floating base model." << endl; +#endif + cout << " --json : prints output in json format." << endl; + cout << " --no-fd : disables benchmarking of forward dynamics." << endl; + cout << " --no-fd-aba : disables benchmark for forwards dynamics using" << endl; + cout << " the Articulated Body Algorithm" << endl; + cout << " --no-fd-lagrangian : disables benchmark for forward dynamics via" << endl; + cout << " solving the lagrangian equation." << endl; + cout << " --no-id-rnea : disables benchmark for inverse dynamics using" << endl; + cout << " the recursive newton euler algorithm." << endl; + cout << " --no-crba : disables benchmark for joint space inertia" << endl; + cout << " matrix computation using the composite rigid" << endl; + cout << " body algorithm." << endl; + cout << " --no-nle : disables benchmark for the nonlinear effects." << endl; + cout << " --no-calc-minv : disables benchmark M^-1 * tau benchmark." << endl; + cout << " --only-contacts | -C : only runs contact model benchmarks." << endl; + cout << " --only-ik : only runs inverse kinematics benchmarks." << endl; + cout << " --help | -h : prints this help." << endl; +} + +void disable_all_benchmarks () { + benchmark_run_fd_aba = false; + benchmark_run_fd_lagrangian = false; + benchmark_run_id_rnea = false; + benchmark_run_crba = false; + benchmark_run_nle = false; + benchmark_run_calc_minv_times_tau = false; + benchmark_run_contacts = false; +} + +void parse_args (int argc, char* argv[]) { + int argi = 1; + + while (argi < argc) { + string arg = argv[argi]; + + if (arg == "--help" || arg == "-h") { + print_usage(); + exit (1); + } else if (arg == "--count" || arg == "-c" ) { + if (argi == argc - 1) { + print_usage(); + + cerr << "Error: missing number of samples!" << endl; + exit (1); + } + + argi++; + stringstream count_stream (argv[argi]); + + count_stream >> benchmark_sample_count; + } else if (arg == "--depth" || arg == "-d" ) { + if (argi == argc - 1) { + print_usage(); + + cerr << "Error: missing number for model depth!" << endl; + exit (1); + } + + argi++; + stringstream depth_stream (argv[argi]); + + depth_stream >> benchmark_model_max_depth; +#ifdef RBDL_BUILD_ADDON_URDFREADER + } else if (arg == "--floating-base" || arg == "-f") { + urdf_floating_base = true; +#endif + } else if (arg == "--json") { + json_output = true; + } else if (arg == "--no-fd" ) { + benchmark_run_fd_aba = false; + benchmark_run_fd_lagrangian = false; + } else if (arg == "--no-fd-aba" ) { + benchmark_run_fd_aba = false; + } else if (arg == "--no-fd-lagrangian" ) { + benchmark_run_fd_lagrangian = false; + } else if (arg == "--no-id-rnea" ) { + benchmark_run_id_rnea = false; + } else if (arg == "--no-crba" ) { + benchmark_run_crba = false; + } else if (arg == "--no-nle" ) { + benchmark_run_nle = false; + } else if (arg == "--no-calc-minv" ) { + benchmark_run_calc_minv_times_tau = false; + } else if (arg == "--only-contacts" || arg == "-C") { + disable_all_benchmarks(); + benchmark_run_contacts = true; + } else if (arg == "--only-ik") { + disable_all_benchmarks(); + benchmark_run_ik = true; +#if defined (RBDL_BUILD_ADDON_LUAMODEL) || defined (RBDL_BUILD_ADDON_URDFREADER) + } else if (model_name == "") { + model_name = arg; +#endif + } else { + print_usage(); + cerr << "Invalid argument '" << arg << "'." << endl; + exit(1); + } + argi++; + } +} + +int main (int argc, char *argv[]) { + parse_args (argc, argv); + + Model *model = NULL; + + model = new Model(); + + if (model_name != "") { + if (model_name.substr (model_name.size() - 4, 4) == ".lua") { +#ifdef RBDL_BUILD_ADDON_LUAMODEL + RigidBodyDynamics::Addons::LuaModelReadFromFile (model_name.c_str(), model); +#else + cerr << "Could not load Lua model: LuaModel addon not enabled!" << endl; + abort(); +#endif + } + if (model_name.substr (model_name.size() - 5, 5) == ".urdf") { +#ifdef RBDL_BUILD_ADDON_URDFREADER + RigidBodyDynamics::Addons::URDFReadFromFile(model_name.c_str(), model, urdf_floating_base); +#else + cerr << "Could not load URDF model: urdfreader addon not enabled!" << endl; + abort(); +#endif + } + + if (benchmark_run_fd_aba) { + report_section("Forward Dynamics: ABA"); + run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_fd_lagrangian) { + report_section("Forward Dynamics: Lagrangian (Piv. LU decomposition)"); + run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_id_rnea) { + report_section("Inverse Dynamics: RNEA"); + run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_crba) { + report_section("Joint Space Inertia Matrix: CRBA"); + run_CRBA_benchmark (model, benchmark_sample_count); + } + + if (benchmark_run_nle) { + report_section("Nonlinear Effects"); + run_nle_benchmark (model, benchmark_sample_count); + } + + delete model; + + return 0; + } + + if (!json_output) { + rbdl_print_version(); + cout << endl; + } + + if (benchmark_run_fd_aba) { + report_section("Forward Dynamics: ABA"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + ostringstream model_name_stream; + model_name_stream << "planar_model_depth_" << depth; + model_name = model_name_stream.str(); + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_forward_dynamics_ABA_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_fd_lagrangian) { + report_section("Forward Dynamics: Lagrangian (Piv. LU decomposition)"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_forward_dynamics_lagrangian_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_id_rnea) { + report_section("Inverse Dynamics: RNEA"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_inverse_dynamics_RNEA_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_crba) { + report_section("Joint Space Inertia Matrix: CRBA"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_CRBA_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_nle) { + report_section("Nonlinear Effects"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_nle_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_calc_minv_times_tau) { + report_section("CalcMInvTimesTau"); + for (int depth = 1; depth <= benchmark_model_max_depth; depth++) { + model = new Model(); + model->gravity = Vector3d (0., -9.81, 0.); + + generate_planar_tree (model, depth); + + run_calc_minv_times_tau_benchmark (model, benchmark_sample_count); + + delete model; + } + } + + if (benchmark_run_contacts) { + report_section("Contacts: ForwardDynamicsConstraintsDirect"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodDirect); + + report_section("Contacts: ForwardDynamicsConstraintsRangeSpaceSparse"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodRangeSpaceSparse); + + report_section("Contacts: ForwardDynamicsConstraintsNullSpace"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodNullSpace); + + report_section("Contacts: ForwardDynamicsContactsKokkevis"); + contacts_benchmark (benchmark_sample_count, ConstraintsMethodKokkevis); + } + + if (benchmark_run_ik) { + report_section("Inverse Kinematics"); + run_all_inverse_kinematics_benchmark(benchmark_sample_count); + } + + if (json_output) { + cout.precision(15); + cout << "{" << endl; + + cout << " \"rbdl_info\" : {" << endl; + int compile_version = rbdl_get_api_version(); + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." << compile_patch; + + cout << " \"version_str\" : \"" << compile_version_string.str() << "\"," << endl; + cout << " \"major\" : " << compile_major << "," << endl; + cout << " \"minor\" : " << compile_minor << "," << endl; + cout << " \"patch\" : " << compile_patch << "," << endl; +#if defined RBDL_USE_SIMPLE_MATH + cout << " \"simple_math\" : " << "true," << endl; +#else + cout << " \"simple_math\" : " << "false," << endl; +#endif + cout << " \"build_type\" : \"" << RBDL_BUILD_TYPE << "\"," << endl; + cout << " \"commit\" : \"" << RBDL_BUILD_COMMIT << "\"," << endl; + cout << " \"branch\" : \"" << RBDL_BUILD_BRANCH << "\"," << endl; + cout << " \"compiler_id\" : \"" << RBDL_BUILD_COMPILER_ID << "\"," << endl; + cout << " \"compiler_version\" : \"" << RBDL_BUILD_COMPILER_VERSION << "\"" << endl; + + cout << " }," << endl; + + cout << " \"host_info\" : {" << endl; + cout << " \"cpu_model_name\" : \"" << get_cpu_model_name() << "\"," << endl; + cout << " \"time_utc\" : " << "\"" << get_utc_time_string() << "\"" << endl; + cout << " }," << endl; + + cout << " \"runs\" : "; + cout << "[" << endl; + + for (unsigned int i=0; i < benchmark_runs.size(); i++) { + const BenchmarkRun& run = benchmark_runs[i]; + + const char* indent = " "; + + cout << " " << "{" << endl; + cout << indent << "\"model\" : \"" << run.model_name << "\"," << endl; + cout << indent << "\"dof\" : " << run.model_dof << "," << endl; + cout << indent << "\"benchmark\" : \"" << run.benchmark << "\"," << endl; + cout << indent << "\"duration\" : " << run.duration << "," << endl; + cout << indent << "\"sample_count\" : " << run.sample_count << "," << endl; + cout << indent << "\"avg\" : " << run.avg << "," << endl; + cout << indent << "\"min\" : " << run.min << "," << endl; + cout << indent << "\"max\" : " << run.max << endl; + cout << " " << "}"; + + if (i != benchmark_runs.size() - 1) { + cout << ","; + } + cout << endl; + } + + cout << " ]" << endl; + + cout << "}" << endl; + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/benchmark/model_generator.cc b/3rdparty/rbdl/addons/benchmark/model_generator.cc new file mode 100644 index 0000000..9b266c2 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/model_generator.cc @@ -0,0 +1,54 @@ +#include "model_generator.h" + +#include "rbdl/rbdl.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +void generate_planar_tree_recursive (Model *model, + unsigned int parent_body_id, + int depth, + double length) { + if (depth == 0) + return; + + // create left child + Joint joint_rot_z (JointTypeRevoluteZ); + Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length)); + + Vector3d displacement (-0.5 * length, -0.25 * length, 0.); + unsigned int child_left = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body); + + generate_planar_tree_recursive (model, + child_left, + depth - 1, + length * 0.4); + + displacement.set (0.5 * length, -0.25 * length, 0.); + unsigned int child_right = model->AddBody (parent_body_id, Xtrans (displacement), joint_rot_z, body); + + generate_planar_tree_recursive (model, + child_right, + depth - 1, + length * 0.4); +} + +void generate_planar_tree (Model *model, int depth) { + // we first add a single body that is hanging straight down from + // (0, 0, 0). After that we generate the tree recursively such that each + // call adds two children. + // + double length = 1.; + + Joint joint_rot_z (JointTypeRevolute, Vector3d (0., 0., 1.)); + Body body (length, Vector3d (0., -0.25 * length, 0.), Vector3d (length, length, length)); + + unsigned int base_child = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, body); + + generate_planar_tree_recursive ( + model, + base_child, + depth, + length * 0.4); +} + diff --git a/3rdparty/rbdl/addons/benchmark/model_generator.h b/3rdparty/rbdl/addons/benchmark/model_generator.h new file mode 100644 index 0000000..f4bf502 --- /dev/null +++ b/3rdparty/rbdl/addons/benchmark/model_generator.h @@ -0,0 +1,11 @@ +#ifndef _MODEL_GENERATOR_H +#define _MODEL_GENERATOR_H + +namespace RigidBodyDynamics { +struct Model; +} + +void generate_planar_tree (RigidBodyDynamics::Model *model, int depth); + +/* _MODEL_GENERATOR_H */ +#endif diff --git a/3rdparty/rbdl/addons/geometry/CMakeLists.txt b/3rdparty/rbdl/addons/geometry/CMakeLists.txt new file mode 100644 index 0000000..dba3e0b --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/CMakeLists.txt @@ -0,0 +1,78 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +SET ( RBDL_ADDON_GEOMETRY_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_GEOMETRY_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_GEOMETRY_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_GEOMETRY_VERSION + ${RBDL_ADDON_GEOMETRY_VERSION_MAJOR}.${RBDL_ADDON_GEOMETRY_VERSION_MINOR}.${RBDL_ADDON_GEOMETRY_VERSION_PATCH} +) + +PROJECT (RBDL_ADDON_GEOMETRY VERSION ${RBDL_ADDON_GEOMETRY_VERSION}) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + ) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl +) + +SET(GEOMETRY_SOURCES + SegmentedQuinticBezierToolkit.cc + SmoothSegmentedFunction.cc + SegmentedQuinticBezierToolkit.h + SmoothSegmentedFunction.h + geometry.h + Function.h +) + +SET(GEOMETRY_HEADERS + geometry.h + Function.h + SegmentedQuinticBezierToolkit.h + SmoothSegmentedFunction.h +) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_geometry-static STATIC ${GEOMETRY_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_geometry-static PROPERTIES PREFIX "lib") + SET_TARGET_PROPERTIES ( rbdl_geometry-static PROPERTIES OUTPUT_NAME "rbdl_geometry") + + TARGET_LINK_LIBRARIES ( + rbdl_geometry-static + rbdl-static + ) + + INSTALL (TARGETS rbdl_geometry-static + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_geometry SHARED ${GEOMETRY_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_geometry PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES ( + rbdl_geometry + rbdl + ) + + INSTALL (TARGETS rbdl_geometry + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + ) + +INSTALL ( FILES ${GEOMETRY_HEADERS} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/geometry + ) diff --git a/3rdparty/rbdl/addons/geometry/Function.h b/3rdparty/rbdl/addons/geometry/Function.h new file mode 100644 index 0000000..0daaf02 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/Function.h @@ -0,0 +1,525 @@ +#ifndef SimTK_SimTKCOMMON_FUNCTION_H_ +#define SimTK_SimTKCOMMON_FUNCTION_H_ + +/* -------------------------------------------------------------------------- * + * Simbody(tm): SimTKcommon * + * -------------------------------------------------------------------------- * + * This is part of the SimTK biosimulation toolkit originating from * + * Simbios, the NIH National Center for Physics-Based Simulation of * + * Biological Structures at Stanford, funded under the NIH Roadmap for * + * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * + * * + * Portions copyright (c) 2008-12 Stanford University and the Authors. * + * Authors: Peter Eastman * + * Contributors: Michael Sherman * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +// Note: this file was moved from Simmath to SimTKcommon 20100601; see the +// Simmath repository for earlier history. + +//#include "SimTKcommon/basics.h" +//#include "SimTKcommon/Simmatrix.h" +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +#include +#include +#include +#include +#include +/** + This abstract class represents a mathematical function that calculates a + value of arbitrary type based on M real arguments. The output type is set + as a template argument, while the number of input components may be + determined at runtime. The name "Function" (with no trailing _) may be + used as a synonym for Function_. + + Subclasses define particular mathematical functions. Predefined subclasses + are provided for several common function types: Function_::Constant, + Function_::Linear, Function_::Polynomial, and Function_::Step. + You can define your own subclasses for other function types. The + Spline_ class also provides a convenient way to create various types of + Functions. + */ + +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + + +template +class Function_ { +public: + class Constant; + class Linear; + class Polynomial; + class Sinusoid; + class Step; + virtual ~Function_() { + } + /** + Calculate the value of this function at a particular point. + + @param x the RigidBodyDynamics::Math::VectorNd of input arguments. Its + size must equal the value returned by getArgumentSize(). + */ + virtual T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const = 0; + /** + Calculate a partial derivative of this function at a particular point. + Which derivative to take is specified by listing the input components + with which to take it. For example, if derivComponents=={0}, that + indicates a first derivative with respective to component 0. If + derivComponents=={0, 0, 0}, that indicates a third derivative with + respective to component 0. If derivComponents=={4, 7}, that indicates a + partial second derivative with respect to components 4 and 7. + + @param derivComponents + The input components with respect to which the derivative should be + taken. Its size must be less than or equal to the value returned + by getMaxDerivativeOrder(). + @param x + The RigidBodyDynamics::Math::VectorNd of input arguments. Its size must + equal the value + returned by getArgumentSize(). + @return + The value of the selected derivative, which is of type T. + */ + virtual T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const = 0; + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(std::vector(derivComponents),x); } + */ + + /** + * Get the number of components expected in the input vector. + */ + virtual int getArgumentSize() const = 0; + /** + * Get the maximum derivative order this Function_ object can calculate. + */ + virtual int getMaxDerivativeOrder() const = 0; +}; + +/** This typedef is used for the very common case that the return type of +the Function object is double. **/ +typedef Function_ Function; + + + +/** + * This is a Function_ subclass which simply returns a fixed value, independent + * of its arguments. + */ +template +class Function_::Constant : public Function_ { +public: + /** + * Create a Function_::Constant object. + * + * @param value the value which should be returned by calcValue(); + * @param argumentSize the value which should be returned by + * getArgumentSize(), with a default of 1. + */ + explicit Constant(T value, int argumentSize=1) + : argumentSize(argumentSize), value(value) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == argumentSize); + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + return static_cast(0); + } + virtual int getArgumentSize() const { + return argumentSize; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(std::vector(derivComponents),x); } + */ +private: + const int argumentSize; + const T value; +}; + +/** + * This is a Function_ subclass whose output value is a linear function of its + * arguments: f(x, y, ...) = ax+by+...+c. + */ +template +class Function_::Linear : public Function_ { +public: + /** + * Create a Function_::Linear object. + * + * @param coefficients + * The coefficients of the linear function. The number of arguments + * expected by the function is equal to coefficients.size()-1. + * coefficients[0] is the coefficient for the first argument, + * coefficients[1] is the coefficient for the second argument, etc. + * The final element of coefficients contains the constant term. + */ + explicit Linear( + const RigidBodyDynamics::Math::VectorNd& coefficients) + : coefficients(coefficients) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == coefficients.size()-1); + T value = static_cast(0); + for (int i = 0; i < x.size(); ++i) + value += x[i]*coefficients[i]; + value += coefficients[x.size()]; + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == coefficients.size()-1); + assert(derivComponents.size() > 0); + if (derivComponents.size() == 1) + return coefficients(derivComponents[0]); + return static_cast(0); + } + virtual int getArgumentSize() const { + return coefficients.size()-1; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const RigidBodyDynamics::Math::VectorNd coefficients; +}; + + +/** + * This is a Function_ subclass whose output value is a polynomial of its + * argument: f(x) = ax^n+bx^(n-1)+...+c. + */ +template +class Function_::Polynomial : public Function_ { +public: + /** + * Create a Function_::Polynomial object. + * + * @param coefficients the polynomial coefficients in order of decreasing + * powers + */ + Polynomial(const RigidBodyDynamics::Math::VectorNd& coefficients) + : coefficients(coefficients) { + } + T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == 1); + double arg = x[0]; + T value = static_cast(0); + for (int i = 0; i < coefficients.size(); ++i) + value = value*arg + coefficients[i]; + return value; + } + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + assert(x.size() == 1); + assert(derivComponents.size() > 0); + double arg = x[0]; + T value = static_cast(0); + const int derivOrder = (int)derivComponents.size(); + const int polyOrder = coefficients.size()-1; + for (int i = 0; i <= polyOrder-derivOrder; ++i) { + T coeff = coefficients[i]; + for (int j = 0; j < derivOrder; ++j) + coeff *= polyOrder-i-j; + value = value*arg + coeff; + } + return value; + } + virtual int getArgumentSize() const { + return 1; + } + int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const RigidBodyDynamics::Math::VectorNd coefficients; +}; + + +/** + * This is a Function_ subclass whose output value is a sinusoid of its + * argument: f(x) = a*sin(w*x + p) where a is amplitude, w is frequency + * in radians per unit of x, p is phase in radians. + * + * This is only defined for a scalar (double) return value. + */ +template <> +class Function_::Sinusoid : public Function_ { +public: + /** + * Create a Function::Sinusoid object, returning a*sin(w*x+p). + * + * @param[in] amplitude 'a' in the above formula + * @param[in] frequency 'w' in the above formula + * @param[in] phase 'p' in the above formula + */ + Sinusoid(double amplitude, double frequency, double phase=0) + : a(amplitude), w(frequency), p(phase) {} + + void setAmplitude(double amplitude) {a=amplitude;} + void setFrequency(double frequency) {w=frequency;} + void setPhase (double phase) {p=phase;} + + double getAmplitude() const {return a;} + double getFrequency() const {return w;} + double getPhase () const {return p;} + + // Implementation of Function_ virtuals. + + virtual double calcValue( + const RigidBodyDynamics::Math::VectorNd& x) const { + + const double t = x[0]; // we expect just one argument + return a*std::sin(w*t + p); + } + + virtual double calcDerivative( + const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const { + + const double t = x[0]; // time is the only argument + const int order = derivComponents.size(); + // The n'th derivative is + // sign * a * w^n * sc + // where sign is -1 if floor(order/2) is odd, else 1 + // and sc is cos(w*t+p) if order is odd, else sin(w*t+p) + switch(order) { + case 0: return a* std::sin(w*t + p); + case 1: return a*w* std::cos(w*t + p); + case 2: return -a*w*w* std::sin(w*t + p); + case 3: return -a*w*w*w*std::cos(w*t + p); + default: + const double sign = double(((order/2) & 0x1) ? -1 : 1); + const double sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p); + const double wn = std::pow(w, order); + return sign*a*wn*sc; + } + } + + virtual int getArgumentSize() const {return 1;} // just time + virtual int getMaxDerivativeOrder() const { + return std::numeric_limits::max(); + } + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + double calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + double a, w, p; +}; + +/** + * This is a Function_ subclass whose output value y=f(x) is smoothly stepped + * from y=y0 to y1 as its input argument goes from x=x0 to x1. This is + * an S-shaped function with first and second derivatives y'(x0)=y'(x1)=0 + * and y''(x0)=y''(x1)==0. The third derivative y''' exists and is continuous + * but we cannot guarantee anything about it at the end points. + */ +template +class Function_::Step : public Function_ { +public: + /** + * Create a Function_::Step object that smoothly interpolates its output + * through a given range as its input moves through its range. + * + * @param y0 Output value when (x-x0)*sign(x1-x0) <= 0. + * @param y1 Output value when (x-x1)*sign(x1-x0) >= 0. + * @param x0 Start of switching interval. + * @param x1 End of switching interval. + * + * @tparam T The template type is the type of y0 and y1. This must + * be a type that supports subtraction and scalar + * multiplication by a double so that we can compute + * an expression like y=y0 + f*(y1-y0) for some double scalar f. + * + * Note that the numerical values of x0 and x1 can be in either order + * x0 < x1 or x0 > x1. + */ + Step(const T& y0, const T& y1, double x0, double x1) + : m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(double(0)*y0), + m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(copysign(1,m_ooxr)) + { + /* + SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_::Step::ctor()", + "A zero-length switching interval is illegal; both ends were %g.", x0); + */ + assert(x0 != x1); + std::printf( "Function_::Step::ctor():" + "A zero-length switching interval " + "is illegal; both ends were %g.", x0); + + } + + T calcValue(const RigidBodyDynamics::Math::VectorNd& xin) const { + /* + SimTK_ERRCHK1_ALWAYS(xin.size() == 1, + "Function_::Step::calcValue()", + "Expected just one input argument but got %d.", xin.size()); + */ + assert(xin.size() == 1); + std::printf( "Function_::Step::calcValue() " + "Expected just one input argument but got %d.", + xin.size()); + + + const double x = xin[0]; + if ((x-m_x0)*m_sign <= 0) return m_y0; + if ((x-m_x1)*m_sign >= 0) return m_y1; + // f goes from 0 to 1 as x goes from x0 to x1. + const double f = step01(m_x0,m_ooxr, x); + return m_y0 + f*m_yr; + } + + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& xin) const { + /* + SimTK_ERRCHK1_ALWAYS(xin.size() == 1, + "Function_::Step::calcDerivative()", + "Expected just one input argument but got %d.", xin.size()); + */ + assert(xin.size() == 1); + std::printf( "Function_::Step::calcDerivative() " + "Expected just one input argument but got %d.", + xin.size()); + + const int derivOrder = (int)derivComponents.size(); + + /* + SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3, + "Function_::Step::calcDerivative()", + "Only 1st, 2nd, and 3rd derivatives of the step are available," + " but derivative %d was requested.", derivOrder); + */ + assert(1 <= derivOrder && derivOrder <= 3); + std::printf("Function_::Step::calcDerivative() " + "Only 1st, 2nd, and 3rd derivatives of the step are available," + " but derivative %d was requested.", derivOrder); + + const double x = xin[0]; + if ((x-m_x0)*m_sign <= 0) return m_zero; + if ((x-m_x1)*m_sign >= 0) return m_zero; + switch(derivOrder) { + case 1: return dstep01(m_x0,m_ooxr, x) * m_yr; + case 2: return d2step01(m_x0,m_ooxr, x) * m_yr; + case 3: return d3step01(m_x0,m_ooxr, x) * m_yr; + default: assert(!"impossible derivOrder"); + } + return NAN*m_yr; /*NOTREACHED*/ + } + + virtual int getArgumentSize() const {return 1;} + int getMaxDerivativeOrder() const {return 3;} + + /** This provides compatibility with std::vector without requiring any + copying. **/ + /* + T calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const + { return calcDerivative(ArrayViewConst_(derivComponents),x); } + */ +private: + const T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0) + const T m_zero; // precalculate T(0) + const double m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0) + const double m_sign; // sign(x1-x0) is 1 or -1 + + double step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double u2 = u*u; + double u3 = u2*u; + return (3*u2 - 2*u3); + } + + double dstep01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + double du2 = 2*u*du; + double du3 = 3*u*u*du; + return (3*du2 - 2*du3); + } + + double d2step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + //double ddu = 0; + double ddu2 = 2*du*du;// + 2*u*ddu since ddu=0; + double ddu3 = 3*du*u*du + 3*u*du*du;// + 3*u*u*du; ditto + return (3*ddu2 - 2*ddu3); + } + + double d3step01(double x0, double x1, double x){ + double u = (x-x0)/(x1-x0); + double du = (1)/(x1-x0); + //double ddu = 0; + //double dddu = 0; + double dddu2 = 0; //2*du*du;// since ddu=0; + double dddu3 = 3*du*du*du + 3*du*du*du;// ditto + return (3*dddu2 - 2*dddu3); + } +}; + +} +} +} + +#endif // SimTK_SimTKCOMMON_FUNCTION_H_ + + diff --git a/3rdparty/rbdl/addons/geometry/LICENSE b/3rdparty/rbdl/addons/geometry/LICENSE new file mode 100644 index 0000000..6e03346 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/LICENSE @@ -0,0 +1,23 @@ +Rigid Body Dynamics Library Geometry Addon - +Copyright (c) 2016 Matthew Millard + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt b/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/LICENSE_APACHE-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/3rdparty/rbdl/addons/geometry/NOTICE b/3rdparty/rbdl/addons/geometry/NOTICE new file mode 100644 index 0000000..d99650d --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/NOTICE @@ -0,0 +1,24 @@ +Author: + Matthew Millard + +Date: + July 2016 + +Notice: these files + + -Function.h + -SegmentedQuinticBezierToolkit.h + -SegmentedQuinticBezierToolkit.cc + -SmoothSegmentedFunction.h + -SmoothSegmentedFunction.cc + -tests/numericalTestFunctions.h + -tests/numericalTestFunctions.cc + -tests/testSmoothSegmentedFunction.cc + +were originally part of OpenSim and have been ported +over to RBDL with some modification. These files are licenced under the +APACHE 2.0 license which, like the zlib license, is quite liberal. The +full licence can be found in this folder in the file "LICENSE_APACHE-2.0.txt" +and online here: + +http://www.apache.org/licenses/LICENSE-2.0.txt diff --git a/3rdparty/rbdl/addons/geometry/README.md b/3rdparty/rbdl/addons/geometry/README.md new file mode 100644 index 0000000..be2861b --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/README.md @@ -0,0 +1,60 @@ +@brief geometry - a set of static tool kits for creating and evaluating curves, + surfaces and solids. This addon is maintained by Matthew + Millard, so if you have problems with it email him. + +@author Matthew Millard + +\copyright 2016 Matthew Millard + +\b Requirements +This addon is standalone as of its first release + +\b Description + This addon currently contains an optimized library for creating and + evaluating 5th order 2D Bezier splines: SegmentedQuinticBezierToolkit.h + and SegmentedQuinticBezierToolkit.cc. In addition, there is a nice class + that can be used to package the memory and functions required to + evaluate these curves: SmoothSegmentedFunction.h and + SmoothSegmentedFunction.cc. + +\b Future Development +In the near future this library will also contain + +1. Geometry tools to represent C2 convex implicit surfaces and enforce + contact constraints between two surfaces. This tool kit will be first + used for simulating foot-ground contact. It could later be used for + 3D muscle wrapping: + + SmoothImplicitSurfaceToolkit + SmoothImplicitSurface + +2. Geometry tools to represent quintic Pythagorean Hodograph curves - which are + special Bezier curves that have an analytic equation for arc-length. This + package will also contain code to represent polar Pythagorean Hodographs + which will be first used to formulate a foot-ground joint. Later this toolkit + will be used for a 2D cable transmission system + (to simulate muscle wrapping). + + SegmentedQuinticPythagoreanHodographToolkit + PolarFunctionToolkit + + +\b Licensing +The following files have been ported over from OpenSim and Simbody and as such +are licenced under the Apache 2.0 Licence: + +SmoothSegmentedFunction.h +SmoothSegmentedFunction.cc +SegmentedQuinticBezierToolkit.h +SegmentedQuinticBezierToolkit.cc +Function.h + +The Apache License is very similar to the zlib license and is quite liberal. +Licensed under the Apache License, Version 2.0 (the "License"); you may +not use this file except in compliance with the License. You may obtain a +copy of the License at http://www.apache.org/licenses/LICENSE-2.0. + +The remaining code has been written from scratch and is licenced under the +zlib license. See the LICENSE file for details. + + diff --git a/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc new file mode 100644 index 0000000..526efcd --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.cc @@ -0,0 +1,1285 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SegmentedQuinticBezierToolkit.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= +#include "SegmentedQuinticBezierToolkit.h" +#include +#include +#include +#include +#include + +//#include +using namespace RigidBodyDynamics::Addons::Geometry; + +//============================================================================= +// STATICS +//============================================================================= +//using namespace SimTK; +//using namespace OpenSim; +using namespace std; +using namespace RigidBodyDynamics::Math; +static double UTOL_DESIRED = std::numeric_limits::epsilon()*1e2; +static double UTOL_INITIALSOLN = std::numeric_limits::epsilon()*1e11; +static int MAXITER_INITIALSOLN = 12; + +static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample + //each Bezier corner section + +double SegmentedQuinticBezierToolkit::scaleCurviness(double curviness) +{ + double c = 0.1 + 0.8*curviness; + return c; +} + +/** +This function will print cvs file of the column vector col0 and the matrix data +@params col0: A vector that must have the same number of rows as the data matrix + This column vector is printed as the first column +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void SegmentedQuinticBezierToolkit:: + printMatrixToFile( const VectorNd& col0, + const MatrixNd& data, + std::string& filename) +{ + + ofstream datafile; + datafile.open(filename.c_str()); + + for(int i = 0; i < data.rows(); i++){ + datafile << col0[i] << ","; + for(int j = 0; j < data.cols(); j++){ + if(j& curveFit, + MatrixNd& ctrlPts, + VectorNd& xVal, + VectorNd& yVal, + std::string& filename) +{ + std::string caller = "printBezierSplineFitCurves"; + int nbezier = int(ctrlPts.cols()/2.0); + int rows = NUM_SAMPLE_PTS*nbezier - (nbezier-1); + + VectorNd y1Val(rows); + VectorNd y2Val(rows); + + VectorNd ySVal(rows); + VectorNd y1SVal(rows); + VectorNd y2SVal(rows); + + MatrixNd printMatrix(rows,6); + + VectorNd tmp(1); + std::vector deriv1(1); + std::vector deriv2(2); + + deriv1[0] = 0; + deriv2[0] = 0; + deriv2[1] = 0; + double u = 0; + int oidx = 0; + int offset = 0; + for(int j=0; j < nbezier ; j++) + { + if(j > 0){ + offset = 1; + } + + for(int i=0; i=0 && curviness <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + "Error: double argument curviness must be between 0.0 and 1.0."); + */ + if( !(curviness>=0 && curviness <= 1) ){ + + cerr << "SegmentedQuinticBezierToolkit::" + <<"calcQuinticBezierCornerControlPoints" + <<"Error: double argument curviness must be between 0.0 and 1.0." + <<"curviness : " << curviness << " " + << endl; + assert (0); + abort(); + } + + + //1. Calculate the location where the two lines intersect + // (x-x0)*dydx0 + y0 = (x-x1)*dydx1 + y1 + // x*(dydx0-dydx1) = y1-y0-x1*dydx1+x0*dydx0 + // x = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); + + double xC = 0.; + double yC = 0.; + double epsilon = std::numeric_limits::epsilon(); + double rootEPS = sqrt(epsilon); + if(abs(dydx0-dydx1) > rootEPS){ + xC = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1); + }else{ + xC = (x1+x0)/2.0; + } + + yC = (xC-x1)*dydx1 + y1; + //Check to make sure that the inputs are consistent with a corner, and will + //not produce an 's' shaped section. To check this we compute the sides of + //a triangle that is formed by the two points that the user entered, and + //also the intersection of the 2 lines the user entered. If the distance + //between the two points the user entered is larger than the distance from + //either point to the intersection loctaion, this function will generate a + //'C' shaped curve. If this is not true, an 'S' shaped curve will result, + //and this function should not be used. + + double xCx0 = (xC-x0); + double yCy0 = (yC-y0); + double xCx1 = (xC-x1); + double yCy1 = (yC-y1); + double x0x1 = (x1-x0); + double y0y1 = (y1-y0); + + + + double a = xCx0*xCx0 + yCy0*yCy0; + double b = xCx1*xCx1 + yCy1*yCy1; + double c = x0x1*x0x1 + y0y1*y0y1; + + //This error message needs to be better. + /* + SimTK_ERRCHK_ALWAYS( ((c > a) && (c > b)), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints", + "The intersection point for the two lines defined by the input" + "parameters must be consistent with a C shaped corner."); + */ + + + + if( !((c > a) && (c > b)) ){ + cerr << "SegmentedQuinticBezierToolkit" + << "::calcQuinticBezierCornerControlPoints:" + << "The line segments at the end of the curve sections " + << "do not intersect within the domain " + << "("<< x0 << "," << x1 << ") of the curve. " + << "and so there is a chance that curve will not" + << " be monotonic. There are 2 ways to fix this problem: " + << endl + << "1. Add an intermediate point," + << endl + << " 2. Space the domain points more widely " + << endl + << "Details: " + << endl << " a: " << a + << endl << " b: " << b + << endl << " c: " << c << endl; + assert (0); + abort(); + + } + + /* + Value of the 2nd derivative at the end points. + This is not exposed to the user for now, as rarely is possible + or even easy to know what these values should be. Internally + I'm using this here because we get curves with nicer 1st + derivatives than if we take the easy option to get a second + derivative of zero (by setting the middle control points equal + to their neighbors. + */ + + double d2ydx20 = 0; + double d2ydx21 = 0; + + //Start point + xyPts(0,0) = x0; + xyPts(0,1) = y0; + //End point + xyPts(5,0) = x1; + xyPts(5,1) = y1; + + + /* + //Original code - leads to 2 localized corners + xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0)); + xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1)); + //xyPts(2,0) = xyPts(1,0); + //xyPts(2,1) = xyPts(1,1); + + //Second two midpoints + xyPts(3,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0)); + xyPts(3,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1)); + xyPts(4,0) = xyPts(3,0); + xyPts(4,1) = xyPts(3,1); + */ + + //Set the 1st and 4th control points (nearest to the end points) + //to get the correct first derivative + xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0)); + xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1)); + + xyPts(4,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0)); + xyPts(4,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1)); + + //Now go and update the middle points to get the desired 2nd + //derivative at the ends. Note that even if d2ydx2 = 0 the + //resulting curve using this method has a much smoother 1st + //derivative than if the middle control points are set to be + //equal to the 1st and 4th control points. + + double dxdu0 = 5.0*(xyPts(1,0) - xyPts(0,0)); + xyPts(2,0) = xyPts(1,0) + 0.5*(xC-xyPts(1,0)) ; + double d2xdu20 = 20.0*(xyPts(2,0) - 2.0*xyPts(1,0) + xyPts(0,0)); + double d2ydu20 = (dxdu0*dxdu0*(d2ydx20) + d2xdu20*(dydx0)) ; + xyPts(2,1) = d2ydu20*(1.0/20.0) + 2.0*xyPts(1,1) - xyPts(0,1) ; + + double dxdu1 = 5.0*(xyPts(5,0) - xyPts(4,0)); + xyPts(3,0) = xyPts(4,0) + 0.5*(xC-xyPts(4,0)); + double d2xdu21 = 20.0*(xyPts(3,0) - 2.0*xyPts(4,0) + xyPts(5,0) ); + double d2ydu21 = (dxdu1*dxdu1*(d2ydx21) + d2xdu21*(dydx1)); + xyPts(3,1) = d2ydu21*(1.0/20.0) + 2.0*xyPts(4,1) - xyPts(5,1); + + return xyPts; +} + +//============================================================================= +// BASIC QUINTIC BEZIER EVALUATION FUNCTIONS +//============================================================================= + +/* +Multiplications Additions Assignments +21 20 13 +*/ +double SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(double u1, const VectorNd& pts) +{ + double val = -1; + + /* + SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + "Error: double argument u must be between 0.0 and 1.0" + "but %f was entered.",u); + */ + if(!(u1 >= 0 && u1 <= 1)){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal" + << "Error: double argument u must be between 0.0 and 1.0" + << "but " << u1 <<" was entered."; + assert (0); + abort(); + } + + /* + SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", + "Error: vector argument pts must have a length of 6."); + */ + if(!(pts.size() == 6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal:" + << "Error: vector argument pts must have a length of 6."; + assert (0); + abort(); + } + + double u2 = u1*u1; + double u3 = u2*u1; + double u4 = u3*u1; + double u5 = u4*u1; + + //v0 = 1; + double v1 = (1-u1); + double v2 = v1*v1; + double v3 = v2*v1; + double v4 = v3*v1; + double v5 = v4*v1; + + val = pts[0] *v5*1.0 + + pts[1]*u1*v4*5.0 + + pts[2]*u2*v3*10.0 + + pts[3]*u3*v2*10.0 + + pts[4]*u4*v1*5.0 + + pts[5]*u5 *1.0; + + + return val; +} + +/* +Detailed Computational Costs + dy/dx Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + dy/dx 1 + total 1 40 38 22 + + d2y/dx2 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d2y/dx2 2 4 1 3 + total 2 78 73 23 + + d3y/dx3 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + + d3y/dx3 4 16 5 6 + total 4 118 105 58 + + d4y/dx4 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + + d4y/dx4 5 44 15 13 + total 5 168 137 71 + + d5y/dx5 Divisions Multiplications Additions Assignments + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + d5y/du5 6 6 1 + d5x/du5 6 6 1 + + d5y/dx5 7 100 36 28 + total 7 236 170 88 + + d6y/dx6 + dy/du 20 19 11 + dx/du 20 19 11 + d2y/du2 17 17 9 + d2x/du2 17 17 9 + d3y/du3 14 14 6 + d3x/du3 14 14 6 + d4y/du4 11 11 3 + d4x/du4 11 11 3 + d5y/du5 6 6 1 + d5x/du5 6 6 1 + + d6y/dx6 9 198 75 46 + total 9 334 209 106 + +*/ +double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( + double u, + const VectorNd& xpts, + const VectorNd& ypts, + int order) +{ + double val = NAN;//SimTK::NaN; + + //Bounds checking on the input + /* + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: double argument u must be between 0.0 and 1.0."); + + SimTK_ERRCHK_ALWAYS( (xpts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument xpts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (ypts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument ypts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (order >= 1), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be greater than."); + + SimTK_ERRCHK_ALWAYS( (order <= 6), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be less than, or equal to 6."); + */ + if( !(u>=0 && u <= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + assert(0); + abort(); + + } + + if( !(xpts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument xpts must have a length of 6." + << endl; + assert(0); + abort(); + + } + if( !(ypts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument ypts must have a length of 6." + << endl; + assert(0); + abort(); + } + + if( !(order >= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than." + << endl; + assert(0); + abort(); + } + + if( !(order <= 6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be less than, or equal to 6." + << endl; + assert(0); + abort(); + } + //std::string localCaller = caller; + //localCaller.append(".calcQuinticBezierCurveDerivDYDX"); + //Compute the derivative d^n y/ dx^n + switch(order){ + case 1: //Calculate dy/dx + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double dydx = dydu/dxdu; + + val = dydx; + //Question: + //how is a divide by zero treated? Is SimTK::INF returned? + } + break; + case 2: //Calculate d^2y/dx^2 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + + //Optimized code from Maple - + //see MuscleCurveCodeOpt_20120210 for details + double t1 = 0.1e1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double d2ydx2 = (d2ydu2 * t1 - dydu / t3 * d2xdu2) * t1; + + val = d2ydx2; + + } + break; + case 3: //Calculate d^3y/dx^3 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//(dxdu ^ 2); + double t4 = 1 / t3; + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (dydu * t4); + double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2 + + 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1 + - (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1; + + val = d3ydx3; + } + break; + case 4: //Calculate d^4y/dx^4 + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d2ydu2 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t23 = (dydu * t9); + double t27 = (dydu * t4); + double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11 + - t27 * d3xdu3; + double t43 = d2ydu2 * t1 - t27 * d2xdu2; + double t47 = t43 * t4; + double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2 + + 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3 + - 6 * dydu / t17 * t11 * d2xdu2 + + 6 * t23 * d2xdu2 * d3xdu3 + - t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2 + + 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1 + - (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1; + + + val = d4ydx4; + + } + break; + case 5: + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + + double t1 = 1 / dxdu; + double t3 = dxdu*dxdu;//dxdu ^ 2; + double t4 = 1 / t3; + double t9 = (0.1e1 / t3 / dxdu); + double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t14 = (d3ydu3 * t4); + double t17 = t3*t3;//(t3 ^ 2); + double t18 = 1 / t17; + double t20 = (t11 * d2xdu2); + double t23 = (d2ydu2 * t9); + double t24 = (d2xdu2 * d3xdu3); + double t27 = (d2ydu2 * t4); + double t33 = t11*t11;//(t11 ^ 2); + double t36 = (dydu * t18); + double t40 = (dydu * t9); + double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t47 = (dydu * t4); + double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2 + + 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3 + - 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24 + - 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33 + - 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41 + + 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5; + double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11 + - 3 * t27 * d3xdu3 - 6 * t36 * t20 + + 6 * t40 * t24 - t47 * d4xdu4; + double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11 + - t47 * d3xdu3; + double t77 = t73 * t4; + double t82 = d2ydu2 * t1 - t47 * d2xdu2; + double t86 = t82 * t9; + double t89 = t82 * t4; + double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11 + - t89 * d3xdu3; + double t105 = t73 * t1 - t89 * d2xdu2; + double t109 = t105 * t4; + double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2 + + 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3 + - 6 * t82 * t18 * t20 + + 6 * t86 * t24 - t89 * d4xdu4) * t1 + - 2 * t99 * t4 * d2xdu2 + + 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1 + - (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1; + + + val = d5ydx5; + + } + break; + case 6: + { + double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1); + double dydu =calcQuinticBezierCurveDerivU(u,ypts,1); + double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2); + double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2); + double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3); + double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3); + double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4); + double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4); + double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5); + double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5); + double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6); + double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6); + + double t1 = dxdu*dxdu;//(dxdu ^ 2); + double t3 = (0.1e1 / t1 / dxdu); + double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2); + double t8 = t1*t1;//(t1 ^ 2); + double t9 = 1 / t8; + double t11 = (t5 * d2xdu2); + double t14 = (d3ydu3 * t3); + double t15 = (d2xdu2 * d3xdu3); + double t19 = (0.1e1 / t8 / dxdu); + double t21 = t5*t5;//(t5 ^ 2); + double t24 = (d2ydu2 * t9); + double t25 = (t5 * d3xdu3); + double t28 = (d2ydu2 * t3); + double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2); + double t32 = (d2xdu2 * d4xdu4); + double t41 = (dydu * t19); + double t45 = (dydu * t9); + double t49 = (dydu * t3); + double t56 = 1 / dxdu; + double t61 = 1 / t1; + double t62 = (dydu * t61); + double t67 = (d4ydu4 * t61); + double t70 = (d2ydu2 * t61); + double t73 = (d3ydu3 * t61); + double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11 + + 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21 + - 180 * t24 * t25 + + 30 * t28 * t29 + 40 * t28 * t32 + - 120 * dydu / t8 / t1 * t21 * d2xdu2 + + 240 * t41 *t11*d3xdu3 + - 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4 + + 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56 + - 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6 + - 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3 + - 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4; + + double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5 + - 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15 + - 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25 + + 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5; + + double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5 + - 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15 + - t62 * d4xdu4; + + double t120 = t116 * t61; + double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5 + - t62 * d3xdu3; + double t133 = t129 * t3; + double t136 = t129 * t61; + double t141 = d2ydu2 * t56 - t62 * d2xdu2; + double t145 = t141 * t9; + double t148 = t141 * t3; + double t153 = t141 * t61; + double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2 + + 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3 + - 24 * t129 * t9 * t11 + 24 * t133 * t15 + - 4 * t136 * d4xdu4 + + 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29 + + 8 * t148 * t32 - t153 * d5xdu5; + + double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5 + - 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15 + - t153 * d4xdu4; + + double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5 + - t153 * d3xdu3; + + double t183 = t179 * t61; + double t188 = t129 * t56 - t153 * d2xdu2; + double t192 = t188 * t3; + double t195 = t188 * t61; + double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5 + - t195 * d3xdu3; + double t211 = t179 * t56 - t195 * d2xdu2; + double t215 = t211 * t61; + double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2 + + 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3 + - 6 * t188 * t9 *t11 + + 6 * t192 * t15 - t195 * d4xdu4) * t56 + - 2 * t205 * t61 * d2xdu2 + + 2 * t211*t3*t5-t215*d3xdu3)*t56 + - (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56; + + + val = d6ydx6; + } + break; + default: + val = NAN; //SimTK::NaN; + } + return val; +} + +/* Computational Cost Details + Divisions Multiplications Additions Assignments +dx/du 20 19 11 +d2x/du2 17 17 9 +d3y/du3 14 14 6 + +*/ +double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU( + double u, + const VectorNd& pts, + int order) +{ + double val = -1; + /* + SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: double argument u must be between 0.0 and 1.0."); + + SimTK_ERRCHK_ALWAYS( (pts.size()==6) , + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: vector argument pts \nmust have a length of 6."); + + SimTK_ERRCHK_ALWAYS( (order >= 1), + "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", + "Error: order must be greater than, or equal to 1"); + */ + if( !(u>=0 && u <= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: double argument u must be between 0.0 and 1.0." + << endl; + assert(0); + abort(); + } + if( !(pts.size()==6) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: vector argument pts must have a length of 6." + << endl; + assert(0); + abort(); + } + + if( !(order >= 1) ){ + cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:" + << "Error: order must be greater than, or equal to 1" + << endl; + assert(0); + abort(); + } + + //Compute the Bezier point + double p0 = pts[0]; + double p1 = pts[1]; + double p2 = pts[2]; + double p3 = pts[3]; + double p4 = pts[4]; + double p5 = pts[5]; + + switch(order){ + case 1: + { + double t1 = u*u;//u ^ 2; + double t2 = t1*t1;//t1 ^ 2; + double t4 = t1 * u; + double t5 = t4 * 0.20e2; + double t6 = t1 * 0.30e2; + double t7 = u * 0.20e2; + double t10 = t2 * 0.25e2; + double t11 = t4 * 0.80e2; + double t12 = t1 * 0.90e2; + double t16 = t2 * 0.50e2; + val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1) + + p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1) + + p2 * (-t16 + t4 * 0.120e3 - t12 + t7) + + p3 * (t16 - t11 + t6) + + p4 * (-t10 + t5) + + p5 * t2 * 0.5e1; + + } + break; + case 2: + { + double t1 = u*u;//u ^ 2; + double t2 = t1 * u; + double t4 = t1 * 0.60e2; + double t5 = u * 0.60e2; + double t8 = t2 * 0.100e3; + double t9 = t1 * 0.240e3; + double t10 = u * 0.180e3; + double t13 = t2 * 0.200e3; + val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2) + + p1 * (t8 - t9 + t10 - 0.40e2) + + p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2) + + p3 * (t13 - t9 + t5) + + p4 * (-t8 + t4) + + p5 * t2 * 0.20e2; + + } + break; + case 3: + { + double t1 = u*u;//u ^ 2; + double t3 = u * 0.120e3; + double t6 = t1 * 0.300e3; + double t7 = u * 0.480e3; + double t10 = t1 * 0.600e3; + val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2) + + p1 * (t6 - t7 + 0.180e3) + + p2 * (-t10 + u * 0.720e3 - 0.180e3) + + p3 * (t10 - t7 + 0.60e2) + + p4 * (-t6 + t3) + + p5 * t1 * 0.60e2; + + } + break; + case 4: + { + double t4 = u * 0.600e3; + double t7 = u * 0.1200e4; + val = p0 * (u * (-0.120e3) + 0.120e3) + + p1 * (t4 - 0.480e3) + + p2 * (-t7 + 0.720e3) + + p3 * (t7 - 0.480e3) + + p4 * (-t4 + 0.120e3) + + p5 * u * 0.120e3; + } + break; + case 5: + { + val = p0 * (-0.120e3) + + p1 * 0.600e3 + + p2 * (-0.1200e4) + + p3 * 0.1200e4 + + p4 * (-0.600e3) + + p5 * 0.120e3; + } + break; + default: + val=0; + + } + + return val; + + +} + +double SegmentedQuinticBezierToolkit::clampU(double u){ + double uC = u; + if(u<0.0){ + uC=0; + } + if(u>1.0){ + uC=1; + } + return uC; +} + +/*Detailed Computational Costs + Comparisons Div Mult Additions Assignments + Geuss calculation 1 1 1 + + Newton Iter + f 21 20 13 + df 20 19 11 + update 4 1 3 6 + total 4 1 41 42 30 + \endverbatim + + To evaluate u to SimTK::Eps*100 this typically involves 2 Newton + iterations, yielding a total cost of + + \verbatim + Comparisons Div Mult Additions Assignments + eval U 7+8=15 2 82 42 60 +*/ +double SegmentedQuinticBezierToolkit::calcU(double ax, + const VectorNd& bezierPtsX, + double tol, + int maxIter) +{ + //Check to make sure that ax is in the curve domain + double minX = std::numeric_limits::max(); + double maxX = -minX; + for(int i=0; i maxX) + maxX = bezierPtsX[i]; + if(bezierPtsX[i] < minX) + minX = bezierPtsX[i]; + } + + /* + SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, + "SegmentedQuinticBezierToolkit::calcU", + "Error: input ax was not in the domain of the Bezier curve specified \n" + "by the control points in bezierPtsX."); + */ + if( !(ax >= minX && ax <= maxX) ){ + cerr << "SegmentedQuinticBezierToolkit::calcU:" + << "Error: input ax was not in the domain of the " + << "Bezier curve specified by the control points in bezierPtsX." + << endl; + assert(0); + abort(); + } + + double u = ax/(maxX-minX); + double f = 0; + + u = clampU(u); + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + + //Use the bisection method to get a good initial + //start for the Newton method. This is necessary + //as these curves, though C2, can be so nonlinear + //that the Newton method oscillates unless it is + //close to the initial solution. + if(abs(f) > tol){ + double uL = 0; + double uR = 1; + + double fL = calcQuinticBezierCurveVal(uL,bezierPtsX)-ax; + double fR = calcQuinticBezierCurveVal(uR,bezierPtsX)-ax; + + int iterBisection = 0; + + while(iterBisection < MAXITER_INITIALSOLN + && min(abs(fL),abs(fR)) > UTOL_INITIALSOLN ){ + u = 0.5*(uL+uR); + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + + if(signbit(f) == signbit(fL)){ + fL = f; + uL = u; + }else{ + fR = f; + uR = u; + } + iterBisection++; + } + + if(abs(fL) < abs(fR)){ + u = uL; + f = fL; + }else{ + u = uR; + f = fR; + } + } + + + + double df = 0; + double du = 0; + int iter = 0; + bool pathologic = false; + double fprev = f; + double stepLength = 1.0; + double perturbation01 = 0.0; + //Take Newton steps to the desired tolerance + while((abs(f) > min(tol,UTOL_DESIRED)) + && (iter < maxIter) + && (pathologic == false) ){ + //Take a Newton step + df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1); + + if(abs(df) > 0){ + du = -f/df; + u = u + stepLength*du; + u = clampU(u); + fprev = f; + f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax; + }else{ + //This should never happen. If we are unluky enough to get here + //purturb the current solution and continue until we run out of + //iterations. + perturbation01 = double(rand()%100)/100.0; + u = u + perturbation01*0.1; + u = clampU(u); + } + + iter++; + } + + //Check for convergence + if( abs(f) > tol ){ + std::stringstream errMsg; + errMsg.precision(17); + errMsg << "SegmentedQuinticBezierToolkit::calcU:" << endl + << "Error: desired tolerance of " << tol << endl + << " on U not met by the Newton iteration." << endl + << " A tolerance of " << f << " was reached." << endl + << " Curve range x(u): " << minX << "-" << maxX << endl + << " Desired x: " << ax << " closest u " << u << endl + << " Bezier points " << endl << bezierPtsX << endl; + cerr << errMsg.str(); + assert(0); + abort(); + } + + return u; +} +/* + +Cost: n comparisons, for a quintic Bezier curve with n-spline sections + + Comp Div Mult Add Assignments +Cost 3*n+2 1*n 3 + +*/ +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const MatrixNd& bezierPtsX) +{ + int idx = 0; + bool flag_found = false; + + for(int i=0; i= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){ + idx = i; + i = bezierPtsX.cols(); + flag_found = true; + } + } + + //Check if the value x is identically the last point + if(flag_found == false && x == bezierPtsX(5,bezierPtsX.cols()-1)){ + idx = bezierPtsX.cols()-1; + flag_found = true; + } + + /* + SimTK_ERRCHK_ALWAYS( (flag_found == true), + "SegmentedQuinticBezierToolkit::calcIndex", + "Error: A value of x was used that is not within the Bezier curve set."); + */ + if( !(flag_found == true)){ + cerr << "SegmentedQuinticBezierToolkit::calcIndex" + << "Error: A value of x was used that is not" + << " within the Bezier curve set." << endl; + assert(0); + abort(); + } + + + + + return idx; +} + +int SegmentedQuinticBezierToolkit::calcIndex(double x, + const std::vector& bezierPtsX) +{ + int idx = 0; + bool flag_found = false; + + int n = bezierPtsX.size(); + for(int i=0; i= bezierPtsX[i][0] && x < bezierPtsX[i][5] ){ + idx = i; + flag_found = true; + break; + } + } + + //Check if the value x is identically the last point + if(!flag_found && x == bezierPtsX[n-1][5]){ + idx = n-1; + flag_found = true; + } + + if(!(flag_found == true)){ + cerr << "SegmentedQuinticBezierToolkit::calcIndex " + << "Error: A value of x was used that is not " + << "within the Bezier curve set." + << endl; + assert(0); + abort(); + } + + return idx; +} + + + +/* + Comp Div Mult Additions Assignments +calcIdx 3*3+2=11 1*3=3 3 +calcU 15 2 82 42 60 +calcQuinticBezierCurveVal + 21 20 13 +Total 26 2 103 65 76 +\endverbatim + +Ignoring the costs associated with the integrator itself, and assuming +that the integrator evaluates the function 6 times per integrated point, +the cost of evaluating the integral at each point in vX is: + +\verbatim + Comp Div Mult Additions Assignments +RK45 on 1pt 6*(26 2 103 65 76) +Total 156 12 618 390 456 +\endverbatim + +Typically the integral is evaluated 100 times per section in order to +build an accurate spline-fit of the integrated function. Once again, +ignoring the overhead of the integrator, the function evaluations alone +for the current example would be + +\verbatim +RK45 on 100pts per section, over 3 sections + Comp Div Mult Additions Assignments + 3*100*(156 12 618 390 456 +Total 46,800 3600 185,400 117,000 136,000 + +*/ +/* +MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX( + const VectorNd& vX, + double ic0, double intAcc, + double uTol, int uMaxIter, + const MatrixNd& mX, const MatrixNd& mY, + const SimTK::Array_& aSplineUX, + bool flag_intLeftToRight, + const std::string& caller) +{ + MatrixNd intXY(vX.size(),2); + BezierData bdata; + bdata._mX = mX; + bdata._mY = mY; + bdata._initalValue = ic0; + bdata._aArraySplineUX = aSplineUX; + bdata._uMaxIter = uMaxIter; + bdata._uTol = uTol; + bdata._flag_intLeftToRight = flag_intLeftToRight; + bdata._name = caller; + + //These aren't really times, but I'm perpetuating the SimTK language + //so that I don't make a mistake + double startTime = vX(0); + double endTime = vX(vX.size()-1); + + if(flag_intLeftToRight){ + bdata._startValue = startTime; + }else{ + bdata._startValue = endTime; + } + + MySystem sys(bdata); + State initState = sys.realizeTopology(); + initState.setTime(startTime); + + + RungeKuttaMersonIntegrator integ(sys); + integ.setAccuracy(intAcc); + integ.setFinalTime(endTime); + integ.setReturnEveryInternalStep(false); + integ.initialize(initState); + + int idx = 0; + double nextTimeInterval = 0; + Integrator::SuccessfulStepStatus status; + + while (idx < vX.nelt()) { + if(idx < vX.nelt()){ + if(flag_intLeftToRight){ + nextTimeInterval = vX(idx); + }else{ + nextTimeInterval = endTime-vX(vX.size()-idx-1); + } + } + status=integ.stepTo(nextTimeInterval); + + // Use this for variable step output. + //status = integ.stepTo(Infinity); + + if (status == Integrator::EndOfSimulation) + break; + + const State& state = integ.getState(); + + if(flag_intLeftToRight){ + intXY(idx,0) = nextTimeInterval; + intXY(idx,1) = (double)state.getZ()[0]; + }else{ + intXY(vX.size()-idx-1,0) = vX(vX.size()-idx-1); + intXY(vX.size()-idx-1,1) = (double)state.getZ()[0]; + } + idx++; + + } + //intXY.resizeKeep(idx,2); + return intXY; +} +*/ diff --git a/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h new file mode 100644 index 0000000..bb76346 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SegmentedQuinticBezierToolkit.h @@ -0,0 +1,836 @@ +#ifndef SEGMENTEDQUINTICBEZIERTOOLKIT_H_ +#define SEGMENTEDQUINTICBEZIERTOOLKIT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: SegmentedQuinticBezierToolkit.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +#include +#include +#include "Function.h" + +/** +This is a low level Quintic Bezier curve class that contains functions to design +continuous sets of 'C' shaped Bezier curves, and to evaluate their values and +derivatives. A set in this context is used to refer to 2 or more quintic Bezier +curves that are continuously connected to eachother to form one smooth curve, +hence the name QuinticBezierSet. + +In the special case when this class is being used to generate and evaluate +2D Bezier curves, that is x(u) and y(u), there are also functions to evaluate +y(x), the first six derivatives of y(x), and also the first integral of y(x). + +This class was not designed to be a stand alone Quintic Bezier class, but rather +was developed out of necessity to model muscles. I required curves that, when +linearly extrapolated, were C2 continuous, and by necessity I had to use +quintic Bezier curves. In addition, the curves I was developing were functions +in x,y space, allowing many of the methods (such as the evaluation of y(x) given +that x(u) and y(u), the derivatives of y(x), and its first integral) to be +developed, though in general this can't always be done. + +I have parcelled all of these tools into their own class so that others may more +easily use and develop this starting point for their own means. I used the +following text during the development of this code: + +Mortenson, Michael E (2006). Geometric Modeling Third Edition. Industrial Press +Inc., New York. Chapter 4 was quite helpful. + +Future Upgrades + +1. Analytical Inverse to x(u): + I think this is impossible because it is not possible, in general, to find the + roots to a quintic polynomial, however, this fact may not preclude forming the + inverse curve. The impossibility of finding the roots to a quintic polynomial + was proven by Abel (Abel's Impossibility Theorem) and Galois + + http://mathworld.wolfram.com/QuinticEquation.html + + At the moment I am approximating the curve u(x) using cubic splines to return + an approximate value for u(x), which I polish using Newton's method to the + desired precision. + + **Note as of Nov 2015** + -> The cubic spline approximation of the inverse curve has been + removed. Since there is no spline class in RBDL (and I'm not + motivated to port it over from SimTK) this functionality does + not work. In addition, I've since found that this nice inverse + only saves a few Newton iterations over a calculated guess. + It's not worth the effort to include. + +2. Analytical Integral of y(x): + This is possible using the Divergence Theorem applied to 2D curves. A nice + example application is shown in link 2 for computing the area of a closed + cubic Bezier curve. While I have been able to get the simple examples to work, + I have failed to successfully compute the area under a quintic Bezier curve + correctly. I ran out of time trying to fix this problem, and so at the present + time I am numerically computing the integral at a number of knot points and + then evaluating the spline to compute the integral value. + + a. http://en.wikipedia.org/wiki/Divergence_theorem + b. http://objectmix.com/graphics/133553-area-closed-bezier-curve.html + + **Note as of Nov 2015** + -> The splined numeric integral of the curve has been removed. There + is not an error-controlled numerical integrator in RBDL and so + it is not straight forward to include this feature. + -> For later: discuss options with Martin. + +3. calcU: + Currently the Bezier curve value and its derivative are computed separately to + evaluate f and df in the Newton iteration to solve for u(x). Code optimized to + compute both of these quantites at the same time could cut the cost of + evaluating x(u) and dx/du in half. Since these quantities are evaluated in an + iterative loop, this one change could yield substantial computational savings. + +4. calcIndex: +The function calcIndex computes which spline the contains the point of interest. +This function has been implemented assuming a small number of Bezier curve sets, +and so it simply linearly scans through the sets to determine the correct one to +use. This function should be upgraded to use the bisection method if large +quintic Bezier curve sets are desired. + +5. The addition of additional Bezier control point design algorithms, to create + 'S' shaped curves, and possibly do subdivision. + +6. Low level Code Optimization: +I have exported all of the low level code as optimized code from Maple. Although +the code produced using this means is reasonably fast, it is usally possible +to obtain superior performance (and sometimes less round off error) by +doing this work by hand. + +Computational Cost Details +All computational costs assume the following operation costs: + +\verbatim +Operation Type : #flops ++,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 +\endverbatim + +These relative weightings will vary processor to processor, and so any of +the quoted computational costs are approximate. + + + RBDL Port Notes + +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + +3. There have been some improvements to the function calcU in the + SegmentedQuinticBezierToolkit.cc code. This function evaluates + u such that x(u) - x* = 0. This is done using a Newton method. + However, because these curves can be very nonlinear, the Newton + method requires a very good initial start. In the OpenSim version + this good initial guess was provided by splined interpolation of + u(x). In the RBDL version this initial guess is provided by using + a bisection method until the error of x(u)-x* is within + sqrt(sqrt(tol)) or 2 Newton steps of the desired tolerance. + +@author Matt Millard +@version 0.0 + +*/ +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + +class SegmentedQuinticBezierToolkit +{ + + + public: + + /** + This scales the users value of curviness to be between [0+delta, 1-delta] + because if curviness is allowed to equal 0 or 1, the second derivative + becomes quite violent and the resulting curve is difficult to fit + splines to. + + @param curviness + @retval a scaled version of curviness + + */ + static double scaleCurviness(double curviness); + + /** + This function will compute the u value that correesponds to the given x + for a quintic Bezier curve. + + @param ax The x value + @param bezierPtsX The 6 Bezier point values + @param tol The desired tolerance on u. + @param maxIter The maximum number of Newton iterations allowed + + \b aborts \b + -if ax is outside the range defined in this Bezier spline section + -if the desired tolerance is not met + -if the derivative goes to 0 to machine precision + + This function will compute the u value that correesponds to the given x + for a quintic Bezier curve. This is accomplished by using an approximate + spline inverse of u(x) to get a very good initial guess, and then one or + two Newton iterations to polish the answer to the desired tolerance. + + Computational Costs + \verbatim + ~219 flops + \endverbatim + + Example: + @code + double xVal = 2; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + RigidBodyDynamics::Math::VectorNd x(100); + RigidBodyDynamics::Math::VectorNd u(100); + + //Now evalutate u at the given xVal + double u = SegmentedQuinticBezierToolkit:: + calcU(xVal,vX, 1e-12,20); + + @endcode + */ + static double calcU( + double ax, + const RigidBodyDynamics::Math::VectorNd& bezierPtsX, + double tol, + int maxIter); + + + + /** + Given a set of Bezier curve control points, return the index of the + set of control points that x lies within. + + @param x A value that is interpolated by the set of Bezier + curves + @param bezierPtsX A matrix of 6xn Bezier control points + + \b aborts \b + -If the index is not located within this set of Bezier points + + Given a set of Bezier curve control points, return the index of the + set of control points that x lies within. This function has been coded + assuming a small number of Bezier curve sets (less than 10), and so, + it simply scans through the Bezier curve sets until it finds the correct + one. + + + Computational Costs + Quoted for a Bezier curve set containing 1 to 5 curves. + \verbatim + ~9-25 + \endverbatim + + Example: + @code + RigidBodyDynamics::Math::MatrixNd mX(6,2); + + //The first set of spline points + mX(0,0) = -2; + mX(1,0) = -1; + mX(2,0) = -1; + mX(3,0) = 1; + mX(4,0) = 1; + mX(5,0) = 2; + //The second set of spline points + mX(0,1) = 2; + mX(1,1) = 3; + mX(2,1) = 3; + mX(3,1) = 5; + mX(4,1) = 5; + mX(5,1) = 6; + + //The value of x for which we want the index for + double xVal = 1.75; + int idx = SegmentedQuinticBezierToolkit::calcIndex(xVal,mX); + @endcode + + + */ + static int calcIndex( + double x, + const RigidBodyDynamics::Math::MatrixNd& bezierPtsX); + + static int calcIndex( + double x, + const std::vector& bezierPtsX); + + + + + + /** + Calculates the value of a quintic Bezier curve at value u. + + @param u The independent variable of a Bezier curve, which ranges + between 0.0 and 1.0. + @param pts The locations of the control points in 1 dimension. + + \b aborts \b + -If u is outside of [0,1] + -if pts has a length other than 6 + @return The value of the Bezier curve located at u. + + Calculates the value of a quintic Bezier curve at value u. This + calculation is acheived by mulitplying a row vector comprised of powers + of u, by the 6x6 coefficient matrix associated with a quintic Bezier + curve, by the vector of Bezier control points, pV, in a particular + dimension. The code to compute the value of a quintic bezier curve has + been optimized to have the following cost: + + Computational Costs + \verbatim + ~54 flops + \endverbatim + + + + The math this function executes is decribed in pseudo code as the + following: + + \verbatim + uV = [u^5 u^4 u^3 u^2 u 1]; + + cM = [ -1 5 -10 10 -5 1; + 5 -20 30 -20 5 0; + -10 30 -30 10 0 0; + 10 -20 10 0 0 0; + -5 5 0 0 0 0; + 1 0 0 0 0 0 ]; + pV = [x1; x2; x3; x4; x5; x6]; + + xB = (uV*cM)*pV + \endverbatim + + + + Example: + @code + double u = 0.5; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,vX); + @endcode + + + */ + static double calcQuinticBezierCurveVal( + double u, + const RigidBodyDynamics::Math::VectorNd& pts); + + /** + Calculates the value of a quintic Bezier derivative curve at value u. + @param u The independent variable of a Bezier curve, which ranges + between 0.0 and 1.0. + @param pts The locations of the control points in 1 dimension. + @param order The desired order of the derivative. Order must be >= 1 + + \b aborts \b + -u is outside [0,1] + -pts is not 6 elements long + -if order is less than 1 + @return The value of du/dx of Bezier curve located at u. + + Calculates the value of a quintic Bezier derivative curve at value u. + This calculation is acheived by taking the derivative of the row vector + uV and multiplying it by the 6x6 coefficient matrix associated with a + quintic Bezier curve, by the vector of Bezier control points, pV, in a + particular dimension. + + Pseudo code for the first derivative (order == 1) would be + \verbatim + uV = [5*u^4 4*u^3 3*u^2 2u 1 0]; + + cM = [ -1 5 -10 10 -5 1; + 5 -20 30 -20 5 0; + -10 30 -30 10 0 0; + 10 -20 10 0 0 0; + -5 5 0 0 0 0; + 1 0 0 0 0 0 ]; + pV = [x1; x2; x3; x4; x5; x6]; + + dxdu = (uV*cM)*pV + \endverbatim + + Note that the derivative of uV only needed to be computed to compute + dxdu. This process is continued for all 5 derivatives of x(u) until + the sixth and all following derivatives, which are 0. Higher derivatives + w.r.t. to U are less expensive to compute than lower derivatives. + + Computational Costs + \verbatim + dy/dx : ~50 flops + d2x/du2: ~43 flops + d3x/du3: ~34 flops + d4x/du4: ~26 flops + d5x/du5: ~15 flops + d6x/du6: ~1 flop + \endverbatim + + + Example: + @code + double u = 0.5; + + //Choose the control points + RigidBodyDynamics::Math::VectorNd vX(6); + vX(0) = -2; + vX(1) = 0; + vX(2) = 0; + vX(3) = 4; + vX(4) = 4; + vX(5) = 6; + + double dxdu =calcQuinticBezierCurveDerivU(u,vX,1); + @endcode + */ + static double calcQuinticBezierCurveDerivU( + double u, + const RigidBodyDynamics::Math::VectorNd& pts, + int order); + + /** + Calculates the value of dydx of a quintic Bezier curve derivative at u. + + @param u The u value of interest. Note that u must be [0,1] + @param xpts The 6 control points associated with the x axis + @param ypts The 6 control points associated with the y axis + @param order The order of the derivative. Currently only orders from 1-6 + can be evaluated + + \b aborts \b + -If u is outside [0,1] + -If xpts is not 6 elements long + -If ypts is not 6 elements long + -If the order is less than 1 + -If the order is greater than 6 + @retval The value of (d^n y)/(dx^n) evaluated at u + + Calculates the value of dydx of a quintic Bezier curve derivative at u. + Note that a 2D Bezier curve can have an infinite number of derivatives, + because x and y are functions of u. Thus + + \verbatim + dy/dx = (dy/du)/(dx/du) + d^2y/dx^2 = d/du(dy/dx)*du/dx + = [(d^2y/du^2)*(dx/du) - (dy/du)*(d^2x/du^2)]/(dx/du)^2 + *(1/(dx/du)) + etc. + \endverbatim + + Computational Costs + + This obviously only functions when the Bezier curve in question has a + finite derivative. Additionally, higher order derivatives are more + numerically expensive to evaluate than lower order derivatives. For + example, here are the number of operations required to compute the + following derivatives + \verbatim + Name : flops + dy/dx : ~102 + d2y/dx2 : ~194 + d3y/dx3 : ~321 + d4y/dx4 : ~426 + d5y/dx5 : ~564 + d6y/dx6 : ~739 + \endverbatim + + Example: + @code + RigidBodyDynamics::Math::VectorNd vX(6), vY(6); + + double u = 0.5; + + vX(0) = 1; + vX(1) = 1.01164; + vX(2) = 1.01164; + vX(3) = 1.02364; + vX(4) = 1.02364; + vY(5) = 1.04; + + vY(0) = 0; + vY(1) = 3e-16; + vY(2) = 3e-16; + vY(3) = 0.3; + vY(4) = 0.3; + vY(5) = 1; + + + d2ydx2 = SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX( + u,vX, vY, 2); + @endcode + + */ + static double calcQuinticBezierCurveDerivDYDX( + double u, + const RigidBodyDynamics::Math::VectorNd& xpts, + const RigidBodyDynamics::Math::VectorNd& ypts, + int order); + + + /** + Calculates the location of quintic Bezier curve control points to + create a C shaped curve like that shown in the figure. Using a series + of these simple and predictably shaped Bezier curves it is easy to build + quite complex curves. + + \image html fig_GeometryAddon_quinticCornerSections.png + + + @param x0 First intercept x location + @param y0 First intercept y location + @param dydx0 First intercept slope + @param x1 Second intercept x location + @param y1 Second intercept y location + @param dydx1 Second intercept slope + @param curviness A parameter that ranges between 0 and 1 to denote a + straight line or a curve + + \b aborts \b + -If the curviness parameter is less than 0, or greater than 1; + -If the points and slopes are chosen so that an "S" shaped curve would + be produced. This is tested by examining the points (x0,y0) and + (x1,y1) together with the intersection (xC,yC) of the lines beginning + at these points with slopes of dydx0 and dydx1 form a triangle. If the + line segment from (x0,y0) to (x1,y1) is not the longest line segment, + an exception is thrown. This is an overly conservative test as it + prevents very deep 'V' shapes from being respresented. + + @return a RigidBodyDynamics::Math::MatrixNd of 6 points Matrix(6,2) that + correspond to the X, and Y control points for a quintic Bezier curve that + has the above properties + + + Calculates the location of quintic Bezier curve control points to + create a C shaped curve that intersects points 0 (x0, y0) and point 1 + (x1, y1) with slopes dydx0 and dydx1 respectively, and a second + derivative of 0. The curve that results can approximate a line + (curviness = 0), or in a smooth C shaped curve (curviniess = 1) + + The current implementation of this function is not optimized in anyway + and has the following costs: + + Computational Costs + \verbatim + ~55 flops + \endverbatim + + + Example: + @code + double x0 = 1; + double y0 = 0; + double dydx0 = 0; + double x1 = 1.04; + double y1 = 1; + double dydx1 = 43; + double c = 0.75; + + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0, y0, dydx0,x1,y1,dydx01, + c); + @endcode + + */ + static RigidBodyDynamics::Math::MatrixNd + calcQuinticBezierCornerControlPoints( double x0, double y0, + double dydx0, + double x1, double y1, + double dydx1, + double curviness); + + /* + This function numerically integrates the Bezier curve y(x). + + @param vX Values of x to evaluate the integral of y(x) + @param ic0 The initial value of the integral + @param intAcc Accuracy of the integrated solution + @param uTol Tolerance on the calculation of the intermediate u term + @param uMaxIter Maximum number of iterations allowed for u to reach its + desired tolerance. + @param mX The 6xn matrix of Bezier control points for x(u) + @param mY The 6xn matrix of Bezier control points for y(u) + + @param flag_intLeftToRight Setting this flag to true will evaluate the + integral from the left most point to the right most + point. Setting this flag to false will cause the + integral to be evaluated from right to left. + @param name Name of caller. + @return RigidBodyDynamics::Math::MatrixNd Col 0: X vector, Col 1: int(y(x)) + + + This function numerically integrates the Bezier curve y(x), when really + both x and y are specified in terms of u. Evaluate the integral at the + locations specified in vX and return the result. + + Computational Costs + + This the expense of this function depends on the number of points in + vX, the points for which the integral y(x) must be calculated. The + integral is evaluated using a Runge Kutta 45 integrator, and so each + point requires 6 function evaluations. + (http://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method) + + The cost of evaluating 1 Bezier curve y(x) scales with the number + of points in xVal: + \verbatim + ~1740 flops per point + \endverbatim + + The example below is quite involved, but just so it can show you an + example of how to create the array of Spline objects that approximate + the function u(x). Although the example has been created for only 1 + Bezier curve set, simply changing the size and entries of the matricies + _mX and _mY will allow multiple sets to be integrated. + + + Example: + @code + //Integrator and u tolerance settings + double INTTOL = 1e-12; + double UTOL = 1e-14; + int MAXITER= 10; + + //Make up a Bezier curve - these happen to be the control points + //for a tendon curve + RigidBodyDynamics::Math::MatrixNd _mX(6,1), _mY(6,1); + _mX(0)= 1; + _mX(1)= 1.01164; + _mX(2)= 1.01164; + _mX(3)= 1.02364; + _mX(4)= 1.02364; + _mX(5)= 1.04; + + _mY(0) = 0; + _mY(1) = 3.10862e-16; + _mY(2) = 3.10862e-16; + _mY(3) = 0.3; + _mY(4) = 0.3; + _mY(5) = 1; + + _numBezierSections = 1; + bool _intx0x1 = true; //Says we're integrating from x0 to x1 + ////////////////////////////////////////////////// + //Generate the set of splines that approximate u(x) + ////////////////////////////////////////////////// + RigidBodyDynamics::Math::VectorNd u(NUM_SAMPLE_PTS); + //Used for the approximate inverse + RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); + //Used for the approximate inverse + + //Used to generate the set of knot points of the integral of y(x) + RigidBodyDynamics::Math::VectorNd + xALL(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); + _arraySplineUX.resize(_numBezierSections); + int xidx = 0; + + for(int s=0; s < _numBezierSections; s++){ + //Sample the local set for u and x + for(int i=0;i 1){ + //Skip the last point of a set that has another set of points + //after it. Why? The last point and the starting point of the + //next set are identical in value. + if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){ + xALL(xidx) = x(i); + xidx++; + } + }else{ + xALL(xidx) = x(i); + xidx++; + } + } + //Create the array of approximate inverses for u(x) + _arraySplineUX[s] = SimTK::SplineFitter:: + fitForSmoothingParameter(3,x,u,0).getSpline(); + } + + + ////////////////////////////////////////////////// + //Compute the integral of y(x) and spline the result + ////////////////////////////////////////////////// + + RigidBodyDynamics::Math::VectorNd yInt = SegmentedQuinticBezierToolkit:: + calcNumIntBezierYfcnX(xALL,0,INTTOL, UTOL, MAXITER,_mX, _mY, + _arraySplineUX,_name); + + if(_intx0x1==false){ + yInt = yInt*-1; + yInt = yInt - yInt(yInt.nelt()-1); + } + + _splineYintX = SimTK::SplineFitter:: + fitForSmoothingParameter(3,xALL,yInt,0).getSpline(); + + @endcode + + + */ + +//MM: Can't port this over to RBDL as RBDL doesn't have an error +// controlled integrator. I could add this if a dependency +// like Boost was allowed. +// +// static RigidBodyDynamics::Math::MatrixNd +// calcNumIntBezierYfcnX( +// const RigidBodyDynamics::Math::VectorNd& vX, +// double ic0, +// double intAcc, +// double uTol, +// int uMaxIter, +// const RigidBodyDynamics::Math::MatrixNd& mX, +// const RigidBodyDynamics::Math::MatrixNd& mY, +// const SimTK::Array_& aSplineUX, +// bool flag_intLeftToRight,const std::string& name); + + + private: + + + /** + This function will print cvs file of the column vector col0 and the + matrix data. + + @param col0 A vector that must have the same number of rows as the + data matrix. This column vector is printed as the first + column + @param data A matrix of data + @param filename The name of the file to print + */ + static void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + std::string& filename); + + /** + @param curveFit a function that evaluates a curve + @param ctrlPts control point locations for the fitted Bezier curve + @param xVal the x values at the control points + @param yVal the y values at the control points + @param filename of the output file. + + */ + static void printBezierSplineFitCurves( + const Function_& curveFit, + RigidBodyDynamics::Math::MatrixNd& ctrlPts, + RigidBodyDynamics::Math::VectorNd& xVal, + RigidBodyDynamics::Math::VectorNd& yVal, + std::string& filename); + + /** + This function will return a value that is equal to u, except when u is + outside of[0,1], then it is clamped to be 0, or 1 + @param u The parameter to be clamped + @retval u but restricted to 0,1. + */ + static double clampU(double u); + + +///@cond +/** +Class that contains data that describes the Bezier curve set. This class is used +by the function calcNumIntBezierYfcnX, which evaluates the numerical integral +of a Bezier curve set. +*/ +class BezierData { + public: + /**A 6xn matrix of Bezier control points for the X axis (domain)*/ + RigidBodyDynamics::Math::MatrixNd _mX; + /**A 6xn matrix of Bezier control points for the Y axis (range)*/ + RigidBodyDynamics::Math::MatrixNd _mY; + /**An n element array containing the approximate spline fits of the + inverse function of x(u), namely u(x)*/ + //std::vector< std::vector > _aArraySplineUX; + /**The initial value of the integral*/ + double _initalValue; + /**The tolerance to use when computing u. Solving u(x) can only be done + numerically at the moment, first by getting a good guess (using the + splines) and then using Newton's method to polish the value up. This + is the tolerance that is used in the polishing stage*/ + double _uTol; + /**The maximum number of interations allowed when evaluating u(x) using + Newton's method. In practice the guesses are usually very close to the + actual solution, so only 1-3 iterations are required.*/ + int _uMaxIter; + /**If this flag is true the function is integrated from its left most + control point to its right most. If this flag is false, the function + is integrated from its right most control point to its left most.*/ + //bool _flag_intLeftToRight; + /**The starting value*/ + //double _startValue; + + /**The name of the curve being intergrated. This is used to generate + useful error messages when something fails*/ + std::string _name; +}; +///@endcond + + +}; + +} +} +} + + +#endif diff --git a/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc b/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc new file mode 100644 index 0000000..1312973 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc @@ -0,0 +1,901 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunction.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + /* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= +#include "SmoothSegmentedFunction.h" +#include +#include + +//============================================================================= +// STATICS +//============================================================================= +//using namespace SimTK; +//using namespace OpenSim; +using namespace std; +using namespace RigidBodyDynamics::Addons::Geometry; + +static bool DEBUG = false; +static double UTOL = std::numeric_limits::epsilon()*1e6; +static double INTTOL = std::numeric_limits::epsilon()*1e2; +static double SQRTEPS = std::sqrt(numeric_limits::epsilon()); +static int MAXITER = 20; +static int NUM_SAMPLE_PTS = 100; +//============================================================================= +// UTILITY FUNCTIONS +//============================================================================= +/* + DETAILED COMPUTATIONAL COSTS: + ========================================================================= + WITHOUT INTEGRAL + _________________________________________________________________________ + Function Comp Div Mult Add Assignments + _________________________________________________________________________ + member assign M:2, 9 + curve gen: m,m*100 m m*100 m m*100*(4) + +m*100(3) +m*100*(3) + + Function detail + Evaluations Function + m SimTK::SplineFitter:: + fitForSmoothingParameter(3,x,u,0).getSpline(); + Cost: ? + + m*100 SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal + Cost: Mult Add Assignments + 21 20 13 + + Total ~typically > 2100*m multiplications, additions, + > 1000*m assignments + > 100*m divisions + _________________________________________________________________________ + Comp Div Mult Add Assignments + Total: m+m*100(3) m*100 m*100*21 m*100*20 m*100*13 + +m+m*100*3 +m*100*4+9+M:2 + + m*Cost(SimTK::SplineFitter ...) + ========================================================================= + ADDITIONAL COST OF COMPUTING THE INTEGRAL CURVE + + Comp Div Mult Add Assign + RK45 Fn.Eval m*100*(156 12 618 390 456) + RK45 Overhead m*100*(? ? ? ? ? ) + Spline cost m*100*(? ? ? ? ? ) + + Total: ~typically > 100,000's mult, additions, assignments + > 40,000 comparisions + > 3000 divisions + + ========================================================================= + M: Matrix + V: Vector + + N.B. These costs are dependent on SegmentedQuinticBezierToolkit +*/ +SmoothSegmentedFunction:: + SmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name): + _x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1), + _name(name) +{ + + + _numBezierSections = mX.cols(); + _mXVec.resize(_numBezierSections); + _mYVec.resize(_numBezierSections); + for(int s=0; s < _numBezierSections; s++){ + _mXVec[s] = mX.col(s); + _mYVec[s] = mY.col(s); + } +} + +//============================================================================== + SmoothSegmentedFunction::SmoothSegmentedFunction(): + _x0(NAN),_x1(NAN), + _y0(NAN),_y1(NAN), + _dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET") + { + //_arraySplineUX.resize(0); + _mXVec.resize(0); + _mYVec.resize(0); + //_splineYintX = SimTK::Spline(); + _numBezierSections = (int)NAN; + } + +//============================================================================== +void SmoothSegmentedFunction:: + updSmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name) +{ + + if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ){ + cerr<<"SmoothSegmentedFunction::updSmoothSegmentedFunction " + << _name.c_str() + <<": matrices mX and mY must have 6 rows, and the same" + <<" number of columns." + << endl; + assert(0); + abort(); + } + + _x0 =x0; + _x1 =x1; + _y0 =y0; + _y1 =y1; + _dydx0=dydx0; + _dydx1=dydx1; + + if(mX.cols() != _mXVec.size()){ + _mXVec.resize(mX.cols()); + _mYVec.resize(mY.cols()); + } + + _numBezierSections = mX.cols(); + for(int s=0; s < mX.cols(); s++){ + _mXVec[s] = mX.col(s); + _mYVec[s] = mY.col(s); + } + + _name = name; +} +//============================================================================== +void SmoothSegmentedFunction::shift(double xShift, double yShift) +{ + _x0 += xShift; + _x1 += xShift; + _y0 += yShift; + _y1 += yShift; + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.at(i).rows();++j){ + _mXVec.at(i)[j] += xShift; + _mYVec.at(i)[j] += yShift; + } + } + +} + +void SmoothSegmentedFunction::scale(double xScale, double yScale) +{ + + if( abs( xScale ) <= SQRTEPS){ + cerr<<"SmoothSegmentedFunction::scale " + << _name.c_str() + <<": xScale must be greater than sqrt(eps). Setting xScale to such" + <<" a small value will cause the slope of the curve to approach " + <<" infinity, or become undefined." + << endl; + assert(0); + abort(); + } + + _x0 *= xScale; + _x1 *= xScale; + _y0 *= yScale; + _y1 *= yScale; + _dydx0 *= yScale/xScale; + _dydx1 *= yScale/xScale; + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.at(i).rows();++j){ + _mXVec.at(i)[j] *= xScale; + _mYVec.at(i)[j] *= yScale; + } + } + +} + + +//============================================================================== + + + /*Detailed Computational Costs + ________________________________________________________________________ + If x is in the Bezier Curve + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + SegmentedQuinticBezierToolkit:: + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 + calcQuinticBezierCurveVal 21 20 13 + total 15+3*m+2 2 103 62+1*m 76 + + *Approximate. Uses iteration +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + 1 1 2 1 +________________________________________________________________________ + + */ + +double SmoothSegmentedFunction::calcValue(double x) const +{ + double yVal = 0; + if(x >= _x0 && x <= _x1 ) + { + int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); + double u = SegmentedQuinticBezierToolkit:: + calcU(x,_mXVec[idx], UTOL, MAXITER); + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,_mYVec[idx]); + }else{ + if(x < _x0){ + yVal = _y0 + _dydx0*(x-_x0); + }else{ + yVal = _y1 + _dydx1*(x-_x1); + } + } + + return yVal; +} + + +double SmoothSegmentedFunction::calcInverseValue(double y, + double xGuess) const +{ + + double xVal = 0; + + int idx = -1; + double yLeft = 0.; + double yRight = 0; + double xLeft = 0.; + double xRight = 0; + double xDist = 0; + double xDistBest = numeric_limits::infinity(); + + for(unsigned int i=0; i < _numBezierSections; ++i){ + + yLeft = y - _mYVec[i][0]; + yRight= _mYVec[i][5] - y; + + xLeft = xGuess - _mXVec[i][0]; + xRight= _mXVec[i][5] - xGuess; + xDist = fabs(xLeft)+fabs(xRight); + + //If the y value is in the spline interval and the + //x interval is closer to the guess, update the interval + if(yLeft*yRight >= 0 && xDist < xDistBest){ + idx = i; + xDistBest = xDist; + } + + } + + //y value is in the linear region + if(idx == -1){ + if( (y-_y1)*_dydx1 >= 0 + && fabs(_dydx1) > numeric_limits< double >::epsilon() ) { + xVal = (y-_y1)/_dydx1 + _x1; + }else if( (_y0-y)*_dydx0 >= 0 + && fabs(_dydx0) > numeric_limits< double >::epsilon() ){ + xVal = (y-_y0)/_dydx0 + _x0; + }else{ + xVal = numeric_limits::signaling_NaN(); + } + + }else{ + //y is in an interval + double u = SegmentedQuinticBezierToolkit:: + calcU(y,_mYVec[idx], UTOL, MAXITER); + xVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveVal(u,_mXVec[idx]); + } + + return xVal; + +} + +double SmoothSegmentedFunction::calcValue( + const RigidBodyDynamics::Math::VectorNd& ax) const +{ + + if( !(ax.size() == 1) ){ + cerr<<"SmoothSegmentedFunction::calcValue " << _name.c_str() + <<": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but a function with " + << ax.size() << " elements was entered" + << endl; + assert(0); + abort(); + + } + + return calcValue(ax[0]); +} + +/*Detailed Computational Costs +________________________________________________________________________ +If x is in the Bezier Curve, and dy/dx is being evaluated + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ +Overhead: + SegmentedQuinticBezierToolkit:: + calcIndex 3*m+2 1*m 3 + *calcU 15 2 82 42 60 + Derivative Evaluation: + **calcQuinticBezierCurveDYDX 21 20 13 + dy/du 20 19 11 + dx/du 20 19 11 + dy/dx 1 + + total 17+3*m 3 143 m+100 98 + +*Approximate. Uses iteration +**Higher order derivatives cost more +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + 1 1 +________________________________________________________________________ + */ + +double SmoothSegmentedFunction::calcDerivative(double x, int order) const +{ + //return calcDerivative( SimTK::Array_(order,0), + // RigidBodyDynamics::Math::VectorNd(1,x)); + double yVal = 0; + + //QUINTIC SPLINE + + + if(order==0){ + yVal = calcValue(x); + }else{ + if(x >= _x0 && x <= _x1){ + int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec); + double u = SegmentedQuinticBezierToolkit:: + calcU(x,_mXVec[idx],UTOL,MAXITER); + yVal = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx], + _mYVec[idx], order); +/* + std::cout << _mX(3, idx) << std::endl; + std::cout << _mX(idx) << std::endl;*/ + }else{ + if(order == 1){ + if(x < _x0){ + yVal = _dydx0; + }else{ + yVal = _dydx1;} + }else{ + yVal = 0;} + } + } + + return yVal; +} + + + +double SmoothSegmentedFunction:: + calcDerivative( const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& ax) const +{ + /* + for(int i=0; i < (signed)derivComponents.size(); i++){ + SimTK_ERRCHK2_ALWAYS( derivComponents[i] == 0, + "SmoothSegmentedFunction::calcDerivative", + "%s: derivComponents can only be populated with 0's because " + "SmoothSegmentedFunction is only valid for a 1D function, but " + "derivComponents had a value of %i in it", + _name.c_str(), derivComponents[i]); + } + SimTK_ERRCHK2_ALWAYS( derivComponents.size() <= 6, + "SmoothSegmentedFunction::calcDerivative", + "%s: calcDerivative is only valid up to a 6th order derivative" + " but derivComponents had a size of %i", + _name.c_str(), derivComponents.size()); + + SimTK_ERRCHK2_ALWAYS( ax.size() == 1, + "SmoothSegmentedFunction::calcValue", + "%s: Argument x must have only 1 element, as this function is " + "designed only for 1D functions, but ax had a size of %i", + _name.c_str(), ax.size()); + */ + + for(int i=0; i < (signed)derivComponents.size(); i++){ + if( !(derivComponents[i] == 0)){ + cerr << "SmoothSegmentedFunction::calcDerivative " + << _name.c_str() + <<": derivComponents can only be populated with 0's because " + << "SmoothSegmentedFunction is only valid for a 1D function," + << " but derivComponents had a value of " + << derivComponents[i] << " in it" + << endl; + assert(0); + abort(); + + } + } + + if( !(derivComponents.size() <= 6)){ + cerr << "SmoothSegmentedFunction::calcDerivative " << _name.c_str() + << ": calcDerivative is only valid up to a 6th order derivative" + << " but derivComponents had a size of " + << derivComponents.size() + << endl; + assert(0); + abort(); + } + + if( !(ax.size() == 1) ){ + cerr << "SmoothSegmentedFunction::calcValue " << _name.c_str() + << ": Argument x must have only 1 element, as this function is " + << "designed only for 1D functions, but ax had a size of " + << ax.size() + << endl; + assert(0); + abort(); + } + + + return calcDerivative(ax[0], derivComponents.size()); +} + +/*Detailed Computational Costs +________________________________________________________________________ +If x is in the Bezier Curve, and dy/dx is being evaluated + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + *spline.calcValue 7 2 3 1 + +*Approximate cost of evaluating a cubic spline with 100 knots, where +the bisection method is used to find the correct index +________________________________________________________________________ +If x is in the linear region + + Name Comp. Div. Mult. Add. Assign. + *spline.calcValue 1 2 3 1 + integral eval 2 4 5 1 + total 3 6 8 2 + +*Approximate cost of evaluating a cubic spline at its last knot point +________________________________________________________________________ + +*/ +/* +double SmoothSegmentedFunction::calcIntegral(double x) const +{ + SimTK_ERRCHK1_ALWAYS(_computeIntegral, + "SmoothSegmentedFunction::calcIntegral", + "%s: This curve was not constructed with its integral because" + "computeIntegral was false",_name.c_str()); + + double yVal = 0; + if(x >= _x0 && x <= _x1){ + yVal = _splineYintX.calcValue(RigidBodyDynamics::Math::VectorNd(1,x)); + }else{ + //LINEAR EXTRAPOLATION + if(x < _x0){ + RigidBodyDynamics::Math::VectorNd tmp(1); + tmp(0) = _x0; + double ic = _splineYintX.calcValue(tmp); + if(_intx0x1){//Integrating left to right + yVal = _y0*(x-_x0) + + _dydx0*(x-_x0)*(x-_x0)*0.5 + + ic; + }else{//Integrating right to left + yVal = -_y0*(x-_x0) + - _dydx0*(x-_x0)*(x-_x0)*0.5 + + ic; + } + }else{ + RigidBodyDynamics::Math::VectorNd tmp(1); + tmp(0) = _x1; + double ic = _splineYintX.calcValue(tmp); + if(_intx0x1){ + yVal = _y1*(x-_x1) + + _dydx1*(x-_x1)*(x-_x1)*0.5 + + ic; + }else{ + yVal = -_y1*(x-_x1) + - _dydx1*(x-_x1)*(x-_x1)*0.5 + + ic; + } + } + } + + return yVal; +} +*/ +/* +bool SmoothSegmentedFunction::isIntegralAvailable() const +{ + return _computeIntegral; +} +*/ + +/* +bool SmoothSegmentedFunction::isIntegralComputedLeftToRight() const +{ + return _intx0x1; +} +*/ + +int SmoothSegmentedFunction::getArgumentSize() const +{ + return 1; +} + +int SmoothSegmentedFunction::getMaxDerivativeOrder() const +{ + return 6; +} + +std::string SmoothSegmentedFunction::getName() const +{ + return _name; +} + +void SmoothSegmentedFunction::setName(const std::string &name) +{ + _name = name; +} + +RigidBodyDynamics::Math::VectorNd + SmoothSegmentedFunction::getCurveDomain() const +{ + RigidBodyDynamics::Math::VectorNd domain(2); + + domain[0] = 0; + domain[1] = 0; + if (!_mXVec.empty()) { + domain[0] = _mXVec[0][0]; + domain[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1]; + } + return domain; +} + +/////////////////////////////////////////////////////////////////////////////// +// Utility functions +/////////////////////////////////////////////////////////////////////////////// + +/*Detailed Computational Costs + +_______________________________________________________________________ + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + + *overhead (17+3*m 2 82 42+m 63)*7 + 119+21*m 14 574 294+7m 441 + + calcValue 21 20 13 + calcDerivative: dy/dx 1 40 38 22 + : d2y/dx2 2 78 73 23 + : d3y/dx3 4 118 105 58 + : d4y/dx4 5 168 137 71 + : d5y/dx5 7 236 170 88 + : d6y/dx6 9 334 209 106 + + **calcIntegral 7 2 3 1 + + total per point 126+21*m 42 1571 1049 823 + total per elbow 126k+21k*m 42k 1571k 1049k 823k + + *Approximate. Overhead associated with finding the correct Bezier + spline section, and evaluating u(x). + Assumes 2 Newton iterations in calcU + + **Approximate. Includes estimated cost of evaluating a cubic spline + with 100 knots +*/ +RigidBodyDynamics::Math::MatrixNd + SmoothSegmentedFunction::calcSampledCurve(int maxOrder, + double domainMin, + double domainMax) const{ + + int pts = 1; //Number of points between each of the spline points used + //to fit u(x), and also the integral spline + if( !(maxOrder <= getMaxDerivativeOrder()) ){ + cerr << "SmoothSegmentedFunction::calcSampledCurve " + << "Derivative order past the maximum computed order requested" + << endl; + assert(0); + abort(); + } + + double x0,x1,delta; + //y,dy,d1y,d2y,d3y,d4y,d5y,d6y,iy + RigidBodyDynamics::Math::VectorNd + midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1)); + RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS); + + //Generate a sample of X values inside of the curve that is denser where + //the curve is more curvy. + double u; + int idx = 0; + for(int s=0; s < _numBezierSections; s++){ + //Sample the local set for u and x + for(int i=0;i 1){ + //Skip the last point of a set that has another set of points + //after it. Why? The last point and the starting point of the + //next set are identical in value. + if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){ + midX[idx] = x[i]; + idx++; + } + }else{ + midX[idx] = x[i]; + idx++; + } + } + } + + + RigidBodyDynamics::Math::VectorNd xsmpl(pts*(midX.size()-1)+2*10*pts); + + RigidBodyDynamics::Math::MatrixNd results; + + /* + if(_computeIntegral){ + results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2+1); + }else{ + + } + */ + results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2); + + //Array initialization is so ugly ... + std::vector d1y(1),d2y(2),d3y(3),d4y(4),d5y(5),d6y(6); + d1y[0]=0; + d2y[0] = 0; + d2y[1] = 0; + for(int i=0;i<3;i++) + d3y[i]=0; + for(int i=0;i<4;i++) + d4y[i]=0; + for(int i=0;i<5;i++) + d5y[i]=0; + for(int i=0;i<6;i++) + d6y[i]=0; + + //generate some sample points in the extrapolated region + idx = 0; + x0 = _x0 - 0.1*(_x1-_x0); + if(domainMin < x0) + x0 = domainMin; + + x1 = _x0; + delta = (0.1)*(x1-x0)/(pts); + + for(int j=0; j x1) + x1 = domainMax; + + delta = (1.0/9.0)*(x1-x0)/(pts); + + for(int j=0;j=1) + results(i,2) = calcDerivative(d1y,ax); + + if(maxOrder>=2) + results(i,3) = calcDerivative(d2y,ax); + + if(maxOrder>=3) + results(i,4) = calcDerivative(d3y,ax); + + if(maxOrder>=4) + results(i,5) = calcDerivative(d4y,ax); + + if(maxOrder>=5) + results(i,6) = calcDerivative(d5y,ax); + + if(maxOrder>=6) + results(i,7) = calcDerivative(d6y,ax); + + /* + if(_computeIntegral){ + results(i,maxOrder+2) = calcIntegral(ax(0)); + } + */ + } + return results; +} + +void SmoothSegmentedFunction::getXControlPoints( + RigidBodyDynamics::Math::MatrixNd& mat) const +{ + mat.resize(_mXVec.size(), _mXVec.at(0).size()); + + for(int i=0; i<_mXVec.size();++i){ + for(int j=0; j<_mXVec.size();++j){ + mat(i,j) = _mXVec.at(i)[j]; + } + } + +} +void SmoothSegmentedFunction::getYControlPoints( + RigidBodyDynamics::Math::MatrixNd& mat) const +{ +mat.resize(_mYVec.size(), _mYVec.at(0).size()); + +for(int i=0; i<_mYVec.size();++i){ + for(int j=0; j<_mYVec.size();++j){ + mat(i,j) = _mYVec.at(i)[j]; + } +} + +} + + +/*Detailed Computational Costs + +_______________________________________________________________________ + Name Comp. Div. Mult. Add. Assign. +_______________________________________________________________________ + + *overhead (17+3*m 2 82 42+m 63)*3 + 51+9m 6 246 126+3m 189 + + calcValue 21 20 13 + calcDerivative : dy/dx 1 40 38 22 + : d2y/dx2 2 78 73 23 + + **calcIntegral 7 2 3 1 + + total per point 58+9m 9 387 260+3m 248 + total per elbow 5.8k+900m 900 38.7k 26k+300m 24.8k + + *Approximate. Overhead associated with finding the correct Bezier + spline section, and evaluating u(x). + Assumes 2 Newton iterations in calcU + + **Approximate. Includes estimated cost of evaluating a cubic spline + with 100 knots +*/ +void SmoothSegmentedFunction::printCurveToCSVFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + double domainMin, + double domainMax) const +{ + //Only compute up to the 2nd derivative + RigidBodyDynamics::Math::MatrixNd results = + calcSampledCurve(2,domainMin,domainMax); + std::vector colNames(results.cols()); + colNames[0] = "x"; + colNames[1] = "y"; + colNames[2] = "dy/dx"; + colNames[3] = "d2y/dx2"; + /* + if(results.cols() == 5){ + colNames[4] = "int_y(x)"; + } + */ + std::string fname = fileNameWithoutExtension; + fname.append(".csv"); + printMatrixToFile(results,colNames,path,fname); +} +/* +This function will print cvs file of the column vector col0 and the matrix data +*/ +void SmoothSegmentedFunction:: + printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, + std::vector& colNames, + const std::string& path, + const std::string& filename) const +{ + + ofstream datafile; + std::string fullpath = path; + + if(fullpath.length() > 0) + fullpath.append("/"); + + fullpath.append(filename); + + datafile.open(fullpath.c_str(),std::ios::out); + + if(!datafile){ + datafile.close(); + cerr << "SmoothSegmentedFunction::printMatrixToFile " + << _name.c_str() << ": Failed to open the file path: " + << fullpath.c_str() + << endl; + assert(0); + abort(); + } + + + for(int i = 0; i < (signed)colNames.size(); i++){ + if(i < (signed)colNames.size()-1) + datafile << colNames[i] << ","; + else + datafile << colNames[i] << "\n"; + } + + for(int i = 0; i < data.rows(); i++){ + for(int j = 0; j < data.cols(); j++){ + if(j +#include + + + + /** + This class contains the quintic Bezier curves, x(u) and y(u), that have been + created by SmoothSegmentedFunctionFactory to follow a physiologically + meaningful muscle characteristic. A SmoothSegmentedFunction cannot be + created directly,you must use SmoothSegmentedFunctionFactory to create the + muscle curve of interest. + + Computational Cost Details + All computational costs assume the following operation costs: + + \verbatim + Operation Type : #flops + +,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 + \endverbatim + + These relative weightings will vary processor to processor, and so any + of the quoted computational costs are approximate. + + + RBDL Port Notes +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + + + @author Matt Millard + @version 0.0 + + + */ + +namespace RigidBodyDynamics { + namespace Addons { + namespace Geometry{ + + class SmoothSegmentedFunction : public Function_ + { + + + public: + + ///The default constructor, which populates the member data fields with + ///NaN's + SmoothSegmentedFunction(); + + //SmoothSegmentedFunction(); + /** + Creates a set of quintic Bezier curves. + + @param mX The matrix of quintic Bezier x point locations (6xn). + Each column vector is the 6 control points required + for each quintic Bezier curve. For C0 continuity + adjacent columns must share the last and first control + points. For C1 continuity the last 2 and first two + control points of adjacent curves should be on the same + curve. + + @param mY The matrix of quintic Bezier y point locations (6xn). + + @param x0 The minimum x value. This is used for the linear + extrapolation of the Bezier curve. This parameter is + explicitly asked for, rather than computed, to prevent + rounding error from reducing the accuracy of the + linear extrapolation. + @param x1 The maximum x value. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y0 The value of y(x) at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y1 The value of y(x) at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx0 The value of dy/dx at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx1 The value of dy/dx at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + + Computational Costs + Generating the integral curve is not cheap, and so should only be used + when if it will be evaluated during a simulation. + \verbatim + Computatonal Cost Per Bezier Section: + Without Integral : 4,100 flops + \endverbatim + + */ + SmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name); + + + /** + Updates the Bezier curve conrol points. The updated curve must have + the same number of control points as the original curve. + + @param mX The matrix of quintic Bezier x point locations (6xn). + Each column vector is the 6 control points required + for each quintic Bezier curve. For C0 continuity + adjacent columns must share the last and first control + points. For C1 continuity the last 2 and first two + control points of adjacent curves should be on the same + curve. + + @param mY The matrix of quintic Bezier y point locations (6xn). + + @param x0 The minimum x value. This is used for the linear + extrapolation of the Bezier curve. This parameter is + explicitly asked for, rather than computed, to prevent + rounding error from reducing the accuracy of the + linear extrapolation. + @param x1 The maximum x value. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y0 The value of y(x) at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param y1 The value of y(x) at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx0 The value of dy/dx at x=x0. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param dydx1 The value of dy/dx at x=x1. This is used for the linear + extrapolation and is required for the same reasons + as x0. + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + + */ + void updSmoothSegmentedFunction( + const RigidBodyDynamics::Math::MatrixNd& mX, + const RigidBodyDynamics::Math::MatrixNd& mY, + double x0, double x1, + double y0, double y1, + double dydx0, double dydx1, + const std::string& name); + + /** + This function will shift the entire SmoothSegmentedFunction by xShift + and yShift. Setting xShift = yShift = 0.0 will leave the curve unchanged. + + @param xShift - the amount to shift the curve in the x-direction. + @param yShift - the amount to shift the curve in the y-direction + */ + void shift(double xShift, double yShift); + + /** + This function will scale the curve in the x and y directions. Setting + xScale=yScale=1.0 will leave the curve unchanged. + + \b aborts \b + -If abs(xScale) < sqrt(eps) + + @param xScale: the amount to scale the curve in the x direction + @param yScale: the amount to scale the curve in the y direction + */ + void scale(double xScale, double yScale); + + + + /**Calculates the value of the curve this object represents. + + @param x The domain point of interest + + \b aborts \b + -If ax does not have a size of 1 + @returns The value of the curve + + + The curve is parameterized as a set of Bezier curves. If x is within the + domain of these Bezier curves they will be evaluated. If x is outside + of the domain of these Bezier curves a linear extrapolation will be + evalulated + + + Computational Costs + \verbatim + x in curve domain : ~282 flops + x in linear section: ~5 flops + \endverbatim + */ + double calcValue(double x) const; + + /** + This function solves for the x value of the curve given its y value. + Since a SmoothSegmentedFunction is not necessarily monotonic lower + and an upper bound on the argument must be specified so that the + desired root is found. + + @param y: The range value of interest + @param xGuess: guess for the value of x + @return x: the value of x which corresponds to y = f(x) between + xLowerBound, and xUpperBound + */ + double calcInverseValue( double y, + double xGuess) const; + + /**Calculates the value of the derivative of the curve this object + represents. + + @param x The domain point of interest. + + @param order The order of the derivative to compute. Note that order must + be between 0 and 2. Calling 0 just calls calcValue. + + \b aborts \b + -If anything but 0's are stored in derivComponents + -If more than the 6th derivative is asked for + -If ax has a size other than 1 + + @return The value of the d^ny/dx^n th derivative evaluated at x + + + + Computational Costs + \verbatim + x in curve domain : ~391 flops + x in linear section: ~2 flops + \endverbatim + + */ + double calcDerivative(double x, int order) const; + + + + + /*This will return the value of the integral of this objects curve + evaluated at x. + + @param x the domain point of interest + + \b aborts \b + -If the function does not have a pre-computed integral + @return the value of the functions integral evaluated at x + + The integral is approximate, though its errors are small. + The integral is computed by numerically integrating the function when + the constructor for this class is called (if computeIntegral is true) and + then splining the result, thus the regions between the knot points may + have some error in them. A very fine mesh of points is used to create the + spline so the errors will be small + + Computational Costs + \verbatim + x in curve domain : ~13 flops + x in linear section: ~19 flops + \endverbatim + + */ + //double calcIntegral(double x) const; + + /* + Returns a bool that indicates if the integral curve has been computed. + + @return true if the integral of this function is available, false if + it has not been computed. + */ + //bool isIntegralAvailable() const; + + /* + Returns a bool that indicates if the integral computed is compuated left + to right, or right to left. + + @return true if the integral was computed left to right, and false if the + integral was computed right to left. Note that the output of + this function is only valid if isIntegralAvailable() returns + true. + */ + //bool isIntegralComputedLeftToRight() const; + + /** + Returns a string that is the name for this curve, which is set at the + time of construction and cannot be changed after construction. + + @return The string name this object was given during construction*/ + std::string getName() const; + + /** + Sets the name of the SmoothSegmentedFunction object. + + @param name The name of the data this SmoothSegmentedFunction. This name + is used to make human-readable error messages and to + generate sensible names when printing the curve to file. + */ + void setName(const std::string &name); + + /** + This function returns a SimTK::Vec2 that contains in its 0th element + the lowest value of the curve domain, and in its 1st element the highest + value in the curve domain of the curve. Outside of this domain the curve + is approximated using linear extrapolation. + + @return The minimum and maximum value of the domain, x, of the curve + y(x). Within this range y(x) is a curve, outside of this range + the function y(x) is a C2 (continuous to the second + derivative) linear extrapolation*/ + RigidBodyDynamics::Math::VectorNd getCurveDomain() const; + + /**This function will generate a csv file (of 'name_curveName.csv', where + name is the one used in the constructor) of the muscle curve, and + 'curveName' corresponds to the function that was called from + SmoothSegmentedFunctionFactory to create the curve. + + @param path The full path to the location. Note '/' slashes must be used, + and do not put a '/' after the last folder. + @param fileNameWithoutExtension The name of the file to write, not + including the file extension + @param domainMin + the left most domain point of the curve to print. The curve + will extend to at least this point. + @param domainMax + the right most domain point of the curve to print. The + printed curve will extend at least to this point, perhaps + beyond. + \b aborts \b + -If the filename is empty + + For example the tendon + curve for a muscle named 'glutmax' will be: + + 'glutmax_tendonForceLengthCurve.csv' + + The file will contain the following columns: + + \verbatim + Col# 1, 2, 3, 4, 5 + x, y, dy/dx, d2y/dx2, iy + \endverbatim + + Where iy is the integral of y(x). If the curve has been set not to have + an integral, this column will not exist. + + The curve will be sampled from its linear extrapolation region, through + the curve, out to the other linear extrapolation region. The width of + each linear extrapolation region is 10% of the entire range of x, or + 0.1*(x1-x0). + + The number of rows used will vary from curve to curve. Each quintic + Bezier curve section will have 100 samples. Each linearily extrapolated + region will have 10 samples each. Some muscle curves (the tendon, + parallel elements, compressive elements) consist of only 1 elbow, and so + these matrices will have only 100+20 rows. The force velocity curve is + made up of 2 elbows and will have 200+20 rows. The active force length + curve has 5 elbows, and so its sampled matrix will have 500+20 rows + + Computational Costs + This varies depending on the curve (as mentioned above). + \verbatim + ~97,400 to 487,000 flops + \endverbatim + + Example + To read the csv file with a header in from Matlab, you need to use + csvread set so that it will ignore the header row. This is accomplished + by using the extra two numerical arguments for csvread to tell the + function to begin reading from the 1st row, and the 0th index (csvread + is 0 indexed). + \verbatim + data=csvread('test_tendonForceLengthCurve.csv',1,0); + \endverbatim + + */ + void printCurveToCSVFile(const std::string& path, + const std::string& fileNameWithoutExtension, + double domainMin, + double domainMax) const; + + + /** + @param maxOrder The maximum derivative order to compute + @param domainMin + @param domainMax + \b aborts \b + -If the requested derivatve order is greater than + getMaxDerivativeOrder() + @returns a matrix populated with x,y,dy/dx ... d^ny/dx^n,iy + + + This function will generate a RigidBodyDynamics::Math::MatrixNd populated + with samples of the muscle curves values, derivatives (up to 6) and its + first integral (if available). The matrix has the following columns: + + \verbatim + Col# 1, 2, 3, 4, 5, 6, 7, 8, 9, + x, y, dy/dx, d2y/dx2, d3y/dx3, d4y/dx4, d5y/dx5, d6y/dx6, iy + \endverbatim + + Where iy is the integral of y(x). If the curve has been set not to have + an integral, this column will not exist. + + The curve will be sampled from its + linear extrapolation region, through the curve, out to the other linear + extrapolation region. The width of each linear extrapolation region is + 10% of the entire range of x, or 0.1*(x1-x0). + + The rows used will vary from curve to curve. Each quintic Bezier curve + section will have 100 samples + 20 samples for the linear extrapolation + region. Some muscle curves (the tendon, parallel elements, compressive + elements) consist of only 1 elbow, and so these matrices will have only + 100+20 rows. The force velocity curve is made up of 2 elbows and will + have 200+20 rows. The active force length curve has 5 elbows, and so its + sampled matrix will have 500+20 rows + + */ + RigidBodyDynamics::Math::MatrixNd calcSampledCurve(int maxOrder, + double domainMin, + double domainMax) const; + + + void getXControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const; + void getYControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const; + + private: + + + /**Nov 2015: Not needed in the RBDL port. + Array of spline fit functions X(u) for each Bezier elbow*/ + //Nov 2015: Not needed in the RBDL port. + //SimTK::Array_ _arraySplineUX; + /**Nov 2015: Not included in the RBDL port (RBDL doesn't have an + error controlled integrator to generate this curve) + Spline fit of the integral of the curve y(x)*/ + //SimTK::Spline _splineYintX; + + /**Bezier X1,...,Xn control point locations. Control points are + stored in 6x1 vectors in the order above*/ + std::vector _mXVec; + /**Bezier Y1,...,Yn control point locations. Control points are + stored in 6x1 vectors in the order above*/ + std::vector _mYVec; + + /**The number of quintic Bezier curves that describe the relation*/ + int _numBezierSections; + + /**The minimum value of the domain*/ + double _x0; + /**The maximum value of the domain*/ + double _x1; + /**The minimum value of the range*/ + double _y0; + /**The maximum value of the range*/ + double _y1; + /**The slope at _x0*/ + double _dydx0; + /**The slope at _x1*/ + double _dydx1; + /*This is the users */ + //bool _computeIntegral; + + /*This variable, when true, indicates that the user wants the integral + from left to right (x0 to x1). If it is false, the integral from right + to left (x1 to x0) is computed*/ + //bool _intx0x1; + /**The name of the function**/ + std::string _name; + + /**No human should be constructing a SmoothSegmentedFunction, so the + constructor is made private so that mere mortals cannot look at it. + SmoothSegmentedFunctionFactory should be used to create + MuscleCurveFunctions and that's why its a friend*/ + friend class SmoothSegmentedFunctionFactory; + + + + + + /** + This function will print cvs file of the column vector col0 and the + matrix data + + @param data A matrix of data + @param colnames Array of column headings + @param path The desired path to the folder to write the file + @param filename The name of the file to print + \b aborts \b + -If the desired file cannot be created and openened, perhaps + because the path doesn't exist. + */ + void printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data, + std::vector& colnames, + const std::string& path, + const std::string& filename) const; + + /** + Refer to the documentation for calcValue(double x) + because this function is identical in function to + calcValue(double x), but requires different inputs. + This is a required virtual function required because this class extends + the Function interface. + */ + double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const; + /*virtual*/ + + /** Refer to the documentation for calcDerivative(double x, int order) + because this function is identical in function to + calcDerivative(double x, int order), but requires different inputs. + This is a required virtual function required because this class extends + the Function interface.*/ + double calcDerivative(const std::vector& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const; + /*virtual*/ + + /**This will return the size of the vector that the + calcValue(const RigidBodyDynamics::Math::VectorNd& x) require. This is a + required virtual function required because this class extends the + Function interface, though is only needed if you call + + double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const; + + or + + double calcDerivative( const SimTK::Array_& derivComponents, + const RigidBodyDynamics::Math::VectorNd& x) const; + + Since this class is implementing strictly scalar functions you can use + the simplified versions of calcValue(double x) and + calcDerivative(double x, int order) instead. + + */ + int getArgumentSize() const; /*virtual*/ + + /**@return The maximum order derivative that this object is capable of + returning*/ + /*virtual*/ + int getMaxDerivativeOrder() const; + + + }; + +} +} +} + + + +#endif diff --git a/3rdparty/rbdl/addons/geometry/geometry.h b/3rdparty/rbdl/addons/geometry/geometry.h new file mode 100644 index 0000000..702c2fb --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/geometry.h @@ -0,0 +1,16 @@ +/* + * + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + + +#ifndef GEOMETRY_H_ +#define GEOMETRY_H_ + +#include "Function.h" +#include "SegmentedQuinticBezierToolkit.h" +#include "SmoothSegmentedFunction.h" + +#endif diff --git a/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt b/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt new file mode 100644 index 0000000..a8e8df7 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/CMakeLists.txt @@ -0,0 +1,64 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) + +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_MAJOR 1 ) +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_GEOMETRY_TESTS_VERSION + ${RBDL_ADDON_GEOMETRY_TESTS_VERSION_MAJOR}.${RBDL_ADDON_GEOMETRY_TESTS_VERSION_MINOR}.${RBDL_ADDON_GEOMETRY_TESTS_VERSION_PATCH} +) + + +PROJECT (RBDL_GEOMETRY_TESTS VERSION ${RBDL_ADDON_GEOMETRY_TESTS_VERSION}) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) +INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) + +SET ( GEOMETRY_TESTS_SRCS + testSmoothSegmentedFunction.cc + numericalTestFunctions.cc + numericalTestFunctions.h + ../geometry.h + ../SegmentedQuinticBezierToolkit.h + ../SmoothSegmentedFunction.h + ../SegmentedQuinticBezierToolkit.cc + ../SmoothSegmentedFunction.cc + ) + +INCLUDE_DIRECTORIES ( ../ ) + + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +ADD_EXECUTABLE ( rbdl_geometry_tests ${GEOMETRY_TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_geometry_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runGeometryTests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +TARGET_LINK_LIBRARIES ( rbdl_geometry_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) +ADD_CUSTOM_COMMAND (TARGET rbdl_geometry_tests + POST_BUILD + COMMAND ./runGeometryTests + COMMENT "Running automated addon geometry tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc b/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc new file mode 100644 index 0000000..6afe038 --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/numericalTestFunctions.cc @@ -0,0 +1,534 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + + +#include "numericalTestFunctions.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + + +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename) +{ + + ofstream datafile; + datafile.open(filename.c_str()); + + for(int i = 0; i < data.rows(); i++){ + datafile << col0[i] << ","; + for(int j = 0; j < data.cols(); j++){ + if(j y_C3max){ + c3 = 1; + y = y_C3max; + } + + h = pow( ( (EPSILON*y*2.0)/(c3) ) , 1.0/3.0); + + //Now check that h to the left and right are at least similar + //If not, take the smallest one. + xL = x[i]-h/2; + xR = x[i]+h/2; + + fL = mcf.calcDerivative(xL, order-1); + fR = mcf.calcDerivative(xR, order-1); + + //Just for convenience checking ... + dyNUM = (fR-fL)/h; + dy = mcf.calcDerivative(x[i],order); + err = abs(dy-dyNUM); + + /*if(err > tol && abs(dy) > rootEPS && order <= 2){ + err = err/abs(dy); + if(err > tol) + cout << "rel tol exceeded" << endl; + }*/ + + dyV[i] = dyNUM; + + } + + + return dyV; +} + +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult) +{ + bool flag_continuous = true; + + double xL = 0; // left shoulder point + double xR = 0; // right shoulder point + double yL = 0; // left shoulder point function value + double yR = 0; // right shoulder point function value + double dydxL = 0; // left shoulder point derivative value + double dydxR = 0; // right shoulder point derivative value + + double xVal = 0; //x value to test + double yVal = 0; //Y(x) value to test + + double yValEL = 0; //Extrapolation to yVal from the left + double yValER = 0; //Extrapolation to yVal from the right + + double errL = 0; + double errR = 0; + + double errLMX = 0; + double errRMX = 0; + + + for(int i =1; i < xV.size()-1; i++){ + xVal = xV[i]; + yVal = yV.calcDerivative(xVal, order); + + xL = 0.5*(xV[i]+xV[i-1]); + xR = 0.5*(xV[i]+xV[i+1]); + + yL = yV.calcDerivative(xL,order); + yR = yV.calcDerivative(xR,order); + + dydxL = yV.calcDerivative(xL,order+1); + dydxR = yV.calcDerivative(xR,order+1); + + + yValEL = yL + dydxL*(xVal-xL); + yValER = yR - dydxR*(xR-xVal); + + errL = abs(yValEL-yVal); + errR = abs(yValER-yVal); + + errLMX = abs(yV.calcDerivative(xL,order+2)*0.5*(xVal-xL)*(xVal-xL)); + errRMX = abs(yV.calcDerivative(xR,order+2)*0.5*(xR-xVal)*(xR-xVal)); + + errLMX*=taylorErrorMult; + errRMX*=taylorErrorMult; + + if(errLMX < minValueSecondDerivative) + errLMX = minValueSecondDerivative; + + if(errRMX < minValueSecondDerivative) + errRMX = minValueSecondDerivative; // to accomodate numerical + //error in errL + + if(errL > errLMX || errR > errRMX){ + flag_continuous = false; + } + } + + return flag_continuous; +} + +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS) +{ + double dir = y[y.size()-1]-y[0]; + bool isMonotonic = true; + + if(dir < 0){ + for(int i =1; i y[i-1]+EPSILON*multEPS){ + isMonotonic = false; + //printf("Monotonicity broken at idx %i, since %fe-16 > %fe-16\n", + // i,y(i)*1e16,y(i-1)*1e16); + printf("Monotonicity broken at idx %i, since " + "y(i)-y(i-1) < tol, (%f*EPSILON < EPSILON*%i) \n", + i,((y[i]-y[i-1])/EPSILON), multEPS); + } + } + } + if(dir > 0){ + for(int i =1; i = 0; i=i-1){ + width = abs(x[i]-x[i+1]); + inty[i] = inty[i+1] + width*(0.5)*(y[i]+y[i+1]); + } + } + + + return inty; +} + + +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b) +{ + double error = 0; + double cerror=0; + for(int i = 0; i< a.size(); i++) + { + cerror = abs(a[i]-b[i]); + if(cerror > error){ + error = cerror; + } + } + return error; +} + + + +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol) +{ + //cout << " TEST: C2 Continuity " << endl; + + int multC0 = 5; + int multC1 = 50; + int multC2 = 200; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,0); + } + + + bool c0 = isFunctionContinuous(fcnSample, mcf, 0, continuityTol, multC0); + bool c1 = isFunctionContinuous(fcnSample, mcf, 1, continuityTol, multC1); + bool c2 = isFunctionContinuous(fcnSample, mcf, 2, continuityTol, multC2); + + + + return (c0 && c1 && c2); + //printf( " passed: C2 continuity established to a multiple\n" + // " of the next Taylor series error term.\n " + // " C0,C1, and C2 multiples: %i,%i and %i\n", + // multC0,multC1,multC2); + //cout << endl; +} + + +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample) +{ + //cout << " TEST: Monotonicity " << endl; + int multEps = 10; + + RigidBodyDynamics::Math::VectorNd fcnSample = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int i=0; i < mcfSample.rows(); i++){ + fcnSample[i] = mcfSample(i,1); + } + + bool monotonic = isVectorMonotonic(fcnSample,10); + return monotonic; + //printf(" passed: curve is monotonic to %i*EPSILON",multEps); + //cout << endl; +} + + +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol) +{ + //cout << " TEST: Derivative correctness " << endl; + int maxDer = 4;//mcf.getMaxDerivativeOrder() - 2; + + RigidBodyDynamics::Math::MatrixNd numSample(mcfSample.rows(),maxDer); + RigidBodyDynamics::Math::MatrixNd relError(mcfSample.rows(),maxDer); + + + RigidBodyDynamics::Math::VectorNd domainX = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + for(int j=0; j tol){ + relError(j,i) = relError(j,i)/mcfSample(j,i+2); + } + } + + } + + RigidBodyDynamics::Math::VectorNd errRelMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + RigidBodyDynamics::Math::VectorNd errAbsMax = + RigidBodyDynamics::Math::VectorNd::Zero(6); + + double absTol = 5*tol; + + bool flagError12=false; + RigidBodyDynamics::Math::VectorNd tolExceeded12V = + RigidBodyDynamics::Math::VectorNd::Zero(mcfSample.rows()); + + int tolExceeded12 = 0; + int tolExceeded34 = 0; + for(int j=0;j tol && mcfSample(i,j+2) > tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + if(mcfSample(i,j+2) > tol) + if(errRelMax[j] < abs(relError(i,j))) + errRelMax[j] = abs(relError(i,j)); + + //This is a harder test: here we're comparing absolute error + //so the tolerance margin is a little higher + if(relError(i,j) > absTol && mcfSample(i,j+2) <= tol){ + if(j <= 1){ + tolExceeded12++; + tolExceeded12V[i]=1; + flagError12=true; + } + if(j>=2) + tolExceeded34++; + } + + if(mcfSample(i,j+2) < tol) + if(errAbsMax[j] < abs(relError(i,j))) + errAbsMax[j] = abs(relError(i,j)); + + + } + + /* + if(flagError12 == true){ + printf("Derivative %i Rel Error Exceeded:\n",j); + printf("x dx_relErr dx_calcVal dx_sample" + " dx2_relErr dx2_calcVal dx2_sample\n"); + for(int i=0; i +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + +static double EPSILON = numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); +static double TOL = std::numeric_limits::epsilon()*1e4; + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; + +/** +This function will print cvs file of the column vector col0 and the matrix + data + +@params col0: A vector that must have the same number of rows as the data matrix + This column vector is printed as the first column +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::VectorNd& col0, + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** +This function will print cvs file of the matrix + data + +@params data: A matrix of data +@params filename: The name of the file to print +*/ +void printMatrixToFile( + const RigidBodyDynamics::Math::MatrixNd& data, + string& filename); + + +/** + This function computes a standard central difference dy/dx. If + extrap_endpoints is set to 1, then the derivative at the end points is + estimated by linearly extrapolating the dy/dx values beside the end points + + @param x domain vector + @param y range vector + @param extrap_endpoints: (false) Endpoints of the returned vector will be + zero, because a central difference + is undefined at these endpoints + (true) Endpoints are computed by linearly + extrapolating using a first difference from + the neighboring 2 points + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd + calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool extrap_endpoints); + +/** + This function computes a standard central difference dy/dx at each point in + a vector x, for a SmoothSegmentedFunction mcf, to a desired tolerance. This + function will take the best step size at each point to minimize the + error caused by taking a numerical derivative, and the error caused by + numerical rounding error: + + For a step size of h/2 to the left and to the right of the point of + interest the error is + error = 1/4*h^2*c3 + r*f(x)/h, (1) + + Where c3 is the coefficient of the 3rd order Taylor series expansion + about point x. Thus c3 can be computed if the order + 2 derivative is + known + + c3 = (d^3f(x)/dx^3)/(6) (2) + + And r*f(x)/h is the rounding error that occurs due to the central + difference. + + Taking a first derivative of 1 and solving for h yields + + h = (r*f(x)*2/c3)^(1/3) + + Where r is EPSILON + + @param x domain vector + @param mcf the SmoothSegmentedFunction of interest + @param order the order of the numerical derivative + @param tolerance desired tolerance on the returned result + @returns dy/dx computed using central differences +*/ +RigidBodyDynamics::Math::VectorNd +calcCentralDifference( RigidBodyDynamics::Math::VectorNd& x, + SmoothSegmentedFunction& mcf, + double tol, int order); + +/** + This function tests numerically for continuity of a curve. The test is + performed by taking a point on the curve, and then two points (called the + shoulder points) to the left and right of the point in question. The + shoulder points are located half way between the sample points in xV. + + The value of the function's derivative is evaluated at each of the shoulder + points and used to linearly extrapolate from the shoulder points back to the + original point. If the original point and the linear extrapolations of each + of the shoulder points agree within a tolerance, then the curve is assumed + to be continuous. This tolerance is evaluated to be the smaller of the 2nd + derivative times a multiplier (taylorErrorMult) or minValueSecondDerivative + + + @param x Values to test for continuity + + @param yx The SmoothSegmentedFunction to test + + @param order The order of the curve of SmoothSegmentedFunction to test + for continuity + + @param minValueSecondDerivative the minimum value allowed for the 2nd + term in the Taylor series. Here we need to define a minimum because + this 2nd term is used to define a point specific tolerance for the + continuity test. If the 2nd derivative happens to go to zero, we + still cannot let the minimum error tolerance to go to zero - else + the test will fail on otherwise good curves. + + @param taylorErrorMult This scales the error tolerance. The default error + tolerance is the the 2nd order Taylor series term. This term is + dependent on the size of the higher-order-terms and the sample + spacing used for xV (since the shoulder points are picked half- + way between the sampled points) +*/ +bool isFunctionContinuous( RigidBodyDynamics::Math::VectorNd& xV, + SmoothSegmentedFunction& yV, + int order, + double minValueSecondDerivative, + double taylorErrorMult); + + +/** +This function will scan through a vector and determine if it is monotonic or +not + +@param y the vector of interest +@param multEPS The tolerance on the monotoncity check, expressed as a scaling of + EPSILON +@return true if the vector is monotonic, false if it is not +*/ +bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, + int multEPS); + + +/** +This function will compute the numerical integral of y(x) using the trapezoidal +method + +@param x the domain vector +@param y the range vector, of y(x), evaluated at x +@param flag_TrueIntForward_FalseIntBackward + When this flag is set to true, the integral of y(x) will be evaluated from + left to right, starting with int(y(0)) = 0. When this flag is false, then + y(x) will be evaluated from right to left with int(y(n)) = 0, where n is + the maximum number of elements. +@return the integral of y(x) +*/ +RigidBodyDynamics::Math::VectorNd calcTrapzIntegral( + RigidBodyDynamics::Math::VectorNd& x, + RigidBodyDynamics::Math::VectorNd& y, + bool flag_TrueIntForward_FalseIntBackward); + + +/** + @param a The first vector + @param b The second vector + @return Returns the maximum absolute difference between vectors a and b +*/ +double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, + RigidBodyDynamics::Math::VectorNd& b); + + +/* +This function tests the SmoothSegmentedFunction to see if it is C2 continuous. +This function works by using the applying the function isFunctionContinuous +multiple times. For details of the method used please refer to the doxygen for +isFunctionContinuous. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. +@param continuityTol +@return bool: true if the curve is C2 continuous to the desired tolernace + +*/ +bool isCurveC2Continuous(SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double continuityTol); + +/* + 4. The MuscleCurveFunctions which are supposed to be monotonic will be + tested for monotonicity. +*/ +bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample); + +/** +This function compares the i^th derivative return by a SmoothSegmented function +against a numerical derivative computed using a central difference applied to +the (i-1)^th derivative of the function. This function first checks the +1st derivative and continues checking derivatives until the 4th derivative. + +@param mcf - a SmoothSegmentedFunction +@param mcfSample: + A n-by-m matrix of values where the first column is the domain (x) of interest + and the remaining columns are the curve value (y), and its derivatives (dy/dx, + d2y/dx2, d3y/dx3, etc). This matrix is returned by the function + calcSampledCurve in SmoothSegmented Function. + +@param tol : the tolerance used to assess the relative error between the + numerically computed derivatives and the derivatives returned by + the SmoothSegmentedFunction +@return bool: true if all of the derivatives up to the 4th (hard coded) are + within a relative tolerance of tol w.r.t. to the numerically + computed derivatives +*/ +bool areCurveDerivativesCloseToNumericDerivatives( + SmoothSegmentedFunction& mcf, + RigidBodyDynamics::Math::MatrixNd& mcfSample, + double tol); diff --git a/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc b/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc new file mode 100644 index 0000000..e63c8db --- /dev/null +++ b/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc @@ -0,0 +1,552 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testSmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + This also includes additional curves (the Anderson2007 curves) + which are not presently in OpenSim. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +/* +Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory +class, a class that enables the easy generation of C2 continuous curves +that define the various characteristic curves required in a muscle model + */ + +// Author: Matthew Millard + +//============================================================================== +// INCLUDES +//============================================================================== + +#include "geometry.h" +#include "numericalTestFunctions.h" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; + +using namespace std; + + + + +/** + This function will create a quintic Bezier curve y(x) and sample it, its + first derivative w.r.t. U (dx(u)/du and dy(u)/du), and its first derivative + w.r.t. to X and print it to the screen. +*/ +void testSegmentedQuinticBezierDerivatives( + int maximumNumberOfToleranceViolations) +{ + //cout <<"**************************************************"<tol) + //printf("Error > Tol: (%i,%i): %f > %f\n",j,i,errorDer(j,i),tol); + } + } + //errorDer.cwiseAbs(); + //cout << errorDer << endl; + + //printf("...absolute tolerance of %f met\n", tol); + + //cout << " TEST: Bezier Curve Derivative DYDX to d6y/dx6" << endl; + RigidBodyDynamics::Math::MatrixNd numericDerXY(analyticDerXU.rows(), 6); + RigidBodyDynamics::Math::MatrixNd analyticDerXY(analyticDerXU.rows(),6); + + for(int i=0; i< analyticDerXU.rows(); i++) + { + analyticDerXY(i,0) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,1); + analyticDerXY(i,1) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,2); + analyticDerXY(i,2) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,3); + analyticDerXY(i,3) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,4); + analyticDerXY(i,4) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,5); + analyticDerXY(i,5) = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,6); + } + + + for(int j=0; jrelTol){ + //printf("Error > Tol: (%i,%i): %f > %f\n",i,j, + // errorDerXY(i,j),relTol); + relTolExceeded++; + } + } + } + //cout << relTolExceeded << endl; + + //The relative tolerance gets exceeded occasionally in locations of + //rapid change in the curve. Provided there are only a few locations + //where the relative tolerance of 5% is broken, the curves should be + //regarded as being good. Ten errors out of a possible 100*6 data points + //seems relatively small. + CHECK(relTolExceeded < maximumNumberOfToleranceViolations); + + + //std::string fname = "analyticDerXY.csv"; + //printMatrixToFile(analyticDerXY,fname); + //fname = "numericDerXY.csv"; + //printMatrixToFile(numericDerXY,fname); + //fname = "errorDerXY.csv"; + //printMatrixToFile(errorDerXY,fname); + //printf(" ...relative tolerance of %f not exceeded more than %i times\n" + // " across all 6 derivatives, with 100 samples each\n", + // relTol, 10); + //cout <<"**************************************************"< + +DESCRIPTION + +rbdl_luamodel allows to load models that are specified as Lua scripts. Lua +is an open-source light-weight scripting language (http://www.lua.org). + +This addon comes with a standalone utility that can show various +information of a lua model such as degree of freedom overview or model +hierarchy. It is located in addons/luamodel/rbdl_luamodel_test. Use the -h +switch to see available options. + +Note: this addon is not even remotely as thoroughly tested as the RBDL +itself so please use it with some suspicion. + +DOCUMENTATION + +The documentation for this addon is built with the RBDL documentation. You +can find it in Modules -> Addon: rbdl_luamodel. + +LICENSING + +This code is published under the zlib license, however some parts of the +CMake scripts are taken from other projects and are licensed under +different terms. + +Full license text: + +------- +rbdl_luamodel - load RBDL models from Lua files +Copyright (c) 2011-2018 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/luamodel/luamodel.cc b/3rdparty/rbdl/addons/luamodel/luamodel.cc new file mode 100644 index 0000000..238b033 --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luamodel.cc @@ -0,0 +1,565 @@ +#include "rbdl/rbdl.h" +#include "luamodel.h" + +#include +#include + +#include "luatables.h" + +extern "C" +{ +#include +#include +#include +} + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +template<> +Vector3d LuaTableNode::getDefault(const Vector3d &default_value) { + Vector3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + cerr << "LuaModel Error: invalid 3d vector!" << endl; + abort(); + } + + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + } + + stackRestore(); + + return result; +} + +template<> +SpatialVector LuaTableNode::getDefault( + const SpatialVector &default_value +) { + SpatialVector result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 6) { + cerr << "LuaModel Error: invalid 6d vector!" << endl; + abort(); + } + result[0] = vector_table[1]; + result[1] = vector_table[2]; + result[2] = vector_table[3]; + result[3] = vector_table[4]; + result[4] = vector_table[5]; + result[5] = vector_table[6]; + } + + stackRestore(); + + return result; +} + +template<> +Matrix3d LuaTableNode::getDefault(const Matrix3d &default_value) { + Matrix3d result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + if (vector_table.length() != 3) { + cerr << "LuaModel Error: invalid 3d matrix!" << endl; + abort(); + } + + if (vector_table[1].length() != 3 + || vector_table[2].length() != 3 + || vector_table[3].length() != 3) { + cerr << "LuaModel Error: invalid 3d matrix!" << endl; + abort(); + } + + result(0,0) = vector_table[1][1]; + result(0,1) = vector_table[1][2]; + result(0,2) = vector_table[1][3]; + + result(1,0) = vector_table[2][1]; + result(1,1) = vector_table[2][2]; + result(1,2) = vector_table[2][3]; + + result(2,0) = vector_table[3][1]; + result(2,1) = vector_table[3][2]; + result(2,2) = vector_table[3][3]; + } + + stackRestore(); + + return result; +} + +template<> +SpatialTransform LuaTableNode::getDefault( + const SpatialTransform &default_value +) { + SpatialTransform result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + result.r = vector_table["r"].getDefault(Vector3d::Zero(3)); + result.E = vector_table["E"].getDefault(Matrix3d::Identity (3,3)); + } + + stackRestore(); + + return result; +} + +template<> +Joint LuaTableNode::getDefault(const Joint &default_value) { + Joint result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + int joint_dofs = vector_table.length(); + + if (joint_dofs == 1) { + string dof_string = vector_table[1].getDefault(""); + if (dof_string == "JointTypeSpherical") { + stackRestore(); + return Joint(JointTypeSpherical); + } else if (dof_string == "JointTypeEulerZYX") { + stackRestore(); + return Joint(JointTypeEulerZYX); + } + if (dof_string == "JointTypeEulerXYZ") { + stackRestore(); + return Joint(JointTypeEulerXYZ); + } + if (dof_string == "JointTypeEulerYXZ") { + stackRestore(); + return Joint(JointTypeEulerYXZ); + } + if (dof_string == "JointTypeTranslationXYZ") { + stackRestore(); + return Joint(JointTypeTranslationXYZ); + } + } + + if (joint_dofs > 0) { + if (vector_table[1].length() != 6) { + cerr << "LuaModel Error: invalid joint motion subspace description at " + << this->keyStackToString() << endl; + abort(); + } + } + + switch (joint_dofs) { + case 0: + result = Joint(JointTypeFixed); + break; + case 1: + result = Joint (vector_table[1].get()); + break; + case 2: + result = Joint( + vector_table[1].get(), + vector_table[2].get() + ); + break; + case 3: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get() + ); + break; + case 4: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get() + ); + break; + case 5: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get() + ); + break; + case 6: + result = Joint( + vector_table[1].get(), + vector_table[2].get(), + vector_table[3].get(), + vector_table[4].get(), + vector_table[5].get(), + vector_table[6].get() + ); + break; + default: + cerr << "Invalid number of DOFs for joint." << endl; + abort(); + break; + } + } + + stackRestore(); + + return result; +} + +template<> +Body LuaTableNode::getDefault(const Body &default_value) { + Body result = default_value; + + if (stackQueryValue()) { + LuaTable vector_table = LuaTable::fromLuaState (luaTable->L); + + double mass = 0.; + Vector3d com (Vector3d::Zero(3)); + Matrix3d inertia (Matrix3d::Identity(3,3)); + + mass = vector_table["mass"]; + com = vector_table["com"].getDefault(com); + inertia = vector_table["inertia"].getDefault(inertia); + + result = Body (mass, com, inertia); + } + + stackRestore(); + + return result; +} + +namespace RigidBodyDynamics { + +namespace Addons { + +bool LuaModelReadFromTable (LuaTable &model_table, Model *model, bool verbose); +bool LuaModelReadConstraintsFromTable ( + LuaTable &model_table, + Model *model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +); + +typedef map StringIntMap; +StringIntMap body_table_id_map; + +RBDL_DLLAPI +bool LuaModelReadFromLuaState (lua_State* L, Model* model, bool verbose) { + assert (model); + + LuaTable model_table = LuaTable::fromLuaState (L); + + return LuaModelReadFromTable (model_table, model, verbose); +} + +RBDL_DLLAPI +bool LuaModelReadFromFile (const char* filename, Model* model, bool verbose) { + if(!model) { + std::cerr << "Model not provided." << std::endl; + assert(false); + abort(); + } + + LuaTable model_table = LuaTable::fromFile (filename); + + return LuaModelReadFromTable (model_table, model, verbose); +} + + +RBDL_DLLAPI +std::vector LuaModelGetConstraintSetNames(const char* filename) { + std::vector result; + + LuaTable model_table = LuaTable::fromFile (filename); + + std::vector constraint_keys; + if (model_table["constraint_sets"].exists()) { + constraint_keys = model_table["constraint_sets"].keys(); + } + + if (constraint_keys.size() == 0) { + return result; + } + + for (size_t ci = 0; ci < constraint_keys.size(); ++ci) { + if (constraint_keys[ci].type != LuaKey::String) { + std::cerr << "Invalid constraint found in model.constraint_sets: no constraint set name was specified!" + << std::endl; + abort(); + } + + result.push_back(constraint_keys[ci].string_value); + } + + return result; +} + +RBDL_DLLAPI +bool LuaModelReadFromFileWithConstraints ( + const char* filename, + Model* model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +) { + if(!model) { + std::cerr << "Model not provided." << std::endl; + assert(false); + abort(); + } + if(constraint_sets.size() != constraint_set_names.size()) { + std::cerr << "Number of constraint sets different from the number of \ + constraint set names." << std::endl; + assert(false); + abort(); + } + + LuaTable model_table = LuaTable::fromFile (filename); + bool modelLoaded = LuaModelReadFromTable (model_table, model, verbose); + bool constraintsLoaded = LuaModelReadConstraintsFromTable (model_table, model + , constraint_sets, constraint_set_names, verbose); + for(size_t i = 0; i < constraint_sets.size(); ++i) { + constraint_sets[i].Bind(*model); + } + + return modelLoaded && constraintsLoaded; +} + + +bool LuaModelReadFromTable (LuaTable &model_table, Model* model, bool verbose) { + if (model_table["gravity"].exists()) { + model->gravity = model_table["gravity"].get(); + + if (verbose) + cout << "gravity = " << model->gravity.transpose() << endl; + } + + int frame_count = model_table["frames"].length(); + + body_table_id_map["ROOT"] = 0; + + for (int i = 1; i <= frame_count; i++) { + if (!model_table["frames"][i]["parent"].exists()) { + cerr << "Parent not defined for frame " << i << "." << endl; + abort(); + } + + string body_name = model_table["frames"][i]["name"].getDefault(""); + string parent_name = model_table["frames"][i]["parent"].get(); + unsigned int parent_id = body_table_id_map[parent_name]; + + SpatialTransform joint_frame + = model_table["frames"][i]["joint_frame"].getDefault(SpatialTransform()); + Joint joint + = model_table["frames"][i]["joint"].getDefault(Joint(JointTypeFixed)); + Body body = model_table["frames"][i]["body"].getDefault(Body()); + + unsigned int body_id + = model->AddBody (parent_id, joint_frame, joint, body, body_name); + body_table_id_map[body_name] = body_id; + + if (verbose) { + cout << "==== Added Body ====" << endl; + cout << " body_name : " << body_name << endl; + cout << " body id : " << body_id << endl; + cout << " parent_id : " << parent_id << endl; + cout << " joint dofs : " << joint.mDoFCount << endl; + for (unsigned int j = 0; j < joint.mDoFCount; j++) { + cout << " " << j << ": " << joint.mJointAxes[j].transpose() << endl; + } + cout << " joint_frame: " << joint_frame << endl; + } + } + + return true; +} + +bool LuaModelReadConstraintsFromTable ( + LuaTable &model_table, + Model *model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose +) { + for(size_t i = 0; i < constraint_set_names.size(); ++i) { + if (verbose) { + std::cout << "==== Constraint Set: " << constraint_set_names[i] << std::endl; + } + + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + .exists()) { + cerr << "Constraint set not existing: " << constraint_set_names[i] << "." + << endl; + assert(false); + abort(); + } + + size_t num_constraints = model_table["constraint_sets"] + [constraint_set_names[i].c_str()] + .length(); + + for(size_t ci = 0; ci < num_constraints; ++ci) { + if (verbose) { + std::cout << "== Constraint " << ci << "/" << num_constraints << " ==" << std::endl; + } + + if(!model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["constraint_type"].exists()) { + cerr << "constraint_type not specified." << endl; + assert(false); + abort(); + } + string constraintType = model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["constraint_type"] + .getDefault(""); + std::string constraint_name = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["name"].getDefault(""); + + if(constraintType == "contact") { + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["body"].exists()) { + cerr << "body not specified." << endl; + assert(false); + abort(); + } + constraint_sets[i].AddContactConstraint + (model->GetBodyId(model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["body"] + .getDefault("").c_str()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["point"].getDefault(Vector3d::Zero()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["normal"].getDefault(Vector3d::Zero()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["name"].getDefault("").c_str() + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["normal_acceleration"].getDefault(0.)); + if(verbose) { + cout << " type = contact" << endl; + cout << " name = " << constraint_name << std::endl; + cout << " body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["body"].getDefault("") << endl; + cout << " body point = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["point"].getDefault(Vector3d::Zero()).transpose() + << endl; + cout << " world normal = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["normal"].getDefault(Vector3d::Zero()).transpose() + << endl; + cout << " normal acceleration = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["normal_acceleration"].getDefault(0.) << endl; + } + } + else if(constraintType == "loop") { + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].exists()) { + cerr << "predecessor_body not specified." << endl; + assert(false); + abort(); + } + if(!model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_body"].exists()) { + cerr << "successor_body not specified." << endl; + assert(false); + abort(); + } + + // Add the loop constraint as a non-stablized constraint and compute + // and set the actual stabilization cofficients for the Baumgarte + // stabilization afterwards if enabled. + unsigned int constraint_id; + constraint_id = constraint_sets[i].AddLoopConstraint(model->GetBodyId + (model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].getDefault("").c_str()) + , model->GetBodyId(model_table["constraint_sets"] + [constraint_set_names[i].c_str()][ci + 1]["successor_body"] + .getDefault("").c_str()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["predecessor_transform"].getDefault(SpatialTransform()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["successor_transform"].getDefault(SpatialTransform()) + , model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["axis"].getDefault(SpatialVector::Zero()) + , false + , 0.0 + , constraint_name.c_str()); + + bool enable_stabilization = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["enable_stabilization"].getDefault(false); + double stabilization_parameter = 0.0; + if (enable_stabilization) { + stabilization_parameter = model_table["constraint_sets"][constraint_set_names[i].c_str()][ci + 1] + ["stabilization_parameter"].getDefault(0.1); + if (stabilization_parameter <= 0.0) { + std::cerr << "Invalid stabilization parameter: " << stabilization_parameter + << " must be > 0.0" << std::endl; + abort(); + } + double stabilization_coefficient = 1.0 / stabilization_parameter; + constraint_sets[i].baumgarteParameters[i] = Vector2d( + stabilization_coefficient, stabilization_coefficient); + } + + if(verbose) { + cout << " type = loop" << endl; + cout << " name = " << constraint_name << std::endl; + cout << " predecessor body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_body"].getDefault("") << endl; + cout << " successor body = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_body"].getDefault("") << endl; + cout << " predecessor body transform = " << endl + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["predecessor_transform"] + .getDefault(SpatialTransform()) << endl; + cout << " successor body transform = " << endl + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["successor_transform"] + .getDefault(SpatialTransform()) << endl; + cout << " constraint axis (in predecessor frame) = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["axis"].getDefault(SpatialVector::Zero()) + .transpose() << endl; + cout << " enable_stabilization = " << enable_stabilization + << endl; + if (enable_stabilization) { + cout << " stabilization_parameter = " << stabilization_parameter + << endl; + } + cout << " constraint name = " + << model_table["constraint_sets"][constraint_set_names[i].c_str()] + [ci + 1]["name"].getDefault("").c_str() << endl; + } + } + else { + cerr << "Invalid constraint type: " << constraintType << endl; + abort(); + } + } + } + + return true; +} + +} + +} diff --git a/3rdparty/rbdl/addons/luamodel/luamodel.h b/3rdparty/rbdl/addons/luamodel/luamodel.h new file mode 100644 index 0000000..8ab6fba --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luamodel.h @@ -0,0 +1,318 @@ +#ifndef _RBDL_LUAMODEL_H +#define _RBDL_LUAMODEL_H + +#include +#include +#include + +extern "C" { + struct lua_State; +}; + +namespace RigidBodyDynamics { + +struct Model; +struct ConstraintSet; + +namespace Addons { + +/** \page addon_luamodel_page Addon: rbdl_luamodel +* @{ +* +* \section luamodel_introduction Lua Models +* +* The Addon LuaModel allows to load \link RigidBodyDynamics::Model Models\endlink +* that are specified as Lua scripts. Lua is +* an open-source light-weight scripting language (http://www.lua.org). +* This addon is not enabled by default and one has to enable it by +* setting BUILD_ADDON_LUAMODEL to true in when configuring the RBDL with +* CMake. +* +* This addon comes with a standalone utility that can show various +* information of a lua model such as degree of freedom overview or model +* hierarchy. It is located in addons/luamodel/rbdl_luamodel_test. Use the -h +* switch to see available options. +* +* Note: this addon is not even remotely as thoroughly tested as the RBDL +* itself so please use it with some suspicion. +* +* \section luamodel_format Format Overview +* +* Models have to be specified as a specially formatted Lua table which must +* be returned by the script, i.e. if the model is specified in the table +* "model = { ... }" the script has to return this when executed. Within the +* returned table, rbdl_luamodel goes through the table "frames" and builds +* the model from the individual Frame Information Tables (see further down +* for more information about those). +* +* A valid file could look like this: +* +* \code +* model = { +* frames = { +* { +* +* }, +* { +* +* } +* } +* } +* +* return model +* \endcode +* +* Apart from the frames you can also specify gravity in the model file. +* +* Example: +* \code +* model = { +* gravity = {0., 0., -9.81} +* +* frames = { +* ... +* } +* } +* \endcode +* +* Finally, several constraint sets can be defined for the model. +* +* Example: +* \code +* model = { +* constraint_sets = { +* constraint_set_1_name = { +* { +* ... +* }, +* { +* ... +* }, +* }, +* constraint_set_2_name = { +* ... +* }, +* }, +* } +* \endcode +* +* \note The table frames must contain all Frame Information Tables as a list +* and individual tables must *not* be specified with a key, i.e. +* \code +* frames = { +* some_frame = { +* ... +* }, +* { +* .. +* } +* } +* \endcode +* is not possible as Lua does not retain the order of the individual +* frames when an explicit key is specified. +* +* \section luamodel_frame_table Frame Information Table +* +* The Frame Information Table is searched for values needed to call +* Model::AddBody(). The following fields are used by rbdl_luamodel +* (everything else is ignored): +* +* \par name (required, type: string): +* Name of the body that is being added. This name must be unique. +* +* \par parent (required, type: string): +* If the value is "ROOT" the parent frame of this body is assumed to be +* the base coordinate system, otherwise it must be the exact same string +* as was used for the "name"-field of the parent frame. +* +* \par body (optional, type: table) +* Specification of the dynamical parameters of the body. It uses the +* values (if existing): +* \code +* mass (scalar value, default 1.), +* com (3-d vector, default: (0., 0., 0.)) +* inertia (3x3 matrix, default: identity matrix) +* \endcode +* \par +* to create a \ref RigidBodyDynamics::Body. +* +* \par joint (optional, type: table) +* Specifies the type of joint, fixed or up to 6 degrees of freedom. Each +* entry in the joint table should be a 6-d that defines the motion +* subspace of a single degree of freedom. +* \par +* Default joint type is a fixed joint that attaches the body rigidly to +* its parent. +* \par +* 3-DoF joints are described by using strings. The following 3-DoF +* joints are available: +* +* - "JointTypeSpherical": for singularity-free spherical joints +* - "JointTypeEulerZYX": Euler ZYX joint +* - "JointTypeEulerXYZ": Euler XYZ joint +* - "JointTypeEulerYXZ": Euler YXZ joint +* - "JointTypeTranslationXYZ": Free translational joint +* +* \par +* Examples: +* \code +* joint_fixed = {} +* joint_translate_x = { {0., 0., 0., 1., 0., 0.} } +* joint_translate_xy = { +* {0., 0., 0., 1., 0., 0.}, +* {0., 0., 0., 0., 1., 0.} +* } +* joint_rotate_zyx = { +* {0., 0., 1., 0., 0., 0.}, +* {0., 1., 0., 0., 0., 0.}, +* {1., 0., 0., 0., 0., 0.}, +* } +* joint_rotate_euler_zyx = { "JointTypeEulerZYX" } +* \endcode +* +* \par joint_frame (optional, type: table) +* Specifies the origin of the joint in the frame of the parent. It uses +* the values (if existing): +* \code +* r (3-d vector, default: (0., 0., 0.)) +* E (3x3 matrix, default: identity matrix) +* \endcode +* \par +* for which r is the translation and E the rotation of the joint frame +* +* \section luamodel_constraint_table Constraint Information Table +* The Constraint Information Table is searched for values needed to call +* ConstraintSet::AddContactConstraint() or ConstraintSet::AddLoopConstraint(). +* The employed fields are defined as follows. Please note that different fields +* may be used for different constraint types. +* +* \par constraint_type (required, type: string) +* Specifies the type of constraints, either 'contact' or 'loop', other +* values will cause a failure while reading the file. +* +* \par name (optional, type: string) +* Specifies a name for the constraint. +* +* The following fields are used exclusively for contact constraints: +* +* \par body (required, type: string) +* The name of the body involved in the constraint. +* +* \par point(optional, type: table) +* The position of the contact point in body coordinates. Defaults to +* (0, 0, 0). +* +* \par normal(optional, type: table) +* The normal along which the constraint acts in world coordinates. +* Defaults to (0, 0, 0). +* +* \par normal_acceleration(optional, type: number) +* The normal acceleration along the constraint axis. Defaults to 0. +* +* The following fields are used exclusively for loop constraints: +* +* \par predecessor_body (required, type: string) +* The name of the predecessor body involved in the constraint. +* +* \par successor_body (required, type: string) +* The name of the successor body involved in the constraint. +* +* \par predecessor_transform (optional, type: table) +* The transform of the predecessor constrained frame with respect to +* the predecessor body frame. Defaults to the identity transform. +* +* \par sucessor_transform (optional, type: table) +* The transform of the sucessor constrained frame with respect to +* the sucessor body frame. Defaults to the identity transform. +* +* \par axis (optional, type: table) +* The 6d spatial axis along which the constraint acts, in coordinates +* relative to the predecessor constrained frame. Defaults to (0,0,0,0,0,0). +* +* \par enable_stabilization(optional, type: bool, default: false) +* Whether Baumgarte stabilization should be enabled or not. +* +* \par stabilization_parameter(optional, type: number) +* The stabilization parameter T_stab for the Baumgarte stabilization (see +* RBDL documentation on how this parameter is used). Defaults to 0.1. +* +* \section luamodel_example Example +* Here is an example of a model: +* \include samplemodel.lua +* +* \section luamodel_example_constraints Example with constraints +* Here is an example of a model including constraints: +* \include sampleconstrainedmodel.lua +* +*/ + +/** \brief Reads a model from a Lua file. + * + * \param filename the name of the Lua file. + * \param model a pointer to the output Model structure. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model was read successfully. + * + * \note See \ref luamodel_introduction for information on how to define the + * Lua model. + */ +RBDL_DLLAPI +bool LuaModelReadFromFile ( + const char* filename, + Model* model, + bool verbose = false); + +/** \brief Reads a model file and returns the names of all constraint sets. + */ +RBDL_DLLAPI +std::vector LuaModelGetConstraintSetNames(const char* filename); + +/** \brief Reads a model and constraint sets from a Lua file. + * + * \param filename the name of the Lua file. + * \param model a pointer to the output Model structure. + * \param constraint_sets reference to a std::vector of ConstraintSet structures + * in which to save the information read from the file. + * \param constraint_set_names reference to a std::vector of std::string + * specifying the names of the constraint sets to be read from the Lua file. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model and constraint sets were read successfully. + * + * \note constraint_sets and constraint_set_names are required to have the same + * size. See \ref luamodel_introduction for information on how to define the + * Lua model. + */ +RBDL_DLLAPI +bool LuaModelReadFromFileWithConstraints ( + const char* filename, + Model* model, + std::vector& constraint_sets, + const std::vector& constraint_set_names, + bool verbose = false); + +/** \brief Reads a model from a lua_State. + * + * \param L a pointer to the lua_State. + * \param model a pointer to the output Model structure. + * \param verbose specifies wether information on the model should be printed + * (default: true). + * + * \returns true if the model was read successfully. + */ +RBDL_DLLAPI +bool LuaModelReadFromLuaState ( + lua_State* L, + Model* model, + bool verbose = false); + +/** @} */ +} + +} + +/* _RBDL_LUAMODEL_H */ +#endif diff --git a/3rdparty/rbdl/addons/luamodel/luatables.cc b/3rdparty/rbdl/addons/luamodel/luatables.cc new file mode 100644 index 0000000..752529f --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luatables.cc @@ -0,0 +1,932 @@ +/* + * LuaTables++ + * Copyright (c) 2013-2018 Martin Felis . + * All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "luatables.h" + +#include +#include +#include +#include +#include +#include + +extern "C" +{ + #include + #include + #include +} + +#include /* defines FILENAME_MAX */ + +#if defined(WIN32) || defined (_WIN32) + #include + #define get_current_dir _getcwd + #define DIRECTORY_SEPARATOR "\\" +#elif defined(linux) || defined (__linux) || defined(__linux__) || defined(__APPLE__) + #include + #define get_current_dir getcwd + #define DIRECTORY_SEPARATOR "/" +#else + #error Platform not supported! +#endif + +using namespace std; + +std::string get_file_directory (const char* filename) { + string name (filename); + string result = name.substr(0, name.find_last_of (DIRECTORY_SEPARATOR) + 1); + + if (result == "") + result = "./"; +#if defined (WIN32) || defined (_WIN32) + else if (result.substr(1,2) != ":\\") + result = string(".\\") + result; +#else + else if (result.substr(0,string(DIRECTORY_SEPARATOR).size()) != DIRECTORY_SEPARATOR && result[0] != '.') + result = string("./") + result; +#endif + + return result; +} + +// char encoded serialize function that is available in plaintext in +// utils/serialize.lua. Converted using lua auto.lua serialize.lua +const char serialize_lua[] = { + + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x69, 0x73, 0x6c, + 0x69, 0x73, 0x74, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, + 0x20, 0x30, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, + 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x28, + 0x74, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x3d, 0x20, 0x69, 0x74, 0x65, 0x6d, + 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x2b, 0x20, 0x31, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x3d, 0x20, + 0x6e, 0x69, 0x6c, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, + 0x65, 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x6c, 0x61, + 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x6f, 0x72, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, + 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, + 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, + 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, + 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, + 0x76, 0x29, 0x20, 0x7e, 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, + 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x0a, + 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x74, 0x79, 0x70, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x79, 0x70, 0x65, + 0x28, 0x76, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x69, 0x74, 0x65, 0x6d, 0x63, 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x7e, 0x3d, 0x20, 0x23, + 0x74, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x63, 0x6d, 0x70, + 0x5f, 0x61, 0x6c, 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x20, 0x28, 0x61, 0x2c, 0x20, + 0x62, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x28, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, + 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x6f, 0x72, 0x20, 0x74, 0x79, + 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, + 0x61, 0x6e, 0x64, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, + 0x6d, 0x62, 0x65, 0x72, 0x22, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x61, 0x20, 0x3c, 0x20, 0x62, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x61, 0x29, 0x20, 0x3c, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x62, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x67, 0x65, 0x6e, + 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, + 0x74, 0x29, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, + 0x69, 0x63, 0x65, 0x73, 0x20, 0x3d, 0x20, 0x7b, 0x7d, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, + 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, + 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x6b, 0x29, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, 0x73, 0x6f, 0x72, 0x74, 0x20, 0x28, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x5f, 0x69, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x2c, 0x20, 0x63, 0x6d, 0x70, 0x5f, 0x61, 0x6c, + 0x70, 0x68, 0x61, 0x6e, 0x75, 0x6d, 0x65, 0x72, 0x69, 0x63, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, 0x78, 0x74, 0x20, 0x28, 0x74, 0x2c, 0x20, 0x73, 0x74, 0x61, 0x74, + 0x65, 0x29, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, + 0x65, 0x73, 0x20, 0x3d, 0x20, 0x67, 0x65, 0x6e, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x69, + 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, + 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x20, 0x3d, 0x20, 0x31, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x2e, + 0x67, 0x65, 0x74, 0x6e, 0x28, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, + 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x5d, 0x20, 0x3d, 0x3d, 0x20, 0x73, 0x74, 0x61, 0x74, 0x65, 0x20, + 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x6b, 0x65, 0x79, 0x20, 0x3d, 0x20, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, + 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, 0x73, 0x5b, 0x69, 0x20, 0x2b, 0x20, 0x31, 0x5d, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6b, 0x65, 0x79, 0x2c, 0x20, 0x74, 0x5b, 0x6b, 0x65, + 0x79, 0x5d, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x74, 0x2e, 0x5f, 0x5f, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x49, 0x6e, 0x64, 0x69, 0x63, 0x65, + 0x73, 0x20, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x20, 0x28, 0x74, 0x29, 0x0a, + 0x09, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x6f, 0x72, 0x64, 0x65, 0x72, 0x65, 0x64, 0x5f, 0x6e, 0x65, + 0x78, 0x74, 0x2c, 0x20, 0x74, 0x2c, 0x20, 0x6e, 0x69, 0x6c, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, + 0x20, 0x28, 0x6f, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x0a, + 0x20, 0x20, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x22, + 0x22, 0x0a, + 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, + 0x3d, 0x20, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x69, 0x66, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x70, 0x61, 0x69, 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x20, 0x3d, 0x20, 0x6f, 0x72, 0x64, + 0x65, 0x72, 0x65, 0x64, 0x5f, 0x70, 0x61, 0x69, 0x72, 0x73, 0x0a, + 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x3d, 0x20, 0x6e, 0x69, 0x6c, 0x20, 0x74, + 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x3d, 0x20, 0x22, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, + 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x62, 0x6f, 0x6f, 0x6c, 0x65, 0x61, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6f, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x6f, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, + 0x22, 0x22, 0x0a, + 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, 0x3d, + 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x69, 0x73, 0x6c, 0x69, 0x73, + 0x74, 0x28, 0x6f, 0x29, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x22, 0x0a, + 0x09, 0x09, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, + 0x75, 0x62, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x66, 0x6f, 0x72, 0x20, 0x69, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x69, 0x70, 0x61, 0x69, 0x72, + 0x73, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, 0x62, + 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, 0x3d, 0x20, 0x22, + 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, 0x61, + 0x62, 0x6c, 0x65, 0x20, 0x3d, 0x20, 0x74, 0x72, 0x75, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, + 0x3d, 0x3d, 0x20, 0x22, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x5c, 0x22, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x76, 0x20, 0x2e, 0x2e, + 0x20, 0x22, 0x5c, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, + 0x6e, 0x67, 0x20, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x69, 0x66, 0x20, 0x6c, 0x61, 0x73, 0x74, 0x5f, 0x77, 0x61, 0x73, 0x5f, 0x73, 0x75, 0x62, 0x74, + 0x61, 0x62, 0x6c, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, + 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x5c, 0x6e, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x0a, + 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x74, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x6f, 0x2e, 0x64, 0x6f, 0x6e, 0x74, 0x5f, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x5f, 0x6d, 0x65, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x22, 0x7b, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7b, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x6b, 0x2c, 0x76, 0x20, 0x69, 0x6e, 0x20, 0x70, 0x61, 0x69, + 0x72, 0x73, 0x5f, 0x66, 0x75, 0x6e, 0x63, 0x28, 0x6f, 0x29, 0x20, 0x64, 0x6f, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x7e, + 0x3d, 0x20, 0x22, 0x66, 0x75, 0x6e, 0x63, 0x74, 0x69, 0x6f, 0x6e, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x2d, 0x2d, 0x20, 0x6d, 0x61, 0x6b, 0x65, 0x20, 0x73, 0x75, + 0x72, 0x65, 0x20, 0x74, 0x68, 0x61, 0x74, 0x20, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x65, 0x64, 0x20, 0x6b, + 0x65, 0x79, 0x73, 0x20, 0x61, 0x72, 0x65, 0x20, 0x70, 0x72, 0x6f, 0x70, 0x65, 0x72, 0x6c, 0x79, 0x20, 0x61, + 0x72, 0x65, 0x20, 0x69, 0x6e, 0x64, 0x65, 0x78, 0x69, 0x66, 0x69, 0x65, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6b, 0x29, + 0x20, 0x3d, 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x20, 0x20, 0x69, 0x66, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x76, 0x29, 0x20, 0x3d, + 0x3d, 0x20, 0x22, 0x6e, 0x75, 0x6d, 0x62, 0x65, 0x72, 0x22, 0x20, 0x74, 0x68, 0x65, 0x6e, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, + 0x69, 0x6e, 0x67, 0x28, 0x76, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, + 0x75, 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, + 0x5b, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x28, 0x6b, 0x29, 0x20, + 0x2e, 0x2e, 0x20, 0x22, 0x5d, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, + 0x6c, 0x69, 0x7a, 0x65, 0x20, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, + 0x20, 0x20, 0x22, 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, + 0x5c, 0x6e, 0x22, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, + 0x6c, 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x6f, 0x73, 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x28, 0x6b, 0x29, 0x20, 0x2e, + 0x2e, 0x20, 0x22, 0x20, 0x3d, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, + 0x7a, 0x65, 0x28, 0x76, 0x2c, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x20, 0x20, 0x22, + 0x2c, 0x20, 0x73, 0x6f, 0x72, 0x74, 0x65, 0x64, 0x29, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x2c, 0x5c, 0x6e, 0x22, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x20, 0x3d, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, + 0x74, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x61, 0x62, 0x73, 0x20, 0x2e, 0x2e, 0x20, 0x22, 0x7d, 0x22, 0x0a, + 0x20, 0x20, 0x65, 0x6c, 0x73, 0x65, 0x0a, + 0x20, 0x20, 0x20, 0x20, 0x70, 0x72, 0x69, 0x6e, 0x74, 0x20, 0x28, 0x22, 0x6e, 0x6f, 0x74, 0x20, 0x73, 0x65, + 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6e, 0x74, 0x72, 0x79, 0x20, 0x6f, 0x66, + 0x20, 0x74, 0x79, 0x70, 0x65, 0x20, 0x22, 0x20, 0x2e, 0x2e, 0x20, 0x74, 0x79, 0x70, 0x65, 0x28, 0x6f, 0x29, + 0x20, 0x29, 0x0a, + 0x20, 0x20, 0x65, 0x6e, 0x64, 0x0a, + 0x20, 0x20, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x72, 0x65, 0x73, 0x75, 0x6c, 0x74, 0x0a, + 0x65, 0x6e, 0x64, 0x0a, + 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x20, 0x73, 0x65, 0x72, 0x69, 0x61, 0x6c, 0x69, 0x7a, 0x65, 0x0a, +}; + +// +// Lua Helper Functions +// +void bail(lua_State *L, const char *msg){ + std::cerr << msg << lua_tostring(L, -1) << endl; + abort(); +} + +void stack_print (const char *file, int line, lua_State *L) { + cout << file << ":" << line << ": stack size: " << lua_gettop(L) << endl;; + for (int i = 1; i < lua_gettop(L) + 1; i++) { + cout << file << ":" << line << ": "; + cout << i << ": "; + if (lua_istable (L, i)) + cout << " table: " << lua_topointer (L, i) << endl; + else if (lua_isnumber (L, i)) + cout << " number: " << lua_tonumber (L, i) << endl; + else if (lua_isuserdata (L, i)) { + void* userdata = (void*) lua_touserdata (L, i); + cout << " userdata (" << userdata << ")" << endl; + } else if (lua_isstring (L, i)) + cout << " string: " << lua_tostring(L, i) << endl; + else if (lua_isfunction (L, i)) + cout << " function" << endl; + else if (lua_isnil (L, i)) + cout << " nil" << endl; + else + cout << " unknown: " << lua_typename (L, lua_type (L, i)) << endl; + } +} + +void l_push_LuaKey (lua_State *L, const LuaKey &key) { + if (key.type == LuaKey::Integer) + lua_pushnumber (L, key.int_value); + else + lua_pushstring(L, key.string_value.c_str()); +} + +bool query_key_stack (lua_State *L, std::vector key_stack) { + for (int i = key_stack.size() - 1; i >= 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + return false; + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_gettable (L, -2); + + // return if key is not found + if (lua_isnil(L, -1)) { + return false; + } + } + + return true; +} + +void create_key_stack (lua_State *L, std::vector key_stack) { + for (int i = key_stack.size() - 1; i > 0; i--) { + // get the global value when the result of a lua expression was not + // pushed onto the stack via the return statement. + if (lua_gettop(L) == 0) { + lua_getglobal (L, key_stack[key_stack.size() - 1].string_value.c_str()); + + if (lua_isnil(L, -1)) { + lua_pop(L, 1); + lua_newtable(L); + lua_pushvalue(L, -1); + lua_setglobal(L, key_stack[key_stack.size() - 1].string_value.c_str()); + } + + continue; + } + + l_push_LuaKey (L, key_stack[i]); + + lua_pushvalue (L, -1); + lua_gettable (L, -3); + + if (lua_isnil(L, -1)) { + // parent, key, nil + lua_pop(L, 1); // parent, key + lua_newtable(L); // parent, key, table + lua_insert(L, -2); // parent, table, key + lua_pushvalue(L, -2); // parent, table, key, table + lua_settable (L, -4); // parent, table + } + } +} + +// +// LuaTableNode +// +std::vector LuaTableNode::getKeyStack() { + std::vector result; + + const LuaTableNode *node_ptr = this; + + do { + result.push_back (node_ptr->key); + node_ptr = node_ptr->parent; + } while (node_ptr != NULL); + + return result; +} + +std::string LuaTableNode::keyStackToString() { + std::vector key_stack = getKeyStack(); + + ostringstream result_stream (""); + for (int i = key_stack.size() - 1; i >= 0; i--) { + if (key_stack[i].type == LuaKey::String) + result_stream << "[\"" << key_stack[i].string_value << "\"]"; + else + result_stream << "[" << key_stack[i].int_value << "]"; + } + + return result_stream.str(); +} + +bool LuaTableNode::stackQueryValue() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + return query_key_stack (L, key_stack); +} + +void LuaTableNode::stackCreateValue() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + create_key_stack (L, key_stack); +} + +LuaTable LuaTableNode::stackQueryTable() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + if (!query_key_stack (L, key_stack)) { + std::cerr << "Error: could not query table " << key << "." << std::endl; + abort(); + } + + return LuaTable::fromLuaState (L); +} + +LuaTable LuaTableNode::stackCreateLuaTable() { + luaTable->pushRef(); + + lua_State *L = luaTable->L; + stackTop = lua_gettop(L); + + std::vector key_stack = getKeyStack(); + + create_key_stack (L, key_stack); + + // create new table for the CustomType + lua_newtable(luaTable->L); // parent, CustomTable + // add table of CustomType to the parent + stackPushKey(); // parent, CustomTable, key + lua_pushvalue(luaTable->L, -2); // parent, CustomTable, key, CustomTable + lua_settable(luaTable->L, -4); + + LuaTable result; + result.luaStateRef = luaTable->luaStateRef->acquire(); + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + lua_pop (luaTable->luaStateRef->L, 2); + + return result; +} + +void LuaTableNode::stackPushKey() { + l_push_LuaKey (luaTable->L, key); +} + +void LuaTableNode::stackRestore() { + lua_pop (luaTable->L, lua_gettop(luaTable->L) - stackTop); + + luaTable->popRef(); +} + +bool LuaTableNode::exists() { + bool result = true; + + if (!stackQueryValue()) + result = false; + + stackRestore(); + + return result; +} + +void LuaTableNode::remove() { + if (stackQueryValue()) { + lua_pop(luaTable->L, 1); + + if (lua_gettop(luaTable->L) != 0) { + l_push_LuaKey (luaTable->L, key); + lua_pushnil (luaTable->L); + lua_settable (luaTable->L, -3); + } else { + lua_pushnil (luaTable->L); + lua_setglobal (luaTable->L, key.string_value.c_str()); + } + } + + stackRestore(); +} + +size_t LuaTableNode::length() { + size_t result = 0; + + if (stackQueryValue()) { +#if LUA_VERSION_NUM == 501 + result = lua_objlen(luaTable->L, -1); +#elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(luaTable->L, -1); +#endif + } + + stackRestore(); + + return result; +} + +std::vector LuaTableNode::keys() { + std::vector result; + + if (stackQueryValue()) { + // loop over all keys + lua_pushnil(luaTable->L); + while (lua_next(luaTable->L, -2) != 0) { + if (lua_isnumber(luaTable->L, -2)) { + double number = lua_tonumber (luaTable->L, -2); + double frac; + if (modf (number, &frac) == 0) { + LuaKey key (static_cast(number)); + result.push_back (key); + } + } else if (lua_isstring (luaTable->L, -2)) { + LuaKey key (lua_tostring(luaTable->L, -2)); + result.push_back (key); + } else { + cerr << "Warning: invalid LuaKey type for key " << lua_typename(luaTable->L, lua_type(luaTable->L, -2)) << "!" << endl; + } + + lua_pop(luaTable->L, 1); + } + } + + stackRestore(); + + return result; +} + + +template<> bool LuaTableNode::getDefault(const bool &default_value) { + bool result = default_value; + + if (stackQueryValue()) { + result = lua_toboolean (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> float LuaTableNode::getDefault(const float &default_value) { + float result = default_value; + + if (stackQueryValue()) { + result = static_cast(lua_tonumber (luaTable->L, -1)); + } + + stackRestore(); + + return result; +} + +template<> double LuaTableNode::getDefault(const double &default_value) { + double result = default_value; + + if (stackQueryValue()) { + result = lua_tonumber (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> std::string LuaTableNode::getDefault(const std::string &default_value) { + std::string result = default_value; + + if (stackQueryValue() && lua_isstring(luaTable->L, -1)) { + result = lua_tostring (luaTable->L, -1); + } + + stackRestore(); + + return result; +} + +template<> void LuaTableNode::set(const bool &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushboolean(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const float &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, static_cast(value)); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const double &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushnumber(luaTable->L, value); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +template<> void LuaTableNode::set(const std::string &value) { + stackCreateValue(); + + l_push_LuaKey (luaTable->L, key); + lua_pushstring(luaTable->L, value.c_str()); + // stack: parent, key, value + lua_settable (luaTable->L, -3); + + stackRestore(); +} + +// +// LuaTable +// +LuaTable::LuaTable (const LuaTable &other) : + filename (other.filename), + referencesGlobal (other.referencesGlobal) { + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } +} + +LuaTable& LuaTable::operator= (const LuaTable &other) { + if (&other != this) { + if (luaStateRef) { + // cleanup any existing reference + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + + // if this is the last, delete the Lua state + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } + + filename = other.filename; + luaStateRef = other.luaStateRef; + referencesGlobal = other.referencesGlobal; + + if (other.luaStateRef) { + luaStateRef = other.luaStateRef->acquire(); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, other.luaRef); + luaRef = luaL_ref (luaStateRef->L, LUA_REGISTRYINDEX); + } + } + } + + return *this; +} + +LuaTable::~LuaTable() { + if (luaRef != -1) { + luaL_unref (luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + + if (luaStateRef) { + int ref_count = luaStateRef->release(); + + if (ref_count == 0) { + if (luaStateRef->freeOnZeroRefs) { + lua_close (luaStateRef->L); + } + delete luaStateRef; + luaStateRef = NULL; + } + } +} + +int LuaTable::length() { + pushRef(); + + if ((lua_gettop(L) == 0) || (lua_type (L, -1) != LUA_TTABLE)) { + cerr << "Error: cannot query table length. No table on stack!" << endl; + abort(); + } + size_t result = 0; + +#if LUA_VERSION_NUM == 501 + result = lua_objlen(L, -1); +#elif LUA_VERSION_NUM >= 502 + result = lua_rawlen(L, -1); +#endif + + popRef(); + + return result; +} + +void LuaTable::pushRef() { + assert (luaStateRef); + assert (luaStateRef->L); + + if (!referencesGlobal) { + lua_rawgeti(luaStateRef->L, LUA_REGISTRYINDEX, luaRef); + } + L = luaStateRef->L; +} + +void LuaTable::popRef() { + if (!referencesGlobal) { + lua_pop (luaStateRef->L, 1); + } + L = NULL; +} + +LuaTable LuaTable::fromFile (const char* _filename) { + LuaTable result; + + result.filename = _filename; + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); + + // Add the directory of _filename to package.path + result.addSearchPath(get_file_directory (_filename).c_str()); + + // run the file we + if (luaL_dofile (result.luaStateRef->L, _filename)) { + bail (result.luaStateRef->L, "Error running file: "); + } + + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + return result; +} + +LuaTable LuaTable::fromLuaExpression (const char* lua_expr) { + LuaTable result; + + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = luaL_newstate(); + result.luaStateRef->count = 1; + luaL_openlibs (result.luaStateRef->L); + + if (luaL_loadstring (result.luaStateRef->L, lua_expr)) { + bail (result.luaStateRef->L, "Error compiling expression!"); + } + + if (lua_pcall (result.luaStateRef->L, 0, LUA_MULTRET, 0)) { + bail (result.luaStateRef->L, "Error running expression!"); + } + + if (lua_gettop(result.luaStateRef->L) != 0) { + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + } else { + result.referencesGlobal = true; + } + + return result; +} + +LuaTable LuaTable::fromLuaState (lua_State* L) { + LuaTable result; + + result.luaStateRef = new LuaStateRef(); + result.luaStateRef->L = L; + result.luaStateRef->count = 1; + result.luaStateRef->freeOnZeroRefs = false; + + lua_pushvalue (result.luaStateRef->L, -1); + result.luaRef = luaL_ref (result.luaStateRef->L, LUA_REGISTRYINDEX); + + return result; +} + +void LuaTable::addSearchPath(const char* path) { + if (luaStateRef->L == NULL) { + cerr << "Error: Cannot add search path: Lua state is not initialized!" << endl; + abort(); + } + + lua_getglobal(luaStateRef->L, "package"); + lua_getfield (luaStateRef->L, -1, "path"); + if (lua_type(luaStateRef->L, -1) != LUA_TSTRING) { + cerr << "Error: could not get package.path!" << endl; + abort(); + } + + string package_path = lua_tostring (luaStateRef->L, -1); + package_path = package_path + string (";") + string(path) + "?.lua;"; + + lua_pushstring(luaStateRef->L, package_path.c_str()); + lua_setfield (luaStateRef->L, -3, "path"); + + lua_pop(luaStateRef->L, 2); +} + +std::string LuaTable::serialize() { + pushRef(); + + std::string result; + + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } + + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } + + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + if (lua_pcall (L, 1, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + cerr << "Cannot serialize global Lua state!" << endl; + abort(); + } + + lua_pop (L, lua_gettop(L) - current_top); + + popRef(); + + return result; +} + +std::string LuaTable::orderedSerialize() { + pushRef(); + + std::string result; + + int current_top = lua_gettop(L); + if (lua_gettop(L) != 0) { + if (luaL_loadstring(L, serialize_lua)) { + bail (L, "Error loading serialization function: "); + } + + if (lua_pcall(L, 0, 0, 0)) { + bail (L, "Error compiling serialization function: " ); + } + + lua_getglobal (L, "serialize"); + assert (lua_isfunction (L, -1)); + lua_pushvalue (L, -2); + lua_pushstring (L, ""); + lua_pushboolean (L, true); + if (lua_pcall (L, 3, 1, 0)) { + bail (L, "Error while serializing: "); + } + result = string("return ") + lua_tostring (L, -1); + } else { + cerr << "Cannot serialize global Lua state!" << endl; + abort(); + } + + lua_pop (L, lua_gettop(L) - current_top); + + popRef(); + + return result; +} diff --git a/3rdparty/rbdl/addons/luamodel/luatables.h b/3rdparty/rbdl/addons/luamodel/luatables.h new file mode 100644 index 0000000..4571dd6 --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/luatables.h @@ -0,0 +1,261 @@ +/* + * LuaTables++ + * Copyright (c) 2013-2018 Martin Felis . + * All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef LUATABLES_H +#define LUATABLES_H + +#include +#include +#include +#include +#include + +#include + +// Forward declaration for Lua +extern "C" { +struct lua_State; +} + +struct RBDL_DLLAPI LuaKey { + enum Type { + String, + Integer + }; + + Type type; + int int_value; + std::string string_value; + + bool operator<( const LuaKey& rhs ) const { + if (type == String && rhs.type == Integer) { + return false; + } else if (type == Integer && rhs.type == String) { + return true; + } else if (type == Integer && rhs.type == Integer) { + return int_value < rhs.int_value; + } + + return string_value < rhs.string_value; + } + + LuaKey (const char* key_value) : + type (String), + int_value (0), + string_value (key_value) { } + LuaKey (int key_value) : + type (Integer), + int_value (key_value), + string_value ("") {} +}; + +inline std::ostream& operator<<(std::ostream& output, const LuaKey &key) { + if (key.type == LuaKey::Integer) + output << key.int_value << "(int)"; + else + output << key.string_value << "(string)"; + return output; +} +struct RBDL_DLLAPI LuaTable; +struct RBDL_DLLAPI LuaTableNode; + +struct RBDL_DLLAPI LuaTableNode { + LuaTableNode() : + parent (NULL), + luaTable (NULL), + key("") + { } + LuaTableNode operator[](const char *child_str) { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_str); + + return child_node; + } + LuaTableNode operator[](int child_index) { + LuaTableNode child_node; + + child_node.luaTable = luaTable; + child_node.parent = this; + child_node.key = LuaKey (child_index); + + return child_node; + } + bool stackQueryValue(); + void stackPushKey(); + void stackCreateValue(); + void stackRestore(); + LuaTable stackQueryTable(); + LuaTable stackCreateLuaTable(); + + std::vector getKeyStack(); + std::string keyStackToString(); + + bool exists(); + void remove(); + size_t length(); + std::vector keys(); + + // Templates for setters and getters. Can be specialized for custom + // types. + template + void set (const T &value); + template + T getDefault (const T &default_value); + + template + T get() { + if (!exists()) { + std::cerr << "Error: could not find value " << keyStackToString() << "." << std::endl; + abort(); + } + return getDefault (T()); + } + + // convenience operators (assignment, conversion, comparison) + template + void operator=(const T &value) { + set(value); + } + template + operator T() { + return get(); + } + template + bool operator==(T value) { + return value == get(); + } + template + bool operator!=(T value) { + return value != get(); + } + + LuaTableNode *parent; + LuaTable *luaTable; + LuaKey key; + int stackTop; +}; + +template +bool operator==(T value, LuaTableNode node) { + return value == (T) node; +} +template +bool operator!=(T value, LuaTableNode node) { + return value != (T) node; +} + +template<> bool LuaTableNode::getDefault(const bool &default_value); +template<> double LuaTableNode::getDefault(const double &default_value); +template<> float LuaTableNode::getDefault(const float &default_value); +template<> std::string LuaTableNode::getDefault(const std::string &default_value); + +template<> void LuaTableNode::set(const bool &value); +template<> void LuaTableNode::set(const float &value); +template<> void LuaTableNode::set(const double &value); +template<> void LuaTableNode::set(const std::string &value); + +/// Reference counting Lua state +struct RBDL_DLLAPI LuaStateRef { + LuaStateRef () : + L (NULL), + count (0), + freeOnZeroRefs(true) + {} + + LuaStateRef* acquire() { + count = count + 1; + return this; + } + + int release() { + count = count - 1; + return count; + } + + lua_State *L; + unsigned int count; + bool freeOnZeroRefs; +}; + +struct RBDL_DLLAPI LuaTable { + LuaTable () : + filename (""), + luaStateRef (NULL), + luaRef(-1), + L (NULL), + referencesGlobal (false) + {} + LuaTable (const LuaTable &other); + LuaTable& operator= (const LuaTable &other); + ~LuaTable(); + + LuaTableNode operator[] (const char* key) { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + LuaTableNode operator[] (int key) { + LuaTableNode root_node; + root_node.key = LuaKey (key); + root_node.parent = NULL; + root_node.luaTable = this; + + return root_node; + } + int length(); + void addSearchPath (const char* path); + std::string serialize (); + + /// Serializes the data in a predictable ordering. + std::string orderedSerialize (); + + /// Pushes the Lua table onto the stack of the internal Lua state. + // I.e. makes the Lua table active that is associated with this + // LuaTable. + void pushRef(); + /// Pops the Lua table from the stack of the internal Lua state. + // Cleans up a previous pushRef() + void popRef(); + + static LuaTable fromFile (const char *_filename); + static LuaTable fromLuaExpression (const char* lua_expr); + static LuaTable fromLuaState (lua_State *L); + + std::string filename; + LuaStateRef *luaStateRef; + int luaRef; + lua_State *L; + + bool referencesGlobal; +}; + +/* LUATABLES_H */ +#endif diff --git a/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc b/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc new file mode 100644 index 0000000..3551afc --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/rbdl_luamodel_util.cc @@ -0,0 +1,124 @@ +#include "rbdl/rbdl.h" +#include "rbdl/rbdl_utils.h" +#include "luamodel.h" + +#include +#include +#include + +using namespace std; + +using namespace RigidBodyDynamics::Math; + +void usage (const char* argv_0) { + cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] " << endl; + cerr << " -v | --verbose enable additional output" << endl; + cerr << " -d | --dof-overview print an overview of the degress of freedom" << endl; + cerr << " -m | --model-hierarchy print the hierarchy of the model" << endl; + cerr << " -o | --body-origins print the origins of all bodies that have names" << endl; + cerr << " -c | --center_of_mass print center of mass for bodies and full model" << endl; + cerr << " -s | --constraint_sets print all constraint sets defined in the model file" << endl; + cerr << " -h | --help print this help" << endl; + exit (1); +} + + +int main (int argc, char *argv[]) { + if (argc < 2) { + cerr << "Error: not enough arguments!" << endl; + usage(argv[0]); + } + + bool verbose = false; + bool dof_overview = false; + bool model_hierarchy = false; + bool body_origins = false; + bool center_of_mass = false; + bool constraint_sets = false; + + string filename = argv[1]; + + for (int i = 1; i < argc; i++) { + if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose") + verbose = true; + else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview") + dof_overview = true; + else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy") + model_hierarchy = true; + else if (string(argv[i]) == "-o" || string (argv[i]) == "--body-origins") + body_origins = true; + else if (string(argv[i]) == "-c" || string (argv[i]) == "--center-of-mass") + center_of_mass = true; + else if (string(argv[i]) == "-s" || string (argv[i]) == "--constraint-sets") + constraint_sets = true; + else if (string(argv[i]) == "-h" || string (argv[i]) == "--help") + usage(argv[0]); + else + filename = argv[i]; + } + + RigidBodyDynamics::Model model; + bool result; + + if (constraint_sets) { + std::vector constraint_set_names = + RigidBodyDynamics::Addons::LuaModelGetConstraintSetNames(filename.c_str()); + std::vector constraint_sets( + constraint_set_names.size(), RigidBodyDynamics::ConstraintSet()); + result = RigidBodyDynamics::Addons::LuaModelReadFromFileWithConstraints( + filename.c_str(), + &model, + constraint_sets, + constraint_set_names, + verbose + ); + } else { + result = RigidBodyDynamics::Addons::LuaModelReadFromFile( + filename.c_str(), &model, verbose); + } + + if (!result) { + cerr << "Loading of lua model failed!" << endl; + return -1; + } + + cout << "Model loading successful!" << endl; + + if (dof_overview) { + cout << "Degree of freedom overview:" << endl; + cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model); + } + + if (model_hierarchy) { + cout << "Model Hierarchy:" << endl; + cout << RigidBodyDynamics::Utils::GetModelHierarchy (model); + } + + if (body_origins) { + cout << "Body Origins:" << endl; + cout << RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(model); + } + + if (center_of_mass) { + VectorNd q_zero (VectorNd::Zero (model.q_size)); + VectorNd qdot_zero (VectorNd::Zero (model.qdot_size)); + RigidBodyDynamics::UpdateKinematics (model, q_zero, qdot_zero, qdot_zero); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.mBodies[i].mIsVirtual) + continue; + + SpatialRigidBodyInertia rbi_base = model.X_base[i].apply(model.I[i]); + Vector3d body_com = rbi_base.h / rbi_base.m; + cout << setw(12) << model.GetBodyName (i) << ": " << setw(10) << body_com.transpose() << endl; + } + + Vector3d model_com; + double mass; + RigidBodyDynamics::Utils::CalcCenterOfMass (model, q_zero, qdot_zero, NULL, mass, model_com); + cout << setw(14) << "Model COM: " << setw(10) << model_com.transpose() << endl; + cout << setw(14) << "Model mass: " << mass << endl; + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua b/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua new file mode 100644 index 0000000..b93186e --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/sampleconstrainedmodel.lua @@ -0,0 +1,267 @@ +-- 5b3d.lua +-- Copyright (c) 2016 Davide Corradi + +-- Parameters + +m1 = 2 +l1 = 2 +r1 = 0.2 +Izz1 = m1 * l1 * l1 / 3 + +m2 = 2 +l2 = 2 +r2 = 0.2 +Izz2 = m2 * l2 * l2 / 3 + +bodies = { + + virtual = { + mass = 0, + com = {0, 0, 0}, + inertia = { + {0, 0, 0}, + {0, 0, 0}, + {0, 0, 0}, + }, + }, + + link1 = { + mass = m1, + com = {l1/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz1}, + }, + }, + + link2 = { + mass = m2, + com = {l2/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz2}, + }, + }, + +} + +joints = { + + revZ = { + {0, 0, 1, 0, 0, 0}, + }, + + trnXYZ = { + {0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + }, + +} + +meshes = { + link1 = { + name = 'link1', + dimensions = {l1, r1, r1}, + color = {1, 0, 0}, + src = 'unit_cylinder_medres_z.obj', + mesh_center = {l1/2, 0, 0}, + }, + link2 = { + name = 'link2', + dimensions = {l2, r2, r2}, + color = {0, 1, 0}, + src = 'unit_cylinder_medres_z.obj', + mesh_center = {l2/2, 0, 0}, + }, +} + +model = { + + gravity = {0, 0, 0}, + + configuration = { + axis_front = { 1., 0., 0.}, + axis_right = { 0., -1., 0.}, + axis_up = { 0., 0., 1.}, + }, + + frames = { + + { + name = 'base', + parent = 'ROOT', + body = bodies.virtual, + joint = joints.trnXYZ, + }, + { + name = 'l11', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + visuals = { meshes.link1 }, + }, + { + name = 'l12', + parent = 'l11', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + visuals = { meshes.link2 }, + }, + { + name = 'l21', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + visuals = { meshes.link1 }, + }, + { + name = 'l22', + parent = 'l21', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + visuals = { meshes.link2 }, + }, + + }, + + constraint_sets = { + loop_constraints = { + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + + all_constraints = { + { + constraint_type = 'contact', + body = 'base', + point = {0, 0, 0}, + normal = {1, 0, 0}, + name = 'baseTX', + normal_acceleration = 0, + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 1, 0}, + name = 'baseTY', + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 0, 1}, + name = 'baseTZ', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_coefficient = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_coefficient = 0.1, + name = 'linkTY', + }, + }, + }, + +} + +return model \ No newline at end of file diff --git a/3rdparty/rbdl/addons/luamodel/samplemodel.lua b/3rdparty/rbdl/addons/luamodel/samplemodel.lua new file mode 100644 index 0000000..d2d501a --- /dev/null +++ b/3rdparty/rbdl/addons/luamodel/samplemodel.lua @@ -0,0 +1,87 @@ +inertia = { + {1.1, 0.1, 0.2}, + {0.3, 1.2, 0.4}, + {0.5, 0.6, 1.3} +} + +pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } +thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia } +shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } +foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } + +bodies = { + pelvis = pelvis, + thigh_right = thigh, + shank_right = shank, + thigh_left = thigh, + shank_left = shank +} + +joints = { + freeflyer = { + { 0., 0., 0., 1., 0., 0.}, + { 0., 0., 0., 0., 1., 0.}, + { 0., 0., 0., 0., 0., 1.}, + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + spherical_zyx = { + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + rotational_y = { + { 0., 1., 0., 0., 0., 0.} + }, + fixed = {} +} + +model = { + frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.spherical_zyx, + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.thigh_right, + joint = joints.fixed + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.spherical_zyx + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.thigh_left, + joint = joints.rotational_y + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.thigh_left, + joint = joints.fixed + }, + } +} + +return model diff --git a/3rdparty/rbdl/addons/muscle/CMakeLists.txt b/3rdparty/rbdl/addons/muscle/CMakeLists.txt new file mode 100755 index 0000000..3d5335b --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/CMakeLists.txt @@ -0,0 +1,139 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +#CMAKE_POLICY(SET CMP0048 NEW) +#CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_VERSION_MAJOR 2 ) +SET ( RBDL_ADDON_MUSCLE_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_MUSCLE_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_MUSCLE_VERSION + ${RBDL_ADDON_MUSCLE_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_VERSION_PATCH} +) + + + +PROJECT (RBDL_ADDON_MUSCLE VERSION ${RBDL_ADDON_MUSCLE_VERSION}) +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + FIND_PACKAGE (IPOPT REQUIRED) + INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ${IPOPT_INCLUDE_DIR} + ) + SET(MUSCLE_SOURCES + muscle.h + Millard2016TorqueMuscle.h + Millard2016TorqueMuscle.cc + TorqueMuscleFittingToolkit.h + TorqueMuscleFittingToolkit.cc + MuscleFunctionFactory.h + MuscleFunctionFactory.cc + TorqueMuscleFunctionFactory.h + TorqueMuscleFunctionFactory.cc + csvtools.h + csvtools.cc + ) + + SET(MUSCLE_HEADERS + muscle.h + Millard2016TorqueMuscle.h + TorqueMuscleFittingToolkit.h + MuscleFunctionFactory.h + TorqueMuscleFunctionFactory.h + csvtools.h + ) + + +ELSE(RBDL_BUILD_ADDON_MUSCLE_FITTING) + INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ) + + SET(MUSCLE_SOURCES + muscle.h + Millard2016TorqueMuscle.h + Millard2016TorqueMuscle.cc + MuscleFunctionFactory.h + MuscleFunctionFactory.cc + TorqueMuscleFunctionFactory.h + TorqueMuscleFunctionFactory.cc + csvtools.h + csvtools.cc + ) + + SET(MUSCLE_HEADERS + muscle.h + Millard2016TorqueMuscle.h + MuscleFunctionFactory.h + TorqueMuscleFunctionFactory.h + csvtools.h + ) + +ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + + +IF (RBDL_BUILD_STATIC) + + ADD_LIBRARY ( rbdl_muscle-static STATIC ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES PREFIX "lib") + SET_TARGET_PROPERTIES ( rbdl_muscle-static PROPERTIES OUTPUT_NAME "rbdl_muscle") + + IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES (rbdl_muscle-static + rbdl_geometry-static + rbdl-static + ${IPOPT_LIBRARY}) + ELSE(RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES (rbdl_muscle-static + rbdl_geometry-static + rbdl-static) + ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + INSTALL (TARGETS rbdl_muscle-static + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ELSE (RBDL_BUILD_STATIC) + + ADD_LIBRARY ( rbdl_muscle SHARED ${MUSCLE_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_muscle PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES ( + rbdl_muscle + rbdl_geometry + rbdl + ${IPOPT_LIBRARY} + ) + ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) + TARGET_LINK_LIBRARIES ( + rbdl_muscle + rbdl_geometry + rbdl + ) + ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + INSTALL (TARGETS rbdl_muscle + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) + +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" +) + +INSTALL ( FILES ${MUSCLE_HEADERS} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/muscle +) diff --git a/3rdparty/rbdl/addons/muscle/LICENSE b/3rdparty/rbdl/addons/muscle/LICENSE new file mode 100755 index 0000000..a25f3e9 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/LICENSE @@ -0,0 +1,23 @@ +Rigid Body Dynamics Library Muscle Addon - +Copyright (c) 2016 Matthew Millard + +(zlib license) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt b/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt new file mode 100755 index 0000000..d645695 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/LICENSE_APACHE-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc new file mode 100755 index 0000000..9478eaa --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.cc @@ -0,0 +1,2477 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : muscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "Millard2016TorqueMuscle.h" +#include "TorqueMuscleFunctionFactory.h" +#include "csvtools.h" + + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + + +static double EPSILON = std::numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); + + +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + +const double Millard2016TorqueMuscle::mTaLambdaMax = 1.0; +const double Millard2016TorqueMuscle::mTpLambdaMax = 0.0; +//static double mTvLambdaMax = 1.0; + + +/************************************************************* + Table Access Structure Names +*************************************************************/ +const double Gravity = 9.81; //Needed for the strength scaling used + //by Anderson et al. And this value has to + //be equal to the gravity parameter used by + //Anderson et al.: it should not be changed if + //the force of gravity differs in the model! + +const char* DataSet::names[] = { "Anderson2007", + "Gymnast"}; + +const char* GenderSet::names[] = {"Male", + "Female"}; + +const char* AgeGroupSet::names[] = {"Young18To25", + "Middle55To65", + "SeniorOver65"}; + +const char* JointTorqueSet::names[] = { "HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension" , + "AnkleFlexion" , + "ElbowExtension" , + "ElbowFlexion" , + "ShoulderExtension" , + "ShoulderFlexion" , + "WristExtension" , + "WristFlexion" , + "ShoulderHorizontalAdduction", + "ShoulderHorizontalAbduction", + "ShoulderInternalRotation" , + "ShoulderExternalRotation" , + "WristUlnarDeviation" , + "WristRadialDeviation" , + "WristPronation" , + "WristSupination" , + "LumbarExtension" , + "LumbarFlexion" }; + +const char* Anderson2007::GenderNames[] = {"Male","Female"}; + +const char* Anderson2007::AgeGroupNames[] = { "Young18To25", + "Middle55To65", + "SeniorOver65"}; + +const char* Anderson2007::JointTorqueNames[] = {"HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension", + "AnkleFlexion" }; + +const char* Gymnast::GenderNames[] = {"Male"}; +const char* Gymnast::AgeGroupNames[] = {"Young18To25"}; +const char* Gymnast::JointTorqueNames[] = + { "HipExtension" , + "HipFlexion" , + "KneeExtension" , + "KneeFlexion" , + "AnkleExtension" , + "AnkleFlexion" , + "ElbowExtension" , + "ElbowFlexion" , + "ShoulderExtension" , + "ShoulderFlexion" , + "WristExtension" , + "WristFlexion" , + "ShoulderHorizontalAdduction", + "ShoulderHorizontalAbduction", + "ShoulderInternalRotation" , + "ShoulderExternalRotation" , + "WristUlnarDeviation" , + "WristRadialDeviation" , + "WristPronation" , + "WristSupination" , + "LumbarExtension" , + "LumbarFlexion"}; + +/************************************************************* + Coefficient Tables +*************************************************************/ + +/* +This data is taken from Table 3 of + +Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). +Maximum voluntary joint torque as a function of joint angle +and angular velocity: model development and application to +the lower limb. Journal of biomechanics, 40(14), 3105-3113. + +Each row contains the coefficients for the active and +passive torque characteristics for a specific joint, +direction, gender and age group. Each row corresponds +to a single block taken from Table 3, as read from +left to right top to bottom. The first 4 columns have +been added to describe the joint, direction, gender +and age group. + +Column labels: +Parameter Set Meta Data + 0: joint: hip0_knee1_ankle2, + 1: direction: ext0_flex1, + 2: gender: male0_female1, + 3: age: age18to25_0_55to65_1_g65_2, + +Active Torque-Angle and Torque-Velocity Curves + 4: c1, + 5: c2, + 6: c3, + 7: c4, + 8: c5, + 9: c6, +Passive Torque-Angle Curves + 10: b1, + 11: k1, + 12: b2, + 13: k2, +*/ +double const Millard2016TorqueMuscle::Anderson2007Table3Mean[36][14] = { +{0,0,0,0,0.161,0.958,0.932,1.578,3.19,0.242,-1.21,-6.351,0.476,5.91 }, +{0,0,1,0,0.181,0.697,1.242,1.567,3.164,0.164,-1.753,-6.358,0.239,3.872 }, +{0,0,0,1,0.171,0.922,1.176,1.601,3.236,0.32,-2.16,-8.073,0.108,4.593 }, +{0,0,1,1,0.14,0.83,1.241,1.444,2.919,0.317,-1.361,-7.128,0.013,6.479 }, +{0,0,0,2,0.144,0.896,1.125,1.561,3.152,0.477,-2.671,-7.85,0.092,5.192 }, +{0,0,1,2,0.138,0.707,1.542,1.613,3.256,0.36,-0.758,-7.545,0.018,6.061 }, +{0,1,0,0,0.113,0.738,-0.214,2.095,4.267,0.218,1.21,-6.351,-0.476,5.91 }, +{0,1,1,0,0.127,0.65,-0.35,2.136,4.349,0.156,1.753,-6.358,-0.239,3.872 }, +{0,1,0,1,0.107,0.712,-0.192,2.038,4.145,0.206,2.16,-8.073,-0.108,4.593 }, +{0,1,1,1,0.091,0.812,-0.196,2.145,4.366,0.186,1.361,-7.128,-0.013,6.479 }, +{0,1,0,2,0.101,0.762,-0.269,1.875,3.819,0.296,2.671,-7.85,-0.092,5.192 }, +{0,1,1,2,0.081,0.625,-0.422,2.084,4.245,0.196,0.758,-7.545,-0.018,6.061 }, +{1,0,0,0,0.163,1.258,1.133,1.517,3.952,0.095,0,0,-6.25,-4.521 }, +{1,0,1,0,0.159,1.187,1.274,1.393,3.623,0.173,0,0,-8.033,-5.25 }, +{1,0,0,1,0.156,1.225,1.173,1.518,3.954,0.266,0,0,-12.83,-5.127 }, +{1,0,1,1,0.128,1.286,1.141,1.332,3.469,0.233,0,0,-6.576,-4.466 }, +{1,0,0,2,0.137,1.31,1.067,1.141,3.152,0.386,0,0,-10.519,-5.662 }, +{1,0,1,2,0.124,1.347,1.14,1.066,2.855,0.464,0,0,-8.8,-6.763 }, +{1,1,0,0,0.087,0.869,0.522,2.008,5.233,0.304,0,0,6.25,-4.521 }, +{1,1,1,0,0.08,0.873,0.635,1.698,4.412,0.175,0,0,8.033,-5.25 }, +{1,1,0,1,0.081,0.986,0.523,1.83,4.777,0.23,0,0,12.83,-5.127 }, +{1,1,1,1,0.06,0.967,0.402,1.693,4.41,0.349,0,0,6.576,-4.466 }, +{1,1,0,2,0.069,0.838,0.437,1.718,4.476,0.414,0,0,10.519,-5.662 }, +{1,1,1,2,0.06,0.897,0.445,1.121,2.922,0.389,0,0,8.8,-6.763 }, +{2,0,0,0,0.095,1.391,0.408,0.987,3.558,0.295,-0.0005781,-5.819,0.967,6.09 }, +{2,0,1,0,0.104,1.399,0.424,0.862,3.109,0.189,-0.005218,-4.875,0.47,6.425 }, +{2,0,0,1,0.114,1.444,0.551,0.593,2.128,0.35,-0.001311,-10.943,0.377,8.916 }, +{2,0,1,1,0.093,1.504,0.381,0.86,3.126,0.349,-2.888e-05,-17.189,0.523,7.888 }, +{2,0,0,2,0.106,1.465,0.498,0.49,1.767,0.571,-5.693e-05,-21.088,0.488,7.309 }, +{2,0,1,2,0.125,1.299,0.58,0.587,1.819,0.348,-2.35e-05,-12.567,0.331,6.629 }, +{2,1,0,0,0.033,1.51,-0.187,0.699,1.94,0.828,0.0005781,-5.819,-0.967,6.09 }, +{2,1,1,0,0.027,1.079,-0.302,0.864,2.399,0.771,0.005218,-4.875,-0.47,6.425 }, +{2,1,0,1,0.028,1.293,-0.284,0.634,1.759,0.999,0.001311,-10.943,-0.377,8.916 }, +{2,1,1,1,0.024,1.308,-0.254,0.596,1.654,1.006,2.888e-05,-17.189,-0.523,7.888}, +{2,1,0,2,0.029,1.419,-0.174,0.561,1.558,1.198,5.693e-05,-21.088,-0.488,7.309}, +{2,1,1,2,0.022,1.096,-0.369,0.458,1.242,1.401,2.35e-05,-12.567,-0.331,6.629 }}; + + +//See the description for the mean data. This table constains the +//parameter standard deviations +double const Millard2016TorqueMuscle::Anderson2007Table3Std[36][14] = { +{0,0,0,0,0.049,0.201,0.358,0.286,0.586,0.272,0.66,0.97,0.547,4.955 }, +{0,0,1,0,0.047,0.13,0.418,0.268,0.542,0.175,1.93,2.828,0.292,1.895 }, +{0,0,0,1,0.043,0.155,0.195,0.306,0.622,0.189,1.297,2.701,0.091,0.854 }, +{0,0,1,1,0.032,0.246,0.365,0.223,0.45,0.14,1.294,2.541,0.02,2.924 }, +{0,0,0,2,0.039,0.124,0.077,0.184,0.372,0.368,0.271,3.402,0.111,1.691 }, +{0,0,1,2,0.003,0.173,0.279,0.135,0.273,0.237,0.613,0.741,0.031,2.265 }, +{0,1,0,0,0.025,0.217,0.245,0.489,0.995,0.225,0.66,0.97,0.547,4.955 }, +{0,1,1,0,0.033,0.178,0.232,0.345,0.702,0.179,1.93,2.828,0.292,1.895 }, +{0,1,0,1,0.02,0.248,0.274,0.318,0.652,0.088,1.297,2.701,0.091,0.854 }, +{0,1,1,1,0.016,0.244,0.209,0.375,0.765,0.262,1.294,2.541,0.02,2.924 }, +{0,1,0,2,0.025,0.151,0.234,0.164,0.335,0.102,0.271,3.402,0.111,1.691 }, +{0,1,1,2,0.008,0.062,0.214,0.321,0.654,0.28,0.613,0.741,0.031,2.265 }, +{1,0,0,0,0.04,0.073,0.073,0.593,1.546,0.171,0,0,2.617,0.553 }, +{1,0,1,0,0.028,0.084,0.181,0.38,0.989,0.27,0,0,3.696,1.512 }, +{1,0,0,1,0.031,0.063,0.048,0.363,0.947,0.06,0,0,2.541,2.148 }, +{1,0,1,1,0.016,0.094,0.077,0.319,0.832,0.133,0,0,1.958,1.63 }, +{1,0,0,2,0.017,0.127,0.024,0.046,0.04,0.124,0,0,1.896,1.517 }, +{1,0,1,2,0.018,0.044,0.124,0.128,0.221,0.129,0,0,6.141,0.742 }, +{1,1,0,0,0.015,0.163,0.317,1.364,3.554,0.598,0,0,2.617,0.553 }, +{1,1,1,0,0.015,0.191,0.287,0.825,2.139,0.319,0,0,3.696,1.512 }, +{1,1,0,1,0.017,0.138,0.212,0.795,2.067,0.094,0,0,2.541,2.148 }, +{1,1,1,1,0.015,0.21,0.273,0.718,1.871,0.143,0,0,1.958,1.63 }, +{1,1,0,2,0.022,0.084,0.357,0.716,1.866,0.201,0,0,1.896,1.517 }, +{1,1,1,2,0.005,0.145,0.21,0.052,0.135,0.078,0,0,6.141,0.742 }, +{2,0,0,0,0.022,0.089,0.083,0.595,2.144,0.214,0.001193,7.384,0.323,1.196 }, +{2,0,1,0,0.034,0.19,0.186,0.487,1.76,0.213,0.01135,6.77,0.328,1.177 }, +{2,0,0,1,0.029,0.136,0.103,0.165,0.578,0.133,0.003331,10.291,0.403,3.119 }, +{2,0,1,1,0.026,0.235,0.143,0.448,1.613,0.27,3.562e-05,7.848,0.394,1.141 }, +{2,0,0,2,0.035,0.136,0.132,0.262,0.944,0.313,3.164e-05,1.786,0.258,0.902 }, +{2,0,1,2,0.006,0.095,0.115,0.258,0.423,0.158,2.535e-05,10.885,0.247,2.186}, +{2,1,0,0,0.005,0.19,0.067,0.108,0.301,0.134,0.001193,7.384,0.323,1.196 }, +{2,1,1,0,0.006,0.271,0.171,0.446,1.236,0.206,0.01135,6.77,0.328,1.177 }, +{2,1,0,1,0.005,0.479,0.178,0.216,0.601,0.214,0.003331,10.291,0.403,3.119 }, +{2,1,1,1,0.002,0.339,0.133,0.148,0.41,0.284,3.562e-05,7.848,0.394,1.141 }, +{2,1,0,2,0.002,0.195,0.056,0.188,0.521,0.29,3.164e-05,1.786,0.258,0.902 }, +{2,1,1,2,0.003,0.297,0.109,0.089,0.213,0.427,2.535e-05,10.885,0.247,2.186}}; + +/* + This table contains parameters for the newly made torque muscle curves: + + 1. maxIsometricTorque Nm + 2. maxAngularVelocity rad/s + 3. angleAtOneActiveNormTorque rad + 4. angularWidthActiveTorque rad + 5. tvAtMaxEccentricVelocity Nm/Nm + 6. tvAtHalfMaxConcentricVelocity Nm/Nm + 7. angleAtZeroPassiveTorque rad + 8. mAngleAtOneNormPassiveTorque rad + +*/ + + + +double const Millard2016TorqueMuscle::GymnastWholeBody[22][12] = +{ {0,0,0,0,175.746, 9.02335, 1.06465, 1.05941, 1.1, 0.163849, 0.79158, 1.5708 }, + {0,1,0,0,157.293, 9.18043, 0.733038, 1.21999, 1.11905, 0.25, -0.0888019,-0.515207 }, + {1,0,0,0,285.619, 19.2161, 0.942478, 0.509636, 1.13292, 0.115304, 2.00713, 2.70526 }, + {1,1,0,0,98.7579, 16.633, 1.02974, 1.11003, 1.12, 0.19746, 0, -0.174533 }, + {2,0,0,0,127.561, 11.7646, 0.408, 0.660752, 1.159, 0.410591, 0.0292126, 0.785398 }, + {2,1,0,0,44.3106, 17.2746, -0.187, 0.60868, 1.2656, 0.112303, -0.523599, -1.0472 }, + {3,0,0,0,127.401, 16.249, 2.14675, 1.83085, 1.1, 0.250134, 2.8291, 3.55835 }, + {3,1,0,0,91.1388, 19.0764, 0.890118, 1.2898, 1.23011, 0.249656, -0.523599, -1.0472 }, + {5,0,0,0,15.5653, 36.5472, 1.55334, 1.38928, 1.16875, 0.115356, 1.0472, 1.5708 }, + {5,1,0,0,39.2252, 36.3901, 0.663225, 1.71042, 1.14, 0.115456, -0.793739, -1.49714 }, + {3,2,0,0,128.496, 18.05, 0.839204, 1.28041, 1.25856, 0.214179, 1.5708, 2.26893 }, + {3,3,0,0,94.6839, 18 , -0.277611, 2.37086, 1.23042, 0.224227, -0.523599, -1.0472 }, + {3,5,0,0,50.522 , 19.47, -1.18761, 2.80524, 1.27634, 0.285399, 1.39626, 1.91986 }, + {3,4,0,0,43.5837, 18, -0.670796, 1.98361, 1.35664, 0.229104, -1.0472, -1.5708 }, + {4,1,0,0,101.384, 18.1, 0.33, 3.62155, 1.37223, 0.189909, 0, -0.174533 }, + {4,0,0,0,69.8728, 18.45, 1.64319, 1.30795, 1.31709, 0.189676, 2.0944, 2.61799 }, + {5,6,0,0,13.5361, 35.45, -0.209204, 1.33735, 1.23945, 0.250544, -0.785398, -1.5708 }, + {5,7,0,0,12.976 , 27.88, -0.212389, 0.991803, 1.3877, 0.207506, 0.785398, 1.5708 }, + {5,9,0,0,31.4217, 18.02, 0.43, 1.47849, 1.34817, 0.196913, 0, -0.523599}, + {5,8,0,0,23.8345, 21.77, -1.14319, 2.56082, 1.31466, 0.2092, 0.349066, 0.872665 }, + {6,0,0,0,687.864, 7.98695, 1.5506, 1.14543, 1.1, 0.150907, 0.306223, 1.35342 }, + {6,1,0,0,211.65 , 19.2310, 0, 6.28319, 1.1, 0.150907, 0, -0.785398 }}; + + +/* + Original lumbar parameters + {6,0,0,0,687.864, 1.0472, 1.5506, 1.14543, 1.1, 0.45, 0.306223, 1.35342 }, + {6,1,0,0,211.65 , 0.523599, 0, 6.28319, 1.1, 0.45, 0, -0.785398 }}; +*/ + +/************************************************************* + Map that goes from a single joint-torque-direction index to + the pair of joint and direction indicies used in the tables +*************************************************************/ + +const static struct JointSet{ + enum item { Hip = 0, + Knee, + Ankle, + Shoulder, + Elbow, + Wrist, + Lumbar, + Last}; + JointSet(){} +} JointSet; + + +struct DirectionSet{ + enum item { + Extension = 0, + Flexion, + HorizontalAdduction, + HorizontalAbduction, + ExternalRotation, + InternalRotation, + Supination, + Pronation, + RadialDeviation, + UlnarDeviation, + Last + }; + DirectionSet(){} +} DirectionSet; + + +const static int JointTorqueMap[22][3] = { + {(int)JointTorqueSet::HipExtension , (int)JointSet::Hip , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::HipFlexion , (int)JointSet::Hip , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::KneeExtension , (int)JointSet::Knee , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::KneeFlexion , (int)JointSet::Knee , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::AnkleExtension , (int)JointSet::Ankle , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::AnkleFlexion , (int)JointSet::Ankle , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ElbowExtension , (int)JointSet::Elbow , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ElbowFlexion , (int)JointSet::Elbow , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderExtension , (int)JointSet::Shoulder , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::ShoulderFlexion , (int)JointSet::Shoulder , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::WristExtension , (int)JointSet::Wrist , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::WristFlexion , (int)JointSet::Wrist , (int)DirectionSet::Flexion }, + {(int)JointTorqueSet::ShoulderHorizontalAdduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAdduction}, + {(int)JointTorqueSet::ShoulderHorizontalAbduction , (int)JointSet::Shoulder , (int)DirectionSet::HorizontalAbduction}, + {(int)JointTorqueSet::ShoulderInternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::InternalRotation }, + {(int)JointTorqueSet::ShoulderExternalRotation , (int)JointSet::Shoulder , (int)DirectionSet::ExternalRotation }, + {(int)JointTorqueSet::WristUlnarDeviation , (int)JointSet::Wrist , (int)DirectionSet::UlnarDeviation }, + {(int)JointTorqueSet::WristRadialDeviation , (int)JointSet::Wrist , (int)DirectionSet::RadialDeviation }, + {(int)JointTorqueSet::WristPronation , (int)JointSet::Wrist , (int)DirectionSet::Pronation }, + {(int)JointTorqueSet::WristSupination , (int)JointSet::Wrist , (int)DirectionSet::Supination }, + {(int)JointTorqueSet::LumbarExtension , (int)JointSet::Lumbar , (int)DirectionSet::Extension }, + {(int)JointTorqueSet::LumbarFlexion , (int)JointSet::Lumbar , (int)DirectionSet::Flexion }}; + + +/************************************************************* + Constructors +*************************************************************/ + +Millard2016TorqueMuscle:: + Millard2016TorqueMuscle( ) + :mAngleOffset(1.0), + mSignOfJointAngle(1.0), + mSignOfJointTorque(1.0), + mSignOfConcentricAnglularVelocity(1.0), + mMuscleName("empty") +{ + mMuscleCurvesAreDirty = true; + mUseTabularMaxActiveIsometricTorque = true; + mUseTabularOmegaMax = true; + mPassiveTorqueScale = 1.0; + + mTaLambda = 0.0; + mTpLambda = 0.0; + mTvLambda = 0.0; + mTvLambdaMax = 1.0; + mTaAngleScaling = 1.0; +} + +Millard2016TorqueMuscle::Millard2016TorqueMuscle( + DataSet::item dataSet, + const SubjectInformation &subjectInfo, + int jointTorque, + double jointAngleOffsetRelativeToDoxygenFigures, + double signOfJointAngleRelativeToDoxygenFigures, + double mSignOfJointTorque, + const std::string& name + ):mAngleOffset(jointAngleOffsetRelativeToDoxygenFigures), + mSignOfJointAngle(signOfJointAngleRelativeToDoxygenFigures), + mSignOfJointTorque(mSignOfJointTorque), + mSignOfConcentricAnglularVelocity(mSignOfJointTorque), + mMuscleName(name), + mDataSet(dataSet) +{ + + mSubjectHeightInMeters = subjectInfo.heightInMeters; + mSubjectMassInKg = subjectInfo.massInKg; + mPassiveCurveAngleOffset = 0.; + mPassiveTorqueScale = 1.0; + mBetaMax = 0.1; + + int gender = (int) subjectInfo.gender; + int ageGroup = (int) subjectInfo.ageGroup; + + int joint = -1; + int jointDirection = -1; + + for(int i=0; i < JointTorqueSet::Last; ++i){ + if(JointTorqueMap[i][0] == jointTorque){ + joint = JointTorqueMap[i][1]; + jointDirection = JointTorqueMap[i][2]; + } + } + + if(joint == -1 || jointDirection == -1){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": A jointTorqueDirection of " << jointTorque + << " does not exist."; + assert(0); + abort(); + } + + if( abs(abs(mSignOfJointAngle)-1) > EPSILON){ + + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": signOfJointAngleRelativeToAnderson2007 must be [-1,1] not " + << mSignOfJointAngle; + assert(0); + abort(); + } + + if( abs(abs(mSignOfConcentricAnglularVelocity)-1) > EPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": signOfJointAngularVelocityDuringConcentricContraction " + << "must be [-1,1] not " + << mSignOfConcentricAnglularVelocity; + assert(0); + abort(); + } + + if( abs(abs(mSignOfJointTorque)-1) > EPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSignOfJointTorque must be [-1,1] not " + << mSignOfJointTorque; + assert(0); + abort(); + } + + + if(mSubjectHeightInMeters <= 0){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSubjectHeightInMeters > 0, but it's " + << mSubjectHeightInMeters; + assert(0); + abort(); + } + + if(mSubjectMassInKg <= 0){ + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << ": mSubjectMassInKg > 0, but it's " + << mSubjectMassInKg; + assert(0); + abort(); + } + + + + int idx = -1; + int jointIdx = 0; + int dirIdx = 1; + int genderIdx = 2; + int ageIdx = 3; + + switch(mDataSet){ + case DataSet::Anderson2007: + { + mAnderson2007c1c2c3c4c5c6.resize(6); + mAnderson2007b1k1b2k2.resize(4); + + for(int i=0; i<36;++i){ + + if( abs(Anderson2007Table3Mean[i][jointIdx] + -(double)joint) < EPSILON + && abs(Anderson2007Table3Mean[i][dirIdx] + -(double)jointDirection) < EPSILON + && abs(Anderson2007Table3Mean[i][genderIdx] + -(double)gender) < EPSILON + && abs(Anderson2007Table3Mean[i][ageIdx] + -(double)ageGroup) < EPSILON){ + idx = i; + } + } + + if(idx != -1){ + for(int i=0; i<6; ++i){ + mAnderson2007c1c2c3c4c5c6[i] = Anderson2007Table3Mean[idx][i+4]; + } + for(int i=0; i<4; ++i){ + mAnderson2007b1k1b2k2[i] = Anderson2007Table3Mean[idx][i+10]; + } + } + + } break; + + case DataSet::Gymnast: + { + mGymnastParams.resize(8); + for(int i=0; i(this); + mutableThis->updateTorqueMuscleCurves(); + } + + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + updTorqueMuscleSummary(activation, + jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + mutableThis->mTmSummary); + + return mTmSummary.jointTorque; +} + + + +void Millard2016TorqueMuscle:: + calcActivation(double jointAngle, + double jointAngularVelocity, + double jointTorque, + TorqueMuscleSummary &tms) const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + updInvertTorqueMuscleSummary(jointTorque,jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, + mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + tms); +} + +double Millard2016TorqueMuscle:: + calcMaximumActiveIsometricTorqueScalingFactor( + double jointAngle, + double jointAngularVelocity, + double activation, + double jointTorque) const +{ + + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + double scaleFactor = 0.; + + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + + updTorqueMuscleSummary(activation, + jointAngle,jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, + mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + mutableThis->mTmSummary); + + scaleFactor = jointTorque/mTmSummary.jointTorque; + + return scaleFactor; +} + + +void Millard2016TorqueMuscle:: + calcTorqueMuscleInfo(double jointAngle, + double jointAngularVelocity, + double activation, + TorqueMuscleInfo& tmi) const +{ + + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + + updTorqueMuscleInfo(activation, jointAngle, jointAngularVelocity, + mTaLambda,mTpLambda,mTvLambda, + mTaAngleScaling, mAngleAtOneNormActiveTorque, + mPassiveCurveAngleOffset, + mOmegaMax, + mMaxActiveIsometricTorque, + tmi); +} + + +/************************************************************* + Get / Set Functions +*************************************************************/ + +double Millard2016TorqueMuscle:: + getJointTorqueSign() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mSignOfJointTorque; +} + +double Millard2016TorqueMuscle:: + getJointAngleSign() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mSignOfJointAngle; +} + +double Millard2016TorqueMuscle:: + getJointAngleOffset() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mAngleOffset; +} + + +double Millard2016TorqueMuscle:: + getNormalizedDampingCoefficient() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mBetaMax; +} + +void Millard2016TorqueMuscle:: + setNormalizedDampingCoefficient(double betaUpd) +{ + if(betaUpd < 0){ + cerr << "Millard2016TorqueMuscle::" + << "setNormalizedDampingCoefficient:" + << mMuscleName + << "mBetaMax is " << betaUpd + << " but mBetaMax must be > 0 " + << endl; + assert(0); + abort(); + } + + mBetaMax = betaUpd; +} + + + + +double Millard2016TorqueMuscle:: + getMaximumActiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + + /* + if(mUseTabularMaxActiveIsometricTorque){ + return mMaxActiveIsometricTorque; + }else{ + return mMaxActiveIsometricTorque*mMaxActiveIsometricMultiplerProduct; + } + */ + return mMaxActiveIsometricTorque; + +} + +double Millard2016TorqueMuscle:: + getMaximumConcentricJointAngularVelocity() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngularVelocity( mOmegaMax ); +} + +double Millard2016TorqueMuscle:: + getTorqueVelocityMultiplierAtHalfOmegaMax() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mTorqueVelocityMultiplierAtHalfOmegaMax; +} + + +void Millard2016TorqueMuscle:: + setTorqueVelocityMultiplierAtHalfOmegaMax(double tvAtHalfOmegaMax) +{ + if(mDataSet == DataSet::Anderson2007){ + cerr << "Millard2016TorqueMuscle::" + << "setTorqueVelocityMultiplierAtHalfOmegaMax:" + << mMuscleName + << " This function is only compatible with the Gymnast dataset" + << " but this muscle is from the Anderson2007 dataset. Switch" + << " data sets or stop using this function."; + assert(0); + abort(); + } + mMuscleCurvesAreDirty = true; + mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax = false; + mTorqueVelocityMultiplierAtHalfOmegaMax = tvAtHalfOmegaMax; +} + + +void Millard2016TorqueMuscle:: + setMaximumActiveIsometricTorque(double maxIsoTorque) +{ + mMuscleCurvesAreDirty = true; + mUseTabularMaxActiveIsometricTorque = false; + //mMaxActiveIsometricTorqueUserDefined = maxIsoTorque; + mMaxActiveIsometricTorque = maxIsoTorque; +} + +void Millard2016TorqueMuscle:: + setMaximumConcentricJointAngularVelocity(double maxAngularVelocity){ + if(fabs(maxAngularVelocity) < SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "setMaximumJointAngularVelocity:" + << mMuscleName + << " The value of maxJointAngularVelocity needs to be greater " + " than sqrt(epsilon), but it is " + << maxAngularVelocity; + assert(0); + abort(); + } + + mMuscleCurvesAreDirty = true; + mUseTabularOmegaMax = false; + mOmegaMax = fabs(maxAngularVelocity); +} + +double Millard2016TorqueMuscle:: + getJointAngleAtMaximumActiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngle(mAngleAtOneNormActiveTorque); +} + +double Millard2016TorqueMuscle:: + getActiveTorqueAngleCurveWidth() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + VectorNd domain = mTaCurve.getCurveDomain(); + double activeTorqueAngleAngleScaling + = getActiveTorqueAngleCurveAngleScaling(); + double width = fabs(domain[1]-domain[0])/activeTorqueAngleAngleScaling; + + return width; + +} + + +double Millard2016TorqueMuscle:: + getJointAngleAtOneNormalizedPassiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngle(mAngleAtOneNormPassiveTorque); +} + +double Millard2016TorqueMuscle:: + getJointAngleAtSmallestNormalizedPassiveIsometricTorque() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return calcJointAngle(mAngleAtSmallestNormPassiveTorque); +} + + + +double Millard2016TorqueMuscle:: + getPassiveTorqueScale() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mPassiveTorqueScale; +} + +void Millard2016TorqueMuscle:: + setPassiveTorqueScale(double passiveTorqueScaling) +{ + mMuscleCurvesAreDirty = true; + mPassiveTorqueScale = passiveTorqueScaling; +} + + +double Millard2016TorqueMuscle:: + getPassiveCurveAngleOffset() const +{ + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mPassiveCurveAngleOffset; +} + +void Millard2016TorqueMuscle:: + setPassiveCurveAngleOffset(double passiveCurveAngleOffsetVal) +{ + mMuscleCurvesAreDirty = true; + mPassiveCurveAngleOffset = passiveCurveAngleOffsetVal; +} + + +void Millard2016TorqueMuscle:: + fitPassiveCurveAngleOffset(double jointAngleTarget, + double passiveFiberTorqueTarget) +{ + mMuscleCurvesAreDirty = true; + setPassiveCurveAngleOffset(0.0); + updateTorqueMuscleCurves(); + + if(passiveFiberTorqueTarget < SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "fitPassiveTorqueScale:" + << mMuscleName + << ": passiveTorque " << passiveFiberTorqueTarget + << " but it should be greater than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + //Solve for the fiber angle at which the curve develops + //the desired passiveTorque + double normPassiveFiberTorque = passiveFiberTorqueTarget + /mMaxActiveIsometricTorque; + double currentFiberAngle = calcFiberAngleGivenNormalizedPassiveTorque( + normPassiveFiberTorque, + mTpLambda, + mPassiveCurveAngleOffset); //0.); + + //double fiberAngleOffset = mPassiveCurveAngleOffset + // +calcFiberAngle(jointAngleTarget) + // -currentFiberAngle; + double fiberAngleOffset = calcFiberAngle(jointAngleTarget) + -currentFiberAngle; + + setPassiveCurveAngleOffset(fiberAngleOffset); + updateTorqueMuscleCurves(); + +} + +void Millard2016TorqueMuscle:: + fitPassiveTorqueScale(double jointAngleTarget, + double passiveFiberTorqueTarget) +{ + mMuscleCurvesAreDirty = true; + setPassiveTorqueScale(1.0); + updateTorqueMuscleCurves(); + + double normPassiveFiberTorqueTarget = passiveFiberTorqueTarget + /mMaxActiveIsometricTorque; + double fiberAngle = calcFiberAngle(jointAngleTarget); + double normPassiveFiberTorqueCurrent= + calcBlendedCurveDerivative(fiberAngle-mPassiveCurveAngleOffset, + mTpLambda, mTpLambdaMax, + 0,0, + mTpCurve); + + double passiveTorqueScale = normPassiveFiberTorqueTarget + /normPassiveFiberTorqueCurrent; + + + setPassiveTorqueScale(passiveTorqueScale); + updateTorqueMuscleCurves(); +} + +void Millard2016TorqueMuscle::calcTorqueMuscleDataFeatures( + RigidBodyDynamics::Math::VectorNd const &jointTorque, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleDataFeatures &tmf) const +{ + TorqueMuscleSummary tmsCache; + double activation; + double tp; + double minActivation = 1.0; + double maxActivation = 0.; + double maxTp = 0.; + + tmf.isInactive = true; + + for(unsigned int i=0; i 0){ + tmf.isInactive=false; + if(activation <= minActivation){ + minActivation = activation; + tmf.indexOfMinActivation = i; + tmf.summaryAtMinActivation = tmsCache; + } + if(activation >= maxActivation){ + tmf.indexOfMaxActivation = i; + tmf.summaryAtMaxActivation = tmsCache; + maxActivation = activation; + } + } + if(tmsCache.fiberPassiveTorqueAngleMultiplier >= maxTp){ + maxTp = tmsCache.fiberPassiveTorqueAngleMultiplier; + tmf.summaryAtMaxPassiveTorqueAngleMultiplier = tmsCache; + tmf.indexOfMaxPassiveTorqueAngleMultiplier = i; + } + + } + +} + + + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: + getActiveTorqueAngleCurve() const +{ + //This must be updated if the parameters have changed + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mTaCurve; +} + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: +getPassiveTorqueAngleCurve() const +{ + //This must be updated if the parameters have changed + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mTpCurve; +} + +const SmoothSegmentedFunction& Millard2016TorqueMuscle:: +getTorqueAngularVelocityCurve() const +{ + //This must be updated if the parameters have changed + if(mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableThis = + const_cast(this); + mutableThis->updateTorqueMuscleCurves(); + } + return mTvCurve; +} + + + +string Millard2016TorqueMuscle::getName(){ + return mMuscleName; +} + +void Millard2016TorqueMuscle::setName(string &name){ + mMuscleCurvesAreDirty = true; + mMuscleName = name; +} + + +double Millard2016TorqueMuscle:: + getActiveTorqueAngleCurveBlendingVariable() const +{ + return mTaLambda; +} + +double Millard2016TorqueMuscle:: + getPassiveTorqueAngleCurveBlendingVariable() const +{ + return mTpLambda; +} + +double Millard2016TorqueMuscle:: + getTorqueAngularVelocityCurveBlendingVariable() const +{ + return mTvLambda; +} + +double Millard2016TorqueMuscle::getActiveTorqueAngleCurveAngleScaling() const +{ + return mTaAngleScaling; +} + +void Millard2016TorqueMuscle:: + setActiveTorqueAngleCurveAngleScaling(double angleScaling) +{ + if(angleScaling < SQRTEPSILON){ + cerr << "Millard2016TorqueMuscle::" + << "setActiveTorqueAngleCurveAngleScaling:" + << mMuscleName + << ": angleScaling must be > sqrt(eps), this " + << angleScaling + <<" is outside the acceptable range." + << endl; + assert(0); + abort(); + } + + mMuscleCurvesAreDirty = true; + mTaAngleScaling = angleScaling; +} + +void Millard2016TorqueMuscle:: + setActiveTorqueAngleCurveBlendingVariable(double blendingVariable) +{ + if(blendingVariable < 0. || blendingVariable > 1.0){ + cerr << "Millard2016TorqueMuscle::" + << "setActiveTorqueAngleCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + assert(0); + abort(); + } + mMuscleCurvesAreDirty = true; + mTaLambda = blendingVariable; +} + +void Millard2016TorqueMuscle:: + setPassiveTorqueAngleCurveBlendingVariable(double blendingVariable) +{ + if(blendingVariable < 0. || blendingVariable > 1.0){ + cerr << "Millard2016TorqueMuscle::" + << "setPassiveTorqueAngleCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + assert(0); + abort(); + } + mMuscleCurvesAreDirty = true; + mTpLambda = blendingVariable; +} + +void Millard2016TorqueMuscle:: + setTorqueAngularVelocityCurveBlendingVariable(double blendingVariable) +{ + + if(blendingVariable < 0. || blendingVariable > 1.0){ + cerr << "Millard2016TorqueMuscle::" + << "setTorqueAngularVelocityCurveBlendingVariable:" + << mMuscleName + << ": blending variable must be [0,1] and this " + << blendingVariable + << " is outside the acceptable range." + << endl; + assert(0); + abort(); + } + mMuscleCurvesAreDirty = true; + mTvLambda = blendingVariable; +} + +void Millard2016TorqueMuscle::setFittedParameters( + const TorqueMuscleParameterFittingData &fittedParameters) +{ + + if(!fittedParameters.fittingConverged){ + cerr << "Millard2016TorqueMuscle::" + << "setTorqueMuscleParameters:" + << mMuscleName + << ": The fittingConverged field of fittedParameters is false! " + << endl; + assert(0); + abort(); + } + + setPassiveTorqueAngleCurveBlendingVariable( + fittedParameters.passiveTorqueAngleBlendingVariable); + setActiveTorqueAngleCurveBlendingVariable( + fittedParameters.activeTorqueAngleBlendingVariable); + setTorqueAngularVelocityCurveBlendingVariable( + fittedParameters.torqueVelocityBlendingVariable); + setPassiveCurveAngleOffset( + fittedParameters.passiveTorqueAngleCurveOffset); + setMaximumActiveIsometricTorque( + fittedParameters.maximumActiveIsometricTorque); + setMaximumConcentricJointAngularVelocity( + fittedParameters.maximumAngularVelocity); + setActiveTorqueAngleCurveAngleScaling( + fittedParameters.activeTorqueAngleAngleScaling); + +} + +/************************************************************* + Utilities +*************************************************************/ +//fa = signJointAngle*(ja - jaO) +// ja = signJointAngle*fa + ja0 +//dja = signJointAngle*dfa + +double Millard2016TorqueMuscle:: + calcFiberAngle(double jointAngle) const +{ + return mSignOfJointAngle*(jointAngle-mAngleOffset); +} + +double Millard2016TorqueMuscle:: + calcJointAngle(double fiberAngle) const +{ + return fiberAngle*mSignOfJointAngle + mAngleOffset; +} + + +double Millard2016TorqueMuscle:: + calcFiberAngularVelocity(double jointAngularVelocity) const +{ + return mSignOfConcentricAnglularVelocity*jointAngularVelocity; +} + +double Millard2016TorqueMuscle:: + calcJointAngularVelocity(double fiberAngularVelocity) const +{ + return mSignOfConcentricAnglularVelocity*fiberAngularVelocity; +} + +void Millard2016TorqueMuscle::updateTorqueMuscleCurves() +{ + std::string tempName = mMuscleName; + + switch(mDataSet){ + case DataSet::Anderson2007: + { + double c4 = mAnderson2007c1c2c3c4c5c6[3]; + double c5 = mAnderson2007c1c2c3c4c5c6[4]; + + if(mUseTabularOmegaMax){ + mOmegaMax = abs( 2.0*c4*c5/(c5-3.0*c4) ); + } + + mScaleFactorAnderson2007 = mSubjectHeightInMeters + *mSubjectMassInKg + *Gravity; + + if(mUseTabularMaxActiveIsometricTorque){ + mMaxActiveIsometricTorque = mScaleFactorAnderson2007 + *mAnderson2007c1c2c3c4c5c6[0]; + } + + mAngleAtOneNormActiveTorque = mAnderson2007c1c2c3c4c5c6[2]; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueAngleCurve( + mAnderson2007c1c2c3c4c5c6[1], + mAnderson2007c1c2c3c4c5c6[2], + tempName.append("_taCurve"), + mTaCurve); + + tempName = mMuscleName; + + TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueVelocityCurve( + mAnderson2007c1c2c3c4c5c6[3], + mAnderson2007c1c2c3c4c5c6[4], + mAnderson2007c1c2c3c4c5c6[5], + 1.1, + 1.4, + tempName.append("_tvCurve"), + mTvCurve); + + mTorqueVelocityMultiplierAtHalfOmegaMax = mTvCurve.calcValue(0.5); + + tempName = mMuscleName; + + double normMaxActiveIsometricTorque = mMaxActiveIsometricTorque + /mScaleFactorAnderson2007; + + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + mScaleFactorAnderson2007, + normMaxActiveIsometricTorque, + mAnderson2007b1k1b2k2[0], + mAnderson2007b1k1b2k2[1], + mAnderson2007b1k1b2k2[2], + mAnderson2007b1k1b2k2[3], + tempName.append("_tpCurve"), + mTpCurve); + + //mTpCurve.shift(mPassiveCurveAngleOffset,0); + mTpCurve.scale(1.0,mPassiveTorqueScale); + + double k = 0; + double b = 0; + + if(mAnderson2007b1k1b2k2[0] > 0){ + b = mAnderson2007b1k1b2k2[0]; + k = mAnderson2007b1k1b2k2[1]; + }else if(mAnderson2007b1k1b2k2[2] > 0){ + b = mAnderson2007b1k1b2k2[2]; + k = mAnderson2007b1k1b2k2[3]; + } + + + VectorNd xDomain = mTpCurve.getCurveDomain(); + double tpAtX0 = mTpCurve.calcValue(xDomain[0]); + double tpAtX1 = mTpCurve.calcValue(xDomain[1]); + + if(fabs(tpAtX0) < SQRTEPSILON){ + mAngleAtSmallestNormPassiveTorque = xDomain[0]; + }else if(fabs(tpAtX1) < SQRTEPSILON){ + mAngleAtSmallestNormPassiveTorque = xDomain[1]; + }else{ + mAngleAtSmallestNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + + if( fabs(tpAtX0) > SQRTEPSILON + || fabs(tpAtX1) > SQRTEPSILON){ + double argGuess = (1/k)*log(abs(mMaxActiveIsometricTorque/b)); + // + mPassiveCurveAngleOffset; + mAngleAtOneNormPassiveTorque = calcInverseBlendedCurveValue(1.0, + argGuess, + mTpLambda, + mTpLambdaMax, + mTpCurve); + mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset; + }else{ + mAngleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + + + //mAngleAtOneNormPassiveTorque + //mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque] + + + } break; + case DataSet::Gymnast: + { + if(mUseTabularOmegaMax){ + mOmegaMax = mGymnastParams[ + Gymnast::OmegaMax]; + } + if(mUseTabularMaxActiveIsometricTorque){ + mMaxActiveIsometricTorque = mGymnastParams[ + Gymnast::TauMax]; + } + mAngleAtOneNormActiveTorque = mGymnastParams[ + Gymnast::ActiveAngleAtOneNormTorque]; + + TorqueMuscleFunctionFactory:: + createGaussianShapedActiveTorqueAngleCurve( + mGymnastParams[Gymnast::ActiveAngleAtOneNormTorque], + mGymnastParams[Gymnast::ActiveAngularStandardDeviation], + tempName.append("_taCurve"), + mTaCurve); + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + mGymnastParams[Gymnast::PassiveAngleAtZeroTorque], + mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque], + tempName.append("_tpCurve"), + mTpCurve); + + //mTpCurve.shift(mPassiveCurveAngleOffset,0); + mTpCurve.scale(1.0,mPassiveTorqueScale); + + + + if(mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax){ + mTorqueVelocityMultiplierAtHalfOmegaMax + = mGymnastParams[Gymnast::TvAtHalfMaxConcentricVelocity]; + } + + TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + mGymnastParams[Gymnast::TvAtMaxEccentricVelocity], + mTorqueVelocityMultiplierAtHalfOmegaMax, + tempName.append("_tvCurve"), + mTvCurve); + + + //If the passive curve is non-zero, recalculate the angle at which + //it hits one normalized force. + mAngleAtSmallestNormPassiveTorque = + mGymnastParams[Gymnast::PassiveAngleAtZeroTorque] + +mPassiveCurveAngleOffset; + + //Verify that the curve hits zero at this angle + if(fabs(mTpCurve.calcValue(mAngleAtSmallestNormPassiveTorque)) + >SQRTEPSILON){ + mAngleAtSmallestNormPassiveTorque = + numeric_limits::signaling_NaN(); + } + + VectorNd xDomain = mTpCurve.getCurveDomain(); + double tpAtX0 = mTpCurve.calcValue(xDomain[0]); + double tpAtX1 = mTpCurve.calcValue(xDomain[1]); + + if( fabs(tpAtX0) > SQRTEPSILON + || fabs(tpAtX1) > SQRTEPSILON){ + double argGuess =mGymnastParams[Gymnast::PassiveAngleAtOneNormTorque]; + //+ mPassiveCurveAngleOffset; + mAngleAtOneNormPassiveTorque = + calcInverseBlendedCurveValue(1.0,argGuess,mTpLambda,mTpLambdaMax, + mTpCurve); + mAngleAtOneNormPassiveTorque += mPassiveCurveAngleOffset; + }else{ + mAngleAtOneNormPassiveTorque = + std::numeric_limits::signaling_NaN(); + } + } break; + default: + { + cerr << "Millard2016TorqueMuscle::" + << "Millard2016TorqueMuscle:" + << mMuscleName + << "mDataSet has a value of " << mDataSet + << " which is not a valid choice"; + assert(0); + abort(); + } + }; + + VectorNd tvDomain = mTvCurve.getCurveDomain(); + mTvLambdaMax = mTvCurve.calcValue(tvDomain[0]); + assert(mTvLambdaMax >= 1.0); //If this fails you've gotten the incorrect + //domain end. + + /* + double taIsoMax = calcBlendedCurveDerivative(mAngleAtOneNormActiveTorque, + mTaLambda, mTaLambdaMax, + 0,0, mTaCurve); + double tvIsoMax = calcBlendedCurveDerivative(0, + mTvLambda, mTvLambdaMax, + 0,0, mTvCurve); + mMaxActiveIsometricMultiplerProduct = taIsoMax*tvIsoMax; + + if(mUseTabularMaxActiveIsometricTorque == false){ + mMaxActiveIsometricTorque = mMaxActiveIsometricTorqueUserDefined + /mMaxActiveIsometricMultiplerProduct; + } + */ + mMuscleCurvesAreDirty = false; +} + + + +void Millard2016TorqueMuscle::printJointTorqueProfileToFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + int numberOfSamplePoints) +{ + if(mMuscleCurvesAreDirty){ + updateTorqueMuscleCurves(); + } + + VectorNd activeDomain = mTaCurve.getCurveDomain(); + VectorNd passiveDomain = mTpCurve.getCurveDomain(); + VectorNd velocityDomain= mTvCurve.getCurveDomain(); + + double jointMin = calcJointAngle( activeDomain[0] ); + double jointMax = calcJointAngle( activeDomain[1] ); + + if(mTpCurve.calcValue( activeDomain[0] ) >= 0.99){ + jointMin = calcJointAngle( passiveDomain[0] ); + } + + if(mTpCurve.calcValue(activeDomain[1]) >= 0.99){ + jointMax = calcJointAngle( passiveDomain[1] ); + } + + + + if(jointMin > jointMax){ + double tmp = jointMin; + jointMin=jointMax; + jointMax=tmp; + } + double range = jointMax-jointMin; + jointMin = jointMin -range*0.5; + jointMax = jointMax +range*0.5; + double jointDelta = (jointMax-jointMin) + /((double)numberOfSamplePoints-1.); + + double velMin = calcJointAngularVelocity( -mOmegaMax ); + double velMax = calcJointAngularVelocity( mOmegaMax ); + + if(velMin > velMax){ + double tmp = velMin; + velMin = velMax; + velMax = tmp; + } + double velRange = velMax-velMin; + velMin = velMin-0.5*velRange; + velMax = velMax+0.5*velRange; + double velDelta = (velMax-velMin)/((double)numberOfSamplePoints-1.0); + + double angleAtMaxIsoTorque = mAngleAtOneNormActiveTorque; + + std::vector< std::vector < double > > matrix; + std::vector < double > row(21); + std::string header("jointAngle," + "jointVelocity," + "activation," + "fiberAngle," + "fiberAngularVelocity," + "passiveTorqueAngleMultiplier," + "activeTorqueAngleMultiplier," + "torqueVelocityMultiplier," + "activeTorque," + "passiveTorque," + "fiberTorque," + "jointTorque," + "fiberStiffness," + "jointStiffness," + "fiberActivePower," + "fiberPassivePower," + "fiberPower," + "jointPower," + "DjointTorqueDactivation," + "DjointTorqueDjointAngularVelocity," + "DjointTorqueDjointAngle"); + + double activation =1.0; + double jointAngle = 0.; + double jointVelocity = 0.; + + + for(int i=0; i::signaling_NaN(); + VectorNd domain = mTpCurve.getCurveDomain(); + double y0 = mTpCurve.calcValue(domain[0]); + double y1 = mTpCurve.calcValue(domain[1]); + + if( (blendingVariable < 1) && (y0 > SQRTEPSILON || y1 > SQRTEPSILON) ){ + double tp = normPassiveFiberTorque/(1-blendingVariable); + angle = mTpCurve.calcInverseValue(tp,0.)-passiveTorqueAngleCurveOffset; + } + + return angle; +} + + +//============================================================================= +//============================================================================= + +void Millard2016TorqueMuscle:: + updTorqueMuscleSummaryCurveValues(double fiberAngle, + double normFiberAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + TorqueMuscleSummary &updTms) const +{ + + double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque) + /activeTorqueAngleAngleScaling + + activeTorqueAngleAtOneNormTorque; + + updTms.fiberActiveTorqueAngleMultiplier = + calcBlendedCurveDerivative(taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,0, + mTaCurve); + + updTms.fiberPassiveTorqueAngleMultiplier = + calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,0, + mTpCurve); + + updTms.fiberTorqueAngularVelocityMultiplier = + calcBlendedCurveDerivative( + normFiberAngularVelocity, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,0, + mTvCurve); + + updTms.fiberNormalizedDampingTorque = updTms.fiberPassiveTorqueAngleMultiplier + * (-normFiberAngularVelocity*mBetaMax); + +} + +void Millard2016TorqueMuscle:: + updTorqueMuscleInfo( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleInfo &updTmi) const +{ + + //Update state quantities + updTmi.activation = activation; + updTmi.jointAngle = jointAngle; + updTmi.jointAngularVelocity = jointAngularVelocity; + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberAngularVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double omegaNorm = fiberAngularVelocity/maxAngularVelocity; + double D_wn_D_w = 1.0/maxAngularVelocity; + double D_wn_D_wmax = -fiberAngularVelocity + /(maxAngularVelocity*maxAngularVelocity); + + double D2_wn_D_wmax2 = -2.0*D_wn_D_wmax/maxAngularVelocity; + + double taAngle = (fiberAngle-activeTorqueAngleAtOneNormTorque) + /activeTorqueAngleAngleScaling + + activeTorqueAngleAtOneNormTorque; + + double D_taAngle_D_fiberAngle = 1.0/activeTorqueAngleAngleScaling; + + double D_taAngle_D_angleScaling = + -(fiberAngle-activeTorqueAngleAtOneNormTorque) + /(activeTorqueAngleAngleScaling*activeTorqueAngleAngleScaling); + + double D2_taAngle_D_angleScaling2 = -2.0*D_taAngle_D_angleScaling + /activeTorqueAngleAngleScaling; + + //Update force component values + double ta = + calcBlendedCurveDerivative(taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,0, + mTaCurve); + + double tp = + calcBlendedCurveDerivative(fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,0, + mTpCurve); + + double tv = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,0, + mTvCurve); + + double tb = tp * (-omegaNorm*mBetaMax); + + double D_tb_D_omegaMax = tp*(-D_wn_D_wmax*mBetaMax); + + + //Update force component derivative values; + //1st derivatives w.r.t fiber angle/velocity + double D_ta_D_taAngle = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 1,0, + mTaCurve); + + double D_ta_D_angleScaling = D_ta_D_taAngle*D_taAngle_D_angleScaling; + + double D_ta_D_fiberAngle = D_ta_D_taAngle*D_taAngle_D_fiberAngle; + + double D_tp_D_fiberAngle = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 1,0, + mTpCurve); + + double D_tp_D_fiberAngleOffset= -D_tp_D_fiberAngle; + + double D_tv_D_wn = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 1,0, + mTvCurve); + + + + double D_tv_D_fiberAngularVelocity = D_tv_D_wn*D_wn_D_w; + double D_tv_D_omegaMax = D_tv_D_wn*D_wn_D_wmax; + + + //1st derivatives w.r.t fitting-related variables + double D_ta_D_taLambda = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,1, + mTaCurve); + + double D_tp_D_tpLambda = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,1, + mTpCurve); + double D_tv_D_tvLambda = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,1, + mTvCurve); + + //2nd derivatives w.r.t fitting-related variables. + double D2_ta_D_taLambda2 = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 0,2, + mTaCurve); + + double D2_tv_D_tvLambda2 = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 0,2, + mTvCurve); + double D2_tp_D_tpLambda2 = calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 0,2, + mTpCurve); + + + double D2_tp_D_fiberAngle2 =calcBlendedCurveDerivative( + fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 2,0, + mTpCurve); + + double D2_ta_D_taAngle2 = calcBlendedCurveDerivative( + taAngle, + activeTorqueAngleBlendingVariable, + mTaLambdaMax, + 2,0, + mTaCurve); + + double D2_ta_D_angleScaling2 = + D2_ta_D_taAngle2*D_taAngle_D_angleScaling*D_taAngle_D_angleScaling + + D_ta_D_taAngle*D2_taAngle_D_angleScaling2; + + double D2_tv_D_wn2 = calcBlendedCurveDerivative( + omegaNorm, + torqueAngularVelocityBlendingVariable, + mTvLambdaMax, + 2,0, + mTvCurve); + + double D2_tb_D_omegaMax2 = tp*(-D2_wn_D_wmax2*mBetaMax); + double D2_tb_D_omegaMax_D_tpLambda = D_tp_D_tpLambda*(-D_wn_D_wmax*mBetaMax); + double D2_tb_D_omegaMax_D_tpOffset = D_tp_D_fiberAngleOffset + *(-D_wn_D_wmax*mBetaMax); + double D2_tv_D_omegaMax2 = D2_tv_D_wn2*D_wn_D_wmax*D_wn_D_wmax + + D_tv_D_wn*D2_wn_D_wmax2; + + + + //Note that d/d_tpOffsetAngle kicks out another -1, so this 2nd derivative has + //a positive sign. + double D2_tp_D_fiberAngleOffset2= D2_tp_D_fiberAngle2; + + double D2_tp_D_tpLambda_D_fiberAngle = + calcBlendedCurveDerivative( fiberAngle-passiveTorqueAngleCurveOffset, + passiveTorqueAngleBlendingVariable, + mTpLambdaMax, + 1,1, + mTpCurve); + double D2_tp_D_tpLambda_D_fiberAngleOffset = -D2_tp_D_tpLambda_D_fiberAngle; + + //Damping derivatives + double D_tb_D_wn = tp * (-1.0*mBetaMax); + + double D_tb_D_tpLambda = -D_tp_D_tpLambda*mBetaMax*omegaNorm; + + + double D_tb_D_fiberAngularVelocity = D_tb_D_wn*D_wn_D_w; + + double D_tb_D_fiberAngle = -D_tp_D_fiberAngle*mBetaMax*omegaNorm; + + double D_tb_D_fiberAngleOffset= -D_tp_D_fiberAngleOffset*mBetaMax*omegaNorm; + + //Damping second derivatives + double D2_tb_D_tpLambda_D_fiberAngleOffset = + -D2_tp_D_tpLambda_D_fiberAngleOffset*mBetaMax*omegaNorm; + double D2_tb_D_tpLambda2 = + -D2_tp_D_tpLambda2*mBetaMax*omegaNorm; + double D2_tb_D_fiberAngleOffset2 = + -D2_tp_D_fiberAngleOffset2*mBetaMax*omegaNorm; + + //Sign conventions + double D_fiberAngle_D_jointAngle = mSignOfJointAngle; + double D_fiberAngularVelocity_D_jointAngularVelocity = + mSignOfConcentricAnglularVelocity; + + + updTmi.jointAngle = jointAngle; + updTmi.jointAngularVelocity = jointAngularVelocity; + updTmi.fiberAngle = fiberAngle; + updTmi.fiberAngularVelocity = fiberAngularVelocity; + + updTmi.fiberPassiveTorqueAngleMultiplier = tp; + updTmi.fiberActiveTorqueAngleMultiplier = ta; + updTmi.fiberTorqueAngularVelocityMultiplier = tv; + + updTmi.activation = activation; + updTmi.fiberActiveTorque = maxActIsoTorque*(activation*ta*tv); + updTmi.fiberPassiveTorque = maxActIsoTorque*(tp+tb); + updTmi.fiberPassiveElasticTorque = maxActIsoTorque*tp; + updTmi.fiberDampingTorque = maxActIsoTorque*tb; + updTmi.fiberNormDampingTorque = tb; + + updTmi.fiberTorque = updTmi.fiberActiveTorque + updTmi.fiberPassiveTorque; + updTmi.jointTorque = mSignOfJointTorque*updTmi.fiberTorque; + + updTmi.fiberStiffness = maxActIsoTorque*( + activation*D_ta_D_fiberAngle*tv + + D_tp_D_fiberAngle + + D_tb_D_fiberAngle); + + updTmi.jointStiffness = mSignOfJointTorque + *updTmi.fiberStiffness + *D_fiberAngle_D_jointAngle; + + updTmi.fiberActivePower = updTmi.fiberActiveTorque + * updTmi.fiberAngularVelocity; + + updTmi.fiberPassivePower = updTmi.fiberPassiveTorque + * updTmi.fiberAngularVelocity; + + updTmi.fiberPower = updTmi.fiberActivePower + + updTmi.fiberPassivePower; + + updTmi.jointPower = updTmi.jointTorque * jointAngularVelocity; + + + updTmi.DfiberPassiveTorqueAngleMultiplier_DblendingVariable + = D_tp_D_tpLambda; + updTmi.DfiberActiveTorqueAngleMultiplier_DblendingVariable + = D_ta_D_taLambda; + updTmi.DfiberTorqueAngularVelocityMultiplier_DblendingVariable + = D_tv_D_tvLambda; + updTmi.DfiberPassiveTorqueAngleMultiplier_DangleOffset + = D_tp_D_fiberAngleOffset; + + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + // Dtau_Da = tauMaxIso*(ta(theta) * tv(thetaDot) ) + updTmi.DjointTorque_Dactivation = + mSignOfJointTorque + *maxActIsoTorque + *(ta * tv); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_Dtheta = signTq*tauIso*(a*Dta_Dtheta(theta)*tv(thetaDot) + // + Dtp_Dtheta(theta)*(1-beta*omegaNorm) + updTmi.DjointTorque_DjointAngle = + mSignOfJointTorque + * maxActIsoTorque + * ( activation + *D_ta_D_fiberAngle + * tv + + ( D_tp_D_fiberAngle + + D_tb_D_fiberAngle) + )* D_fiberAngle_D_jointAngle; + + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_Domega = signTq*tauIso*(a * ta(theta) * Dtv_DthetaDot(thetaDot) + // - tp(theta)*beta*DomegaNorm_thetaDot + updTmi.DjointTorque_DjointAngularVelocity = + mSignOfJointTorque + * maxActIsoTorque + * ( activation + * ta + * ( D_tv_D_fiberAngularVelocity + *D_fiberAngularVelocity_D_jointAngularVelocity) + + ( D_tb_D_fiberAngularVelocity + *D_fiberAngularVelocity_D_jointAngularVelocity) + ); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_DtaLambda = signTq*tauIso*(a * Dta(theta)_Dlambda * tv(thetaDot) + updTmi.DjointTorque_DactiveTorqueAngleBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(activation*D_ta_D_taLambda*tv); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_DtpLambda = signTq*tauIso*( D_tp(theta)_D_lambda(1-beta*omegaNorm) + updTmi.DjointTorque_DpassiveTorqueAngleBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(D_tp_D_tpLambda + D_tb_D_tpLambda); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)*(1-beta*omegaNorm) + //Dtau_tvLambda = signTq*tauIso*(a*ta(theta)*Dtv_Dlambda(thetaDot) + updTmi.DjointTorque_DtorqueAngularVelocityBlendingVariable = + mSignOfJointTorque + * maxActIsoTorque + *(activation*ta*D_tv_D_tvLambda); + + // mSignOfJointTorque*( (maxActIsoTorque*(activation*ta*tv) + // + maxActIsoTorque*(tp+tb))) + updTmi.DjointTorque_DmaximumIsometricTorque = + mSignOfJointTorque + *((activation*ta*tv) + (tp+tb)); + + updTmi.DjointTorque_DpassiveTorqueAngleCurveAngleOffset = + mSignOfJointTorque + *maxActIsoTorque + *(D_tp_D_fiberAngleOffset + D_tb_D_fiberAngleOffset); + //* D_fiberAngle_D_jointAngle; + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega) + //Dtau_DomegaMax = signTq*tauIso*(a*ta(theta)*Dtv_DomegaMax(thetaDot) + // + tp(theta)*(-beta*D_omegaNorm_DomegaMax) + updTmi.DjointTorque_DmaximumAngularVelocity = + mSignOfJointTorque + *maxActIsoTorque + *(activation*ta*D_tv_D_omegaMax + D_tb_D_omegaMax); + + //tau = signTq*tauIso*(a*ta(theta)*tv(thetaDot) + tp(theta)+tb(theta,omega) + //Dtau_DtaAngleScaling = signTq*tauIso*(a*Dta(theta)_DangleScaling*tv(thetaDot) + + updTmi.DjointTorque_DactiveTorqueAngleAngleScaling = + mSignOfJointTorque + *maxActIsoTorque + *(activation*D_ta_D_angleScaling*tv); + + + /* + Second derivatives of the exposed variables, x, for fitting: + + Option 1: + x = [taLambda,tvLambda,tpLambda,tpOffset] + For brevity, the short forms will be used just in the comments below + x = [a, v, p, o] + For reference: + t(a,v,p,o) = mSignOfJointTorque*( + (maxActIsoTorque*( + activation*ta(a)*tv(v)) + (tp(p,o)+tb(p,o)))) + Where 't' is the shortform used to represent joint-torque. + + The lower right triangle for the Hessian of tau(a,v,p,o) w.r.t. x is + stored, row-wise, in the vector fittingInfo. Thus we have + + a v p o + H(tau(a,v,p,o),x) = a [ 0: d2t/da2 + v [ 1: d2t/dadv 2: d2t/dv2 + p [ 3: d2t/dadp 4: d2t/dvdp 5: d2t/dp2 + o [ 6: d2t/dado 7: d2t/dvdo 8: d2t/dpdo 9: d2t/do2 + The numbers indicate the index of the quantity in fittingInfo. + + Option 2: + x = [taAngleScaling, tvOmegaMax, tpLambda,tpOffset] + for brevity + x = [s, m, p, o] + + For reference: + t(s,m,p,o) = mSignOfJointTorque*( + (maxActIsoTorque*( + activation*ta(s)*tv(m)) + (tp(p,o)+tb(p,o)))) + Where 't' is the shortform used to represent joint-torque. + + The lower right triangle for the Hessian of tau(s,m,p,o) w.r.t. x is + stored, row-wise, in the vector fittingInfo. Thus we have + + s m p o + H(tau(s,m,p,o),x) = s [ 0: d2t/ds2 + m [ 1: d2t/dsdm 2: d2t/dm2 + p [ 3: d2t/dsdp 4: d2t/dmdp 5: d2t/dp2 + o [ 6: d2t/dsdo 7: d2t/dmdo 8: d2t/dpdo 9: d2t/do2 + The numbers indicate the index of the quantity in fittingInfo. + + So that both fitting options are possible, the extra entries for the + above Hessian will just be concatentated to the existing vector. Three + of the entries are duplicates, but for now they are being included + just to make this slightly easier to use for now. Thus the vector of + 2nd derivatives will look like: + + For joint-torque related constraints that use x = [a,v,p,o] + 0: d2t/da2 + 1: d2t/dadv + 2: d2t/dv2 + 3: d2t/dadp + 4: d2t/dvdp + 5: d2t/dp2 + 6: d2t/dado + 7: d2t/dvdo + 8: d2t/dpdo + 9: d2t/do2 + For constraints on the value of the passive element + 10: d2p/dp2 + 11: d2p/dpdo + 12: d2p/do2 + For joint-torque related constraints that use x = [s,m,p,o] + 13: d2t/ds2 + 14: d2t/dsdm + 15: d2t/dm2 + 16: d2t/dsdp + 17: d2t/dmdp + 18: d2t/dp2 + 19: d2t/dsdo + 20: d2t/dmdo + 21: d2t/dpdo + 22: d2t/do2 + + */ + +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + updTmi.fittingInfo.conservativeResize(23); + + //0: d2t/da2 + updTmi.fittingInfo[0] = mSignOfJointTorque*(maxActIsoTorque*( + activation*D2_ta_D_taLambda2*tv)); + + //1: d2t/dadv + updTmi.fittingInfo[1] = mSignOfJointTorque*(maxActIsoTorque* + (activation*D_ta_D_taLambda*D_tv_D_tvLambda)); + + //2: d2t/dv2 + updTmi.fittingInfo[2] = mSignOfJointTorque*(maxActIsoTorque* + (activation*ta*D2_tv_D_tvLambda2)); + + //3: d2t/dadp + updTmi.fittingInfo[3] = 0.; + + //4: d2t/dvdp + updTmi.fittingInfo[4] = 0.; + + //5: d2t/dp2 + updTmi.fittingInfo[5] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_tpLambda2+D2_tb_D_tpLambda2)); + //6: d2t/dado + updTmi.fittingInfo[6] = 0.; + //7: d2t/dvdo + updTmi.fittingInfo[7] = 0.; + //8: d2t/dpdo + updTmi.fittingInfo[8] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_tpLambda_D_fiberAngleOffset + +D2_tb_D_tpLambda_D_fiberAngleOffset + ));//*D_fiberAngle_D_jointAngle); + //9: d2t/do2 + updTmi.fittingInfo[9] = mSignOfJointTorque*(maxActIsoTorque*( + D2_tp_D_fiberAngleOffset2 + +D2_tb_D_fiberAngleOffset2 + ));//*D_fiberAngle_D_jointAngle); + //10: d2tp/dp2 + updTmi.fittingInfo[10] = D2_tp_D_tpLambda2; + //11: d2tp/dpdo + updTmi.fittingInfo[11] = D2_tp_D_tpLambda_D_fiberAngleOffset; + //12: d2tp/do2 + updTmi.fittingInfo[12] = D2_tp_D_fiberAngleOffset2; + + //13: d2t/ds2 + updTmi.fittingInfo[13] = mSignOfJointTorque + *maxActIsoTorque + *(activation*D2_ta_D_angleScaling2*tv); + //14: d2t/dsdm + updTmi.fittingInfo[14] = mSignOfJointTorque + *maxActIsoTorque + *(activation*D_ta_D_angleScaling*D_tv_D_omegaMax); + + //15: d2t/dm2 + updTmi.fittingInfo[15] = mSignOfJointTorque + *maxActIsoTorque + *(activation*ta*D2_tv_D_omegaMax2 + D2_tb_D_omegaMax2); + //16: d2t/dsdp + updTmi.fittingInfo[16] = 0.; + + //17: d2t/dmdp + updTmi.fittingInfo[17] = mSignOfJointTorque + *maxActIsoTorque + *(D2_tb_D_omegaMax_D_tpLambda); + //18: d2t/dp2 + updTmi.fittingInfo[18] = mSignOfJointTorque*( + maxActIsoTorque*( + D2_tp_D_tpLambda2+D2_tb_D_tpLambda2)); + //19: d2t/dsdo + updTmi.fittingInfo[19] = 0.; + + //20: d2t/dmdo + updTmi.fittingInfo[20] = mSignOfJointTorque + *maxActIsoTorque + *(D2_tb_D_omegaMax_D_tpOffset); + + //21: d2t/dpdo + updTmi.fittingInfo[21] = mSignOfJointTorque + *(maxActIsoTorque + *( D2_tp_D_tpLambda_D_fiberAngleOffset + +D2_tb_D_tpLambda_D_fiberAngleOffset)); + + //22: d2t/do2 + updTmi.fittingInfo[22] = mSignOfJointTorque + *(maxActIsoTorque + *( D2_tp_D_fiberAngleOffset2 + +D2_tb_D_fiberAngleOffset2)); + +#endif + +} + +//============================================================================= +void Millard2016TorqueMuscle:: + updTorqueMuscleSummary( double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleSummary &updTms) const +{ + double fiberAngle = calcFiberAngle(jointAngle); + double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double fiberVelocityNorm = fiberVelocity/maxAngularVelocity; + + + updTorqueMuscleSummaryCurveValues(fiberAngle, + fiberVelocityNorm, + activeTorqueAngleBlendingVariable, + passiveTorqueAngleBlendingVariable, + torqueAngularVelocityBlendingVariable, + activeTorqueAngleAngleScaling, + activeTorqueAngleAtOneNormTorque, + passiveTorqueAngleCurveOffset, + updTms); + updTms.fiberAngle = fiberAngle; + updTms.fiberAngularVelocity = fiberVelocity; + updTms.activation = activation; + updTms.fiberTorque = + maxActIsoTorque + *( activation*( updTms.fiberActiveTorqueAngleMultiplier + *updTms.fiberTorqueAngularVelocityMultiplier + ) + + ( updTms.fiberPassiveTorqueAngleMultiplier + + updTms.fiberNormalizedDampingTorque + ) + ); + + updTms.jointTorque = updTms.fiberTorque*mSignOfJointTorque; +} + +void Millard2016TorqueMuscle:: + updInvertTorqueMuscleSummary(double jointTorque, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActIsoTorque, + TorqueMuscleSummary &updTms) const +{ + + double fiberAngle = calcFiberAngle(jointAngle); + double fiberVelocity = calcFiberAngularVelocity(jointAngularVelocity); + double fiberVelocityNorm = fiberVelocity/maxAngularVelocity; + + updTorqueMuscleSummaryCurveValues(fiberAngle, + fiberVelocityNorm, + activeTorqueAngleBlendingVariable, + passiveTorqueAngleBlendingVariable, + torqueAngularVelocityBlendingVariable, + activeTorqueAngleAngleScaling, + activeTorqueAngleAtOneNormTorque, + passiveTorqueAngleCurveOffset, + updTms); + + updTms.fiberAngle = fiberAngle; + updTms.fiberAngularVelocity = fiberVelocity; + + updTms.jointTorque = jointTorque; + updTms.fiberTorque = jointTorque*mSignOfJointTorque; + + updTms.activation = + ( (updTms.fiberTorque/maxActIsoTorque) + - (updTms.fiberPassiveTorqueAngleMultiplier + + updTms.fiberNormalizedDampingTorque) + )/( updTms.fiberActiveTorqueAngleMultiplier + *updTms.fiberTorqueAngularVelocityMultiplier); +} + +//============================================================================== diff --git a/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.h b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.h new file mode 100755 index 0000000..0394f0b --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/Millard2016TorqueMuscle.h @@ -0,0 +1,1844 @@ +#ifndef MILLARD2016TORQUEMUSCLE_H_ +#define MILLARD2016TORQUEMUSCLE_H_ + +/* + * RBDL - Rigid Body Dynamics Library: Addon : muscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include "../geometry/SmoothSegmentedFunction.h" + + + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + #ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + class TorqueMuscleFittingToolkit; + class FitTorqueMuscleParameters; + #endif + + /** + This struct contains an enumerated list of the data sets which + constain torque muscles. Thus far this list includes + + -Anderson2007: Table 3 from Anderson et al. + -Gymnast: A set of torque muscles for the whole body (in progress) + + For details of these data sets please refer to the main description + of this class. + + */ + const static struct DataSet{ + enum item{ + Anderson2007 = 0, + Gymnast, + Last + }; + const static char* names[]; + DataSet(){} + } DataSet; + + + /** + This struct contains an enumerated list of the genders for which + data torque data has been reported. + */ + const static struct GenderSet{ + enum item { + Male = 0, + Female, + Last + }; + const static char* names[]; + GenderSet(){} + } GenderSet; + + /** + This struct contains an enumerated list of the age groups for which + data torque data has been reported. + */ + const static struct AgeGroupSet{ + enum item { + Young18To25 = 0, + Middle55To65, + SeniorOver65, + Last + }; + const static char* names[]; + AgeGroupSet(){} + } AgeGroupSet; + + /** + This struct contains an enumerated list of the joint-torque-directions + for which data torque data has been reported. + */ + const static struct JointTorqueSet{ + enum item{ + HipExtension = 0, + HipFlexion = 1, + KneeExtension = 2, + KneeFlexion = 3, + AnkleExtension = 4, + AnkleFlexion = 5, + ElbowExtension = 6, + ElbowFlexion = 7, + ShoulderExtension = 8, + ShoulderFlexion = 9, + WristExtension = 10, + WristFlexion = 11, + ShoulderHorizontalAdduction = 12, + ShoulderHorizontalAbduction = 13, + ShoulderInternalRotation = 14, + ShoulderExternalRotation = 15, + WristUlnarDeviation = 16, + WristRadialDeviation = 17, + WristPronation = 18, + WristSupination = 19, + LumbarExtension = 20, + LumbarFlexion = 21, + Last = 22 + }; + const static char* names[]; + JointTorqueSet(){} + } JointTorqueSet; + + + /** + This struct contains 3 enumerated lists (Gender, AgeGroup, + JointTorque) that lists the genders, age groups, and + joint-torque-directions for which the Anderson2007 data set contains + data. Please refer to the class description for more details about + this data set and how to use it. + */ + const static struct Anderson2007{ + enum Gender { + Male = GenderSet::Male, + Female = GenderSet::Female, + LastGender + }; + enum AgeGroup { + Young18To25 = AgeGroupSet::Young18To25, + Middle55To65 = AgeGroupSet::Middle55To65, + SeniorOver65 = AgeGroupSet::SeniorOver65, + LastAgeGroup + }; + enum JointTorque{ + HipExtension = JointTorqueSet::HipExtension , + HipFlexion = JointTorqueSet::HipFlexion , + KneeExtension = JointTorqueSet::KneeExtension , + KneeFlexion = JointTorqueSet::KneeFlexion , + AnkleExtension = JointTorqueSet::AnkleExtension, + AnkleFlexion = JointTorqueSet::AnkleFlexion, + LastJointTorque + }; + const static char* GenderNames[]; + const static char* AgeGroupNames[]; + const static char* JointTorqueNames[]; + Anderson2007(){} + } Anderson2007; + + /** + This struct contains 3 enumerated lists (Gender, AgeGroup, + JointTorque) that lists the genders, age groups, and + joint-torque-directions for which the Gymnast data set contains + data. At the present time Gymnast data set only contains data + appropriate for a young (18-25) elite male gymnast. Please refer + to the main class description for details on this data set and how + to use it. + */ + const static struct Gymnast{ + enum Gender { + Male = GenderSet::Male, + LastGender + }; + enum AgeGroup { + Young18To25 = AgeGroupSet::Young18To25, + LastAgeGroup + }; + + enum TableIndex { + TauMax = 0, + OmegaMax, + ActiveAngleAtOneNormTorque, + ActiveAngularStandardDeviation, + TvAtMaxEccentricVelocity, + TvAtHalfMaxConcentricVelocity, + PassiveAngleAtZeroTorque, + PassiveAngleAtOneNormTorque, + LastTableIndex + }; + + enum JointTorque { + HipExtension = JointTorqueSet::HipExtension , + HipFlexion = JointTorqueSet::HipFlexion , + KneeExtension = JointTorqueSet::KneeExtension , + KneeFlexion = JointTorqueSet::KneeFlexion , + AnkleExtension = JointTorqueSet::AnkleExtension , + AnkleFlexion = JointTorqueSet::AnkleFlexion , + ElbowExtension = JointTorqueSet::ElbowExtension , + ElbowFlexion = JointTorqueSet::ElbowFlexion , + ShoulderExtension = JointTorqueSet::ShoulderExtension , + ShoulderFlexion = JointTorqueSet::ShoulderFlexion , + WristExtension = JointTorqueSet::WristExtension , + WristFlexion = JointTorqueSet::WristFlexion , + ShoulderHorizontalAdduction = + JointTorqueSet::ShoulderHorizontalAdduction, + ShoulderHorizontalAbduction = + JointTorqueSet::ShoulderHorizontalAbduction, + ShoulderInternalRotation = + JointTorqueSet::ShoulderInternalRotation , + ShoulderExternalRotation = + JointTorqueSet::ShoulderExternalRotation , + WristUlnarDeviation = JointTorqueSet::WristUlnarDeviation , + WristRadialDeviation = JointTorqueSet::WristRadialDeviation, + WristPronation = JointTorqueSet::WristPronation , + WristSupination = JointTorqueSet::WristSupination , + LumbarExtension = JointTorqueSet::LumbarExtension, + LumbarFlexion = JointTorqueSet::LumbarFlexion, + LastJointTorque + }; + const static char* GenderNames[]; + const static char* AgeGroupNames[]; + const static char* JointTorqueNames[]; + Gymnast(){} + } Gymnast; + + + + /** + This is a struct that contains subject-specific information that + does not change for a given subject. + + @param gender + Male/Female. Selecting a gender that + is not present in a data set will result in + the program aborting and an error message + being printed to the terminal. + + @param ageGroup: + Presently the age group options include Young (18-25), + middle aged (55-65), and senior (>65). Selecting an + age group that is not present in a data set will result + in the program aborting and an error message + being printed to the terminal. + + @param mSubjectHeightInMeters + This parameter is used to scale from the normalized + curves reported by Anderson et al. + See the class description for details. + + @param mSubjectMassInKg + This parameter is used to scale from the normalized + curves reported by Anderson et al. + See the class description for details. + */ + struct SubjectInformation{ + GenderSet::item gender; + AgeGroupSet::item ageGroup; + double heightInMeters; + double massInKg; + }; + + + + struct TorqueMuscleSummary{ + /**The angle of the fiber (rad)*/ + double fiberAngle; + + /**The angular velocity of the fiber (rad/s)*/ + double fiberAngularVelocity; + + + /**The activation of the muscle*/ + double activation; + + /** The normalized value of the passive-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberPassiveTorqueAngleMultiplier; + + /**The normalized value of the active-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberActiveTorqueAngleMultiplier; + + /**The normalized value of the torque-angular-velocity curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberTorqueAngularVelocityMultiplier; + + /**The torque generated by the damping element (Nm/Nm)*/ + double fiberNormalizedDampingTorque; + + /**The torque generated by the entire fiber (Nm)*/ + double fiberTorque; + + /**The joint torque developed by the muscle. This is signed so + that it is consistent with the sign convention of the joint + chosen by the user. (Nm)*/ + double jointTorque; + + + TorqueMuscleSummary(): + fiberAngle(nan("1")), + fiberAngularVelocity(nan("1")), + activation(nan("1")), + fiberPassiveTorqueAngleMultiplier(nan("1")), + fiberActiveTorqueAngleMultiplier(nan("1")), + fiberTorqueAngularVelocityMultiplier(nan("1")), + fiberNormalizedDampingTorque(nan("1")), + fiberTorque(nan("1")), + jointTorque(nan("1")){} + + }; + + struct TorqueMuscleInfo{ + + /**The angle of the joint (radians)*/ + double jointAngle; + + /**The angular velocity of the joint, where the sign convention + is chosen by the user at the time the torque muscle is created + (radians/sec)*/ + double jointAngularVelocity; + + /**The angle that the muscle fiber spans (radians)*/ + double fiberAngle; + + //The angle that the tendon spans (radians) + //double tendonAngle; + + /**The rate-of-angular-lengthening of the fiber. + A positive sign is for a concentric contraction, + that is where the fibers are shortening. (radians/sec)*/ + double fiberAngularVelocity; + + /*The rate-of-angular-lengthening of the tendon. + A positive sign is for a concentric contraction, + that is where the tendon is shortening. (radians/sec)*/ + //double tendonAngularVelocity; + + /** The normalized value of the passive-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberPassiveTorqueAngleMultiplier; + double DfiberPassiveTorqueAngleMultiplier_DblendingVariable; + double DfiberPassiveTorqueAngleMultiplier_DangleOffset; + /**The normalized value of the active-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberActiveTorqueAngleMultiplier; + double DfiberActiveTorqueAngleMultiplier_DblendingVariable; + /**The normalized value of the torque-angular-velocity curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + double fiberTorqueAngularVelocityMultiplier; + double DfiberTorqueAngularVelocityMultiplier_DblendingVariable; + /*The normalized value of the tendon-torque-angle curve. + Here a value of 1 means 1 maximum-isometric-torque. (Nm/Nm)*/ + //double tendonTorqueAngleMultiplier; + + /**The activation of the muscle*/ + double activation; + + /**The torque generated by the active element of the muscle + fiber (Nm)*/ + double fiberActiveTorque; + + /**The total torque generated by the passive elements of the muscle + fiber (Nm)*/ + double fiberPassiveTorque; + + /**The torque generated by the passive elastic element of the + muscle fiber (Nm) + */ + double fiberPassiveElasticTorque; + + /**The torque generated by the damping element (Nm)*/ + double fiberDampingTorque; + + /**The torque generated by the damping element (Nm)*/ + double fiberNormDampingTorque; + + /**The torque generated by the entire fiber (Nm)*/ + double fiberTorque; + + /*The torque transmitted through the tendon across the joint (Nm)*/ + //double tendonTorque; + + /**The joint torque developed by the muscle. This is signed so + that it is consistent with the sign convention of the joint + chosen by the user. (Nm)*/ + double jointTorque; + + /**The stiffness of the fiber (Nm/rad)*/ + double fiberStiffness; + + /*The stiffness of the tendon (Nm/rad)*/ + //double tendonStiffness; + + /** The stiffness of the joint. This is signed so + that it is consistent with the sign convention of the joint + chosen by the user. (Nm/rad)*/ + double jointStiffness; + + /**The power output of the active fiber element. A positive + power means that the fiber is contracting concentrically. + (Watts - Nm/s)*/ + double fiberActivePower; + + /**The power output of the passive fiber element. A positive + power means that the passive element is recoiling concentrically + (Watts - Nm/s)*/ + double fiberPassivePower; + + /**The total power output of the fiber element.(Watts - Nm/s)*/ + double fiberPower; + + /** The power output by this muscle at the joint. This is + signed so that it is consistent with the sign convention of the + joint chosen by the user. (Nm/rad)*/ + double jointPower; + + /**The partial derivative of joint torque w.r.t activation*/ + double DjointTorque_Dactivation; + + /**The partial derivative of joint torque w.r.t the joint angle*/ + double DjointTorque_DjointAngle; + + /**The partial derivative of joint torque w.r.t the joint + angular velocity*/ + double DjointTorque_DjointAngularVelocity; + + ///First derivatives for the fitting algorithm + double DjointTorque_DactiveTorqueAngleBlendingVariable; + double DjointTorque_DpassiveTorqueAngleBlendingVariable; + double DjointTorque_DtorqueAngularVelocityBlendingVariable; + double DjointTorque_DmaximumIsometricTorque; + double DjointTorque_DpassiveTorqueAngleCurveAngleOffset; + + double DjointTorque_DactiveTorqueAngleAngleScaling; + double DjointTorque_DmaximumAngularVelocity; + + + ///This vector contains additional quantities that are needed by + /// the fitting functions, and is not for external use. Use this + /// vector at your own risk, as its definition/sizing etc may + /// change. + RigidBodyDynamics::Math::VectorNd fittingInfo; + + TorqueMuscleInfo(): + jointAngle(nan("1")), + jointAngularVelocity(nan("1")), + fiberAngle(nan("1")), + fiberAngularVelocity(nan("1")), + fiberPassiveTorqueAngleMultiplier(nan("1")), + DfiberPassiveTorqueAngleMultiplier_DblendingVariable(nan("1")), + DfiberPassiveTorqueAngleMultiplier_DangleOffset(nan("1")), + fiberActiveTorqueAngleMultiplier(nan("1")), + DfiberActiveTorqueAngleMultiplier_DblendingVariable(nan("1")), + fiberTorqueAngularVelocityMultiplier(nan("1")), + DfiberTorqueAngularVelocityMultiplier_DblendingVariable(nan("1")), + activation(nan("1")), + fiberActiveTorque(nan("1")), + fiberPassiveTorque(nan("1")), + fiberPassiveElasticTorque(nan("1")), + fiberDampingTorque(nan("1")), + fiberNormDampingTorque(nan("1")), + fiberTorque(nan("1")), + jointTorque(nan("1")), + fiberStiffness(nan("1")), + jointStiffness(nan("1")), + fiberActivePower(nan("1")), + fiberPassivePower(nan("1")), + fiberPower(nan("1")), + jointPower(nan("1")), + DjointTorque_Dactivation(nan("1")), + DjointTorque_DjointAngle(nan("1")), + DjointTorque_DjointAngularVelocity(nan("1")), + DjointTorque_DactiveTorqueAngleBlendingVariable(nan("1")), + DjointTorque_DpassiveTorqueAngleBlendingVariable(nan("1")), + DjointTorque_DtorqueAngularVelocityBlendingVariable(nan("1")), + DjointTorque_DmaximumIsometricTorque(nan("1")), + DjointTorque_DpassiveTorqueAngleCurveAngleOffset(nan("1")), + DjointTorque_DactiveTorqueAngleAngleScaling(nan("1")), + DjointTorque_DmaximumAngularVelocity(nan("1")) + {} + + }; + + + struct TorqueMuscleParameterFittingData { + + unsigned int indexAtMaximumActivation; + unsigned int indexAtMinimumActivation; + unsigned int indexAtMaxPassiveTorqueAngleMultiplier; + + bool isTorqueMuscleActive; + + double activeTorqueAngleBlendingVariable; + double passiveTorqueAngleBlendingVariable; + double torqueVelocityBlendingVariable; + double passiveTorqueAngleCurveOffset; + double maximumActiveIsometricTorque; + + double activeTorqueAngleAngleScaling; + double maximumAngularVelocity; + + //double objectiveValue; + //double constraintError; + bool fittingConverged; + + TorqueMuscleSummary summaryDataAtMinimumActivation; + TorqueMuscleSummary summaryDataAtMaximumActivation; + TorqueMuscleSummary summaryDataAtMaximumPassiveTorqueAngleMultiplier; + + + TorqueMuscleParameterFittingData(): + indexAtMaximumActivation(std::numeric_limits::infinity()), + indexAtMinimumActivation(std::numeric_limits::infinity()), + indexAtMaxPassiveTorqueAngleMultiplier(std::numeric_limits::infinity()), + isTorqueMuscleActive(true), + activeTorqueAngleBlendingVariable(nan("1")), + passiveTorqueAngleBlendingVariable(nan("1")), + torqueVelocityBlendingVariable(nan("1")), + passiveTorqueAngleCurveOffset(nan("1")), + maximumActiveIsometricTorque(nan("1")), + activeTorqueAngleAngleScaling(nan("1")), + maximumAngularVelocity(nan("1")), + fittingConverged(false), + summaryDataAtMinimumActivation(), + summaryDataAtMaximumActivation(), + summaryDataAtMaximumPassiveTorqueAngleMultiplier(){} + }; + + struct TorqueMuscleDataFeatures{ + unsigned int indexOfMaxActivation; + unsigned int indexOfMinActivation; + unsigned int indexOfMaxPassiveTorqueAngleMultiplier; + + bool isInactive; + + TorqueMuscleSummary summaryAtMaxActivation; + TorqueMuscleSummary summaryAtMinActivation; + TorqueMuscleSummary summaryAtMaxPassiveTorqueAngleMultiplier; + }; + + /** + \brief This class implements a rigid-tendon muscle-torque-generator + (MTG) for a growing list of joints and torque-directions. For a + detailed description of the MTGs available and the automatic + fitting routine (implemented in TorqueMuscleFittingToolkit) please + see the publication: M.Millard, A.L.Kleesattel, M.Harant, & + K.Mombaur. A reduced muscle model and planar musculoskeletal model + fit for the synthesis of whole body movements. Journal of + Biomechanics. (under review as of August 2018) + + This rigid-tendon torque muscle model provides modeling support + for 3 phenomena + + - torque-angle curve (\f$\mathbf{t}_A(\theta)\f$): the variation of active isometric torque in one direction as a function of joint angle + - torque-velocity curve (\f$\mathbf{t}_V(\dot{\theta})\f$): the variation of torque as a function of angular velocity + - passive-torque-angle curve (\f$\mathbf{t}_P(\theta-\theta_S)\f$): the variation of passive torque as a function of joint angle. Here \f$s_P\f$ and \f$\theta_S\f$ are user-defined scaling and shift parameters. + + each of which are represented as smooth normalized curves that + vary between 0 and 1. A series of scaling + (\f$s^A, s^V, s^\tau\f$), shifting (\f$\Delta^P\f$), and blending + (\f$\lambda^A,\lambda^V,\lambda^P\f$) variables have been introduced + to these curves to make it possible to easily fit these curves to + a specific subject. These fitting variables can be set/fitted by + making use of the functions in this class. Alternatively these + fitting variables can be automatically adjusted (using IPOPT) by + making use of the TorqueMuscleFittingToolkit. + + \image html fig_MuscleAddon_BlendableTorqueMuscle.png "Examples of the adjustable characteristic curves" + + These three curves are used to compute the + torque developed \f$\tau\f$ given the angle of the joint + \f$\theta\f$, the angular-velocity of the joint \f$\dot{\theta}\f$, + and the activation of the muscle \f$\mathbf{a}\f$ (a 0-1 quantity + that defines how much the muscle is turned-on, or activated), and + the maximum-isometric torque \f$\tau_{ISO}\f$ + \f[ + \tau (\mathbf{a}, \theta,\dot{\theta}) = + \tau_{ISO} ( \mathbf{a} \, \mathbf{t}_A(\theta) \mathbf{t}_V(\dot{\theta}/\dot{\theta}_{MAX}) + +\mathbf{t}_P(1- \beta (\dot{\theta}/\dot{\theta}_{MAX})) \, ) + \f] + The damping term \f$\beta\f$ is necessary to supress vibration + that will occur as the passive element \f$\mathbf{t}_P\f$ is streched, + its stiffness increases, and the natural frequency of the overall + system rises. By default \f$\beta\f$ is set to 0.1 which has proven + effective for supressing vibration in the trunk segments during a + stoop lift in which the stiffness of the lumbar back muscles grows + appreciably. This model does not yet provide support for the + following phenomena but will in the future. + + - activation dynamics: currently is left to the user + - tendon-elasticity + - muscle short-range-stiffness + + All of these characteristic curves are represented using \f$C_2\f$ + continuous \f$5^{th}\f$ order Bezier curves that have been fitted to + the data from data in the literature. In many cases these + curves have been carefully edited so that they fit the curves of + the original papers, but have more desireable numerical properties + for optimal control work. The characterisic curves provided by this + class have been fitted to a growing list of data sets: + -Anderson Data Set: from Anderson et al. 2007 + -Whole-body Gymnast Data Set: from Jackson, Kentel et al., Anderson et al., Dolan et al. and Raschke et al. + + Data Set: Anderson2007 + + This data set uses the mean value of the coefficients published + in Anderson et al. The standard deviation table has also been + entered. However, since it is unclear how to use the standard + deviation in a consistent way across all joints/parameters this + table is not yet accessible through the constructor. This data + set includes coefficients for the following + + - Number of subjects: 34 + - Gender: male and female + - Age: young (18-25, 14 subjects), middle-aged (55-65, 14 subjects), senior (> 65, 6 subjects) + - Joint: hip/knee/ankle + - Direction: extension/flexion + + Notes + -# Angles are plotted using units of degrees for readability. The + actual curves are described in units of radians + -# See Anderson et al. for further details. + + \image html fig_MuscleAddon_Anderson2007AllPositiveSigns.png "Characteristic from Anderson et al. 2007 [1]" + + Data Set: Gymnast + + This data set is an attempt at making enough torque muscles for a + whole body. Since no single source in the literature comes close to + measuring the characteristics of all of the joints, data from + Jackson et al., Kentel et al., Anderson et al., Dolan et al, and + Raschke et al. + have been combined. Since the subjects used in these various studies + are wildly different (Jackson et al. measured an elite male gymnast; + Kentel et al. measured an elite tennis player; Anderson et al. measured, + in the category of young male, a selection of active undergraduate + students, Dolan et al from 126 women and 23 men, and Raschke from 5 + male subjects) scaling has been used to make the strength of the subject + consistent. Scaling coefficients for the lower body, shoulders and + elbow, and forearm/wrist have been calculated using measurements + that overlapped between datasets. Presently this data set includes + curves for 22 torque muscles. + + - Number of subjects: 1 elite gymnast (69.6 kg, 1.732 m) + - Gender: male + - Age: 21 years old + - Joint and Directions available + -# Ankle: flexion/extension (scaled from Anderson) + -# Knee: flexion/extension (from Jackson) + -# Hip: flexion/extension (from Jackson) + -# Lumbar: active extension curves (\f$\mathbf{t}_A\f$ and \f$\mathbf{t}_P\f$) from Raschke et al. passive extension from Dolan et al. The torque velocity curve has since been updated using an estimate from the archiecture of the lumbar extensors + -# Lumbar: active flexion \f$\tau_{ISO}\f$ from Beimborn et al. + -# Shoulder: flexion/extension (from Jackson) + -# Shoulder: horizontal adduction/abduction (from Kentel, scaled to Jackson's subject) + -# Shoulder: internal rotation/external rotation (from Kentel, scaled to Jackson's subject) + -# Elbow: flexion/extension (from Kentel, scaled to Jackson's subject) + -# Wrist: pronation/supination (from Kentel, scaled to Jackson's subject) + -# Wrist: extension/flextion (from Jackson) + -# Wrist: ulnar/radial deviation (from Kentel, scaled to Jackson's subject) + - Missing Joint and directions + -# Ankle inversion/eversion + -# Hip adduction/abduction + -# Hip internal rotation/external rotation + -# Lumbar extension/flexion + -# Lumbar bending + -# Lumbar twisting + -# Shoulder Adduction + -# Shoulder Abduction + -# Scapular elevation/depression + -# Scapular adduction/abduction + -# Scapular upward/downward rotation + + In all cases the curves have been fitted to Bezier curves that + are constructed using functions in TorqueMuscleFunctionFactory. + + Notes + -# Angles are plotted using units of degrees for readability. The actual curves are described in units of radians + -# Hip and Knee characteristics taken from Jackson. Ankle extension is from Anderson et al., scaled using Jackson-to-Anderson hip/knee strength ratios from Jackson ratios + -# Shoulder horizontal adduction/abduction and internal/external rotation is a scaled version of the Kentel. Strength was scaled using the Jackson-to-Kentel shoulder flex/ext ratios. + -# Elbow extension/flexion and forearm pronation/supination. Elbow strength scaled from Kentel using the ratio of maximum isometric shoulder ext/flextion between Kentel and Jackson. Forearm pronation/supination scaled using the maximum torque strength ratio of wrist extension/flextion between Kentel and Jackson + -# Wrist ext/flexion directly from Jackson, while the curves for ulnar and radial deviation have been scaled (using the maximum isometric torque ratios of wrist extension and flexion from both models) from Kentel et al. + -# Lumbar-extension active-torque-angle-curve in extension comes from Raschke et al. + -# Lumbar-extension passive-torque-angle-curve comes from Dolan et al. + -# Lumbar-flexion \f$\tau_{ISO}\f$ comes from Beimborn et al.'s observation that the strength ratio of back extensors to flextors at a flextion angle of zero is most often repored as 1.3:1. + -# Lumbar-extension and flexion torque-velocity-curves have been estumated using the architecture of these two muscles. This update has been made because the force-velocity curve proposed by Raschke and Chaffin in Fig. 4 has such a low maximum angular velocity (60 dec/sec) that none of our optimal control simulations predicted lumbar flexion during movement. Using muscle archectural information for the erector spinae (ES) and rectus abdominus (RA), assuming both are made of slow twitch fibers the maximum angular velocites are 433 deg/sec and 1102 deg/sec about the constant assumed moment arms for the lumbar extensors and flexors respectively. The following architectural information was used: + -# 7.1 cm :ES moment arm from NĂ©meth & OhlsĂ©n + -# 8.08 cm :ES optimal fiber length taken from a weighted PSCA average of the ES muscles from Christophy et al. Table 1 + -# 7.02 \f$\ell/s\f$ :ES maximum angular velocity for slow twitch muscle from Ranatunga + -# 0.151 :ES tv at half max. angular velocity + -# 10.9 cm :RA moment arm from NĂ©meth & OhlsĂ©n + -# 29.9 cm :RA optimal fiber length from Christophy et al. Table 1 + -# 7.02 \f$\ell/s\f$ :RA maximum angular velocity for slow twitch muscle from Ranatunga + -# 0.151 :RA tv at half max. angular velocity for slow twitch muscle from Ranatunga + -# Any passive curve that is not accompanied by a curve from the literature (see the plots for details) is an educated guess. + + + \image html fig_MuscleAddon_Gymnast_HipKneeAnkle.png " Hip/Knee/Ankle: from Jackson and Anderson et al. " + \image html fig_MuscleAddon_Gymnast_Lumbar.png " Lumbar Extension/Flexion: from Dolan et al.,Raschke et al., and Beimborn et al." + \image html fig_MuscleAddon_Gymnast_Shoulder3Dof.png " Shoulder 3 DoF torques: from Jackson and Kentel et al. " + \image html fig_MuscleAddon_Gymnast_ElbowForearm.png " Elbow flexion/extension: from Kentel et al." + \image html fig_MuscleAddon_Gymnast_Wrist3Dof.png " Wrist 3 DoF torques: from Jackson and Kentel et al." + + Parameterized Curves used here vs. Literature + + The curves used in this implementation are 2nd order 2-dimensional + Bezier curves. The curves described in Anderson et al., Jackson, + Kentel were not directly used because they are not continuous to the + second derivative (a requirement for most gradient based optimization + routines). There are some other detailed differences that might be + of interest: + + -# Anderson et al.'s torque-velocity curve tends to large + negative values for fast eccentric contractions. This is + in contrast to the literature which says that at large + eccentric contractions the torque-velocity curve (or the + force-velocity-curve) tends to a value between 1.0 and 1.4. + -# Anderson et al.'s torque-velcity curve for ankle extension + did not cross the x-axis on the concentric side of the curve. + This would endow the plantar flexors with super-human abilities. + This error has been corrected by fitting a Bezier curve to a + Hill-type curve that passes through the point where + \f$\dot{\theta}= \frac{1}{2} \dot{\theta}_{MAX}\f$ + -# Anderson et al.'s, Jackson, and Kentel et al. had discontinuities + in the first derivative of the force velocity curve at + \f$\dot{\theta}=0\f$. While this follows Huxley's famous observations + that the slope does discontinuously change at at \f$\dot{\theta}=0\f$, + this is not a phenomena that is not compatible with most optimal + control formulations and thus this discontinuity is not present in + the force velocity curves used in this model. + -# Anderson et al. and Kentel et al.'s active-torque-angle curves + can achieve negative values - this is obviously undesirable as it + will allow a muscle to push. + -# Kentel et al.'s activation inhibition function does not always + cross 1.0 for \f$\dot{\theta}=0\f$, which means that \f$\tau_{ISO}\f$ + is not reached. This makes for a confusing model to use. + + + Coordinate Mapping + + Every author chose a particular convention for measuring + the angles of the hip, knee, ankle joint, shoulder, elbow, wrist and + lumbar --- see the figure for details. These conventions have all + been mapped to the one used in the illustrations. You will need to + use the figure, your model, and the constructors appropriately + so that + + -# the joint angle of your model is correctly mapped to the + fiber angle of the Millard2016TorqueMuscle; + -# the sign of the muscle's output torque matches the + sign associated with your model. + + To map from your model's joint coordinates to the joint coordines + used in this model (see the figure in the description) + the followinq equation is used at the torque level + + \f$ jointTorque = signOfJointTorque*fiberTorque \f$ + + where fiberTorque is the torque produced by Anderson et al.'s curves, + which is always positive. At the position level, the angles from your + models joint angle to Anderson et al.'s joint angle (called fiberAngle) + are mapped using + + \f$ fiberAngle = signOfJointAngleRelativeToAnderson2007*(jointAngle-jointAngleOffsetRelativeToAnderson2007). \f$ + + Internally the sign of the fiber velocity follows signOfJointTorque so + that the signs of joint power and muscle power are consistent. + + Strength Scaling: Anderson2007 Data Set + + The leg strength (here we mean \f$\tau_{ISO}\f$) + predicted by Anderson et al.'s curves should be taken + as a good first approximation. While Anderson et al.'s data set is + the most comprehensive in the literature, they only measured torques + from active people: they did not include people at the extremes + (both very weak, and very strong), nor did they include children. + Finally, the torques produced by each subject were normalized by + mSubjectMassInKg*subjectHeightInM*accelerationDueToGravity. Strength + is a strange phenomena which is not nicely normalized by just these + quantites, and so the strength predicted by Anderson et al.'s curves + might not fit your subject even if they are represented in Anderson + et al.'s data set. + + + Strength Scaling: Gymnast Data Set + + The strength used in the Gymnast data set is fitted to an elite + male gymnast. It goes without saying that an elite gymnast has + strength proportions, and an absolute strength that are not typical. + In the future it would be nice to have a function that could provide + an educated guess about how to map Gymnast's strengths to that of + another subject. For the moment I have no idea how to do this, nor + am I aware of any works in the literature that can provide insight + of how to do this. For now the whole-body Gymnast model should be + viewed as being a representation of what is possible for a human, + but not a typical human. At the present time the default strength + settings of the Gymnast are not scaled by subject height, nor + weight. + + If you happen to know the maximum-isometric-active-torque (note this + does not include the passive component) that your subject can + produce, you can update the strength of the torque-muscle using the + functions getMaximumActiveIsometricTorque(), and + setMaximumActiveIsometricTorque(). + + Fitting to a Specific Subject + + If you have recorded the motions and external forces acting on a + subject during a movement of interest, you can make use of the + TorqueMuscleFittingToolkit to adjust the MTG so that it is strong + enough and flexible enough to allow a model of the subject to + reproduce the experimental data. Please see the + TorqueMuscleFittingToolkit class for details. + + + Limitations + + This rigid-tendon torque muscle has some limitations that you should + be aware of: + + -# There are no elastic tendons. That means that the mapping between + the mechanical work done by this torque actuator will greatly differ + from the positive mechanical work done by a torque actuator that + includes an elastic tendon. This difference is greatest for those + muscles with long tendons - namely the Achilles tendon in humans. + If you are + interested in fiber kinematics, fiber work, or metabolic energy + consumption you cannot use this model especially for muscles that + have long tendons. + -# This model formulation predicts torque well, but does a poor job + of predicting joint stiffness. In this model stiffness is given + by the partial derivative of torque w.r.t. joint angle. Since the + active-torque-angle curve fits a bell-shaped curve, it is possible + to construct a torque muscle that has a region of physically + impossible negative stiffness. Real muscle, in constrast, always + has a positive stiffness even on the descending limb of the + active-torque-angle curve (see Rassier et al. for details). + -# Muscles that cross 2 joints (e.g. the hamstrings) produce coupled + torques at both of those joints. In this model there is no coupling + between joints. Furthermore, because of the lack of coupling the + curves used here are only valid for the posture that Anderson et al., + Jackson, and Kentel et al. + used when they made their data collection. If you are interested + in simulating postures that are very different from those described + in by these authors then the results produced by this model should + be treated as very rough. + -# Because this is a joint-torque muscle, none of the joint contact + forces predicted will come close to matching what is produced by + line-type muscles. If you are interested in joint-contact forces you + cannot use this model. + + This simple model is a fast approximate means to constrain the joint + torque developed in the body to something that is physiologically + possible. That is it. + + Units + Although the figure in this description has angles in units + of degrees, this is only to help intuitition: when using + the model, use radians. This model uses MKS: + + -Distance: m + -Angles: radians + -Angular velocity: radians/s + -Mass: kg + -Torque: Nm + -Time: second + -Power: Nm/second + + Developer Notes + + All of the numerical evaluations of this model take place + in a few private stateless functions: + -updateTorqueMuscleSummary + -updTorqueMuscleSummaryCurveValues + -updTorqueMuscleInfo + -updInvertTorqueMuscleSummary + If you want to change something fundamental about the + mathematics of the model, then you have to update these functions. + + References + + -# Anderson, D. E., Madigan, M. L., & Nussbaum, M. A. (2007). + Maximum voluntary joint torque as a function of joint angle + and angular velocity: model development and application to + the lower limb. Journal of biomechanics, 40(14), 3105-3113. + + -# Beimborn, D. S., & Morrissey, M. C. (1988). A review of the + literature related to trunk muscle performance. Spine, 13(6), 655-660. + + -# Christophy, M., Senan, N. A. F., Lotz, J. C., & O’Reilly, O. M. + (2012). A musculoskeletal model for the lumbar spine. + Biomechanics and Modeling in Mechanobiology, 11(1-2), 19-34. + + -# Dolan, P., A. F. Mannion, and M. A. Adams. Passive tissues help + the back muscles to generate extensor moments during lifting. + Journal of Biomechanics 27, no. 8 (1994): 1077-1085. + + + -# Jackson, M.I. (2010). The mechanics of the Table Contact + Phase of Gymnastics Vaulting. Doctoral Thesis, Loughborough + University. + + -# Kentel, B.B., King, M.A., & Mitchell, S.R. (2011). + Evaluation of a subject-specific torque-driven computer simulation + model of one-handed tennis backhand ground strokes. Journal of + Applied Biomechanics, 27(4),345-354. + + -# Millard, M., Uchida, T., Seth, A., & Delp, S. L. (2013). + Flexing computational muscle: modeling and simulation of + musculotendon dynamics. Journal of biomechanical engineering, + 135(2), 021005. + + -# NĂ©meth, G., & OhlsĂ©n, H. (1986). Moment arm lengths of trunk + muscles to the lumbosacral joint obtained in vivo with computed + tomography. Spine, 11(2), 158-160. + + -# Ranatunga, K. W. (1984). The force-velocity relation of rat + fast-and slow-twitch muscles examined at different temperatures. + The Journal of Physiology, 351, 517. + + -# Raschke, U., & Chaffin, D. B. (1996). Support for a linear + length-tension relation of the torso extensor muscles: an + investigation of the length and velocity EMG-force relationships. + Journal of biomechanics, 29(12), 1597-1604. + + -# Rassier, D. E., Herzog, W., Wakeling, J., & Syme, D. A. (2003). + Stretch-induced, steady-state force enhancement in single + skeletal muscle fibers exceeds the isometric force at optimum + fiber length. Journal of biomechanics, 36(9), 1309-1316. + + + + */ + class Millard2016TorqueMuscle { +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + friend class TorqueMuscleFittingToolkit; + friend class FitTorqueMuscleParameters; +#endif + + public: + /** + Default constructor, which for the moment does nothing. + Calling any of the models functions after the default + construction will result in a runtime error. + */ + Millard2016TorqueMuscle(); + + + /** + This constructor allows you to easily access the large + table of built-in torque muscle coefficients to create + a torque muscle that best represents the joint of interest. + + Note: directions + This constructs a single joint-torque muscle: + it can only generate torque in one direction. If you want + to generate a torque in two directions, you need 2 torque + muscles. + + Note: signs and offsets + + All of the angles in these models are defined anatomically. + You will need to set a series of variables to correctly + map from your model's joint coordinates and sign conventions + to that of the models: + jointAngleOffsetRelativeToDoxygenFigures, + signOfJointAngleRelativeToDoxygenFigures, + signOfJointTorqueToDoxygenFigures. Also note that due to + the anatomical angle definitions some left and right handed + joints will require different signs. This will be true + for internal/external rotation at the shoulder, + horizontal adduction/abduction at the shoulder, ulnar/radial + deviation at the wrist, pronation/supination of the wrist, + and others as the list of directions grows. + + Note: blending variables + + When this constructor is called the blending variables for the + active-torque-angle-curve, torque-angular-velocity-curve, and + passive-torque-angle-curve are set to 0. + + @param dataSet + The desired source of joint torque + coefficients. Use the DataSet structure to choose + the desired data set (e.g. DataSet::Anderson2007, + or DataSet::Gymnast) + + @param subjectInfo + A struct that contains metadata about the subject + which is used to scale the maximum torque of the + torque muscle. + + @param jointTorque + Select the joint and torque direction of interest. + Use the struct for each data set to choose a + joint-torque-direction that is in the set (e.g. + Anderson2007::HipExtension, or + Gymnast::ShoulderHorizontalAdduction) + + @param jointAngleOffsetRelativeToDoxygenFigures + Offset angle between your model's joints and the + reference figures in class description. + + @param signOfJointAngleRelativeToDoxygenFigures + The sign convention that converts your model's joint + angles to the angles used in the reference figures. + + @param signOfJointTorqueToDoxygenFigures + The sign that maps fiberTorque from Anderson's model + (which is always positive) to the correctly signed + joint torque for your model. + + @param name + The name of the muscle. This is needed to do useful + things like provide error messages that are human + readable. + + @throws abort() when + -# The combination of dataSet, gender, joint, and jointDirection does not correspond to a valid entry + -# mSubjectHeightInMeters <= 0 + -# mSubjectMassInKg <= 0 + -# abs(signOfJointTorque)-1 > epsilon + + */ + Millard2016TorqueMuscle( + DataSet::item dataSet, + const SubjectInformation &subjectInfo, + int jointTorque, + double jointAngleOffsetRelativeToDoxygenFigures, + double signOfJointAngleRelativeToDoxygenFigures, + double signOfJointTorqueToDoxygenFigures, + const std::string& name + ); + + + /** + Calculates the signed joint torque developed by the + muscle. Note that the signs that are needed to map from the + native curves to those of your specific model are set when the + muscle is constructed. + \f[ + \tau_{M} = \pm \tau_{ISO}( \mathbf{a} \, \mathbf{t}_A(\theta) \mathbf{t}_V(\dot{\theta}/\dot{\theta}_{MAX}) + +\mathbf{t}_P(1- \beta (\dot{\theta}/\dot{\theta}_{MAX})) \, ) + \f] + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param activation: the percentage of the muscle that is + turned on [0-1]. This function allows activations to be + outside [0,1], because this is useful during the + intermediate solutions of an optimization run. However, + you must ensure after the fact that your activations + fall within a bound of [0,1]. + + @returns torque developed by the muscle in (Nm). + */ + double calcJointTorque( + double jointAngle, + double jointAngularVelocity, + double activation) const; + + + /** + This function will calculate the muscle activation needed to + generate the joint torque. This function is useful in the + process of fitting the strength of a model to inverse-dynamics + data. The value for activation is arrived at by evaluating this + equation + \f[ + \mathbf{a} = \dfrac{ \dfrac{\tau (\mathbf{a}, \theta,\dot{\theta})}{ \tau_{ISO}} + - \mathbf{t}_P(1- \beta (\dot{\theta}/\dot{\theta}_{MAX}))}{ + \mathbf{t}_A(\theta) \mathbf{t}_V(\dot{\theta}/\dot{\theta}_{MAX})}. + \f] + If the passive forces at the desired angle actually exceed the + desired torque, a negative value for activation will be returned. + If you see this it just means the passive-torque-angle curve of + this muscle needs to be adjusted. + + Note + An activation value of 0 is passed out if the sign of the + desired jointTorque differs from the sign of the joint torque + that this muscle can produce. + + + + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param jointTorque (Nm). + + @param updTorqueMuscleSummaryStruct + TorqueMuscleSummary struct which contains the + calculated activation along with all of the + internal parameters of the muscle so that you can + understand why the activation value takes the + value that it does. See the TorqueMuscleSummary + (in this file) for details. + */ + void calcActivation(double jointAngle, + double jointAngularVelocity, + double jointTorque, + TorqueMuscleSummary + &updTorqueMuscleSummaryStruct) const; + + + /** + This function will compute the scaling factor \f$A\f$ that would + be needed in order for this muscle to generate a specific + joint torque \f$\tau^*\f$ at a specific activation level. + \f[ + A = \dfrac{\tau^*}{ + \tau_{ISO}( \mathbf{a} \, \mathbf{t}_A(\theta) \mathbf{t}_V(\dot{\theta}/\dot{\theta}_{MAX}) + +\mathbf{t}_P(1- \beta (\dot{\theta}/\dot{\theta}_{MAX})) \, )} + \f] + This function + is useful in the process of determining how to much to + scale the strength of a default model to match + inverse-dynamics data of a subject. In contrast to calcActivation, + if the passive forces of the muscle exceed the desired joint + torque the resulting scale factor will drop. + + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param jointTorque (Nm) + + @param activation + + @returns scaleFactor: The scale that would make the current + muscle produce exactly the desired torque at the + specfied angle and angular velocity. Note: a value + of 0 is passed out if the sign of the desired + jointTorque differs from the sign of the + joint torque that this muscle can produce. + */ + double calcMaximumActiveIsometricTorqueScalingFactor( + double jointAngle, + double jointAngularVelocity, + double activation, + double jointTorque) const; + + /** + Calculates a large number of internal quantities of the + torque muscle ranging from the values of the muscle's + components, the stiffness of the muscle, and its power output. + See the struct TorqueMuscleInfo (in this file) for details. + + @param jointAngle (radians) + + @param jointAngularVelocity (radians/sec) + + @param activation: the percentage of the muscle that is + turned on [0-1]. This function allows activations to be + outside [0,1], because this is useful during the + intermediate solutions of an optimization run. However, + you must ensure after the fact that your activations + fall within a bound of [0,1]. + + @param updTorqueMuscleInfoStruct: A torque muscle struct + */ + void calcTorqueMuscleInfo( + double jointAngle, + double jointAngularVelocity, + double activation, + TorqueMuscleInfo& updTorqueMuscleInfoStruct) const; + + /** + @return the sign of the joint torque (+/- 1) + */ + double getJointTorqueSign() const; + + /** + @return the sign of the angle sign relative to the + figures in the class description (+/- 1) + */ + double getJointAngleSign() const; + + /** + @return the offset angle between the model's joint + and the figures in the class description (rad) + */ + double getJointAngleOffset() const; + + + /** + @return the maximum-active-isometric torque that this muscle + can produce in Nm. + */ + double getMaximumActiveIsometricTorque() const; + + /** + @return the joint angle at which the normalized + active-torque-angle curve peaks at its + maximum value of 1.0. Angle is in radians + */ + double getJointAngleAtMaximumActiveIsometricTorque() const; + + /** + @return the width of the active torque angle curve. + Note that this will return the angular width of + the non-blended curve. + Angle is in radians. + + */ + double getActiveTorqueAngleCurveWidth() const; + + /** + @return the joint angle at which the normalized + passive-torque-angle curve reaches a value + of 1.0. If this curve never reaches a value + of 1.0 (because it is flat, or the + mPassiveTorqueScale has been set to 0) a value + of std::numeric_limits::signaling_NaN() + is returned. Use the std function isfinite to + test if a signaling_NaN has been returned. + Angle is in radians + */ + double getJointAngleAtOneNormalizedPassiveIsometricTorque() const; + + /** + @return the joint angle at which the normalized + passive-torque-angle curve reaches its + minimum value + Angle is in radians + */ + double getJointAngleAtSmallestNormalizedPassiveIsometricTorque() const; + + + /** + @return the maximum concentric angular velocity + in radians/sec. This scalar positive quantity + corresponds to the angular speed at which + the torque-velocity curve crosses zero. + */ + double getMaximumConcentricJointAngularVelocity() const; + + /** + @return the passive-torque-scale \f$s_P\f$ that is applied + to the passive-torque-curve. + */ + double getPassiveTorqueScale() const; + + /** + @return the angle \f$\theta_S\f$ that the passive curve has + been shifted. Note that the sign convention is in the sense + of the fiber angle, not in the sense of the joint angle. + (radians). + */ + double getPassiveCurveAngleOffset() const; + + /** + @return The maximum normalized damping term \f$\beta_{MAX}\f$ term + in the torque muscle model. See class description + for details. + */ + double getNormalizedDampingCoefficient() const; + + /** + @param beta -t normalized damping term \f$\beta\f$ term + in the torque muscle model. See class description + for details. + @throw abort() if beta < 0 + */ + void setNormalizedDampingCoefficient(double beta); + /** + Sets the scaling of the passive-joint-torques. By default + this scale + is one. + + @param passiveTorqueScale + The scale \f$s_P\f$ applied to the + passive-joint-torque curve (unitless) + */ + void setPassiveTorqueScale(double passiveTorqueScale); + + /** + @param passiveCurveAngleOffsetVal the angle \f$\Delta_p\f$ + that the passive curve should be shifted. Note that the + sign convention is in the sense of the fiber angle, not + in the sense of the joint angle. Angles in radians + */ + void setPassiveCurveAngleOffset( + double passiveCurveAngleOffsetVal); + + + double getTorqueVelocityMultiplierAtHalfOmegaMax() const; + void setTorqueVelocityMultiplierAtHalfOmegaMax(double tvAtHalfOmegaMax); + + /** + This function iteratively solves for the scaling + so that at the specified jointAngle the passive curve + develops the specified passiveTorque + + @param jointAngle the target joint angle in radians + @param passiveFiberTorque the target passive joint torque in Nm. + @throws abort if jointAngle is not in the domain of the curve + @throws abort if passiveFiberTorque < sqrt(eps) + */ + void fitPassiveTorqueScale(double jointAngle, + double passiveFiberTorque); + + /** + This function solves for the passive curve angle offset + so that at the specified jointAngle the passive curve + develops the specified passiveTorque + + @param jointAngle the target joint angle in radians + @param passiveFiberTorque the target passive joint torque in Nm. + @throws abort if passiveTorque < sqrt(eps) + */ + void fitPassiveCurveAngleOffset(double jointAngle, + double passiveFiberTorque); + + /** + Sets the strength of the muscle to match a desired value. + + @param maxIsometricTorque + The desired maximum-active-isometric torque of the + muscle (Nm) + + */ + void setMaximumActiveIsometricTorque( + double maxIsometricTorque); + + /** + @param maxAngularVelocity the maximum concentric joint + angular velocity magnitude in radians/sec. This scalar + quantity is not signed and must be greater than 0. + */ + void setMaximumConcentricJointAngularVelocity( + double maxAngularVelocity); + + /** + @return the SmoothSegmentedFunction represents the + active-torque-angle curve profile. + */ + + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getActiveTorqueAngleCurve() const; + + /** + @return the SmoothSegmentedFunction that represents the + passive-torque-angle curve. + */ + + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getPassiveTorqueAngleCurve() const; + + /** + @return the SmoothSegmentedFunction the represents the + torque-angular-velocity curve. + */ + const RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& getTorqueAngularVelocityCurve() const; + + + /** + @returns Value of \f$\lambda^A\f$ that is currently in use. + */ + double getActiveTorqueAngleCurveBlendingVariable() const; + + /** + @returns Value of \f$\lambda^P\f$ that is currently in use. + */ + double getPassiveTorqueAngleCurveBlendingVariable() const; + + /** + @returns Value of \f$\lambda^V\f$ that is currently in use. + */ + double getTorqueAngularVelocityCurveBlendingVariable() const; + + /** + @param blendingVariable [0-1] + The desired value of \f$\lambda^A\f$. Note at + \f$\lambda^A=1\f$ the curve is a horizontal line + with a value of 1. + */ + void setActiveTorqueAngleCurveBlendingVariable( + double blendingVariable); + + /** + @param blendingVariable [0-1] + The desired value of \f$\lambda^P\f$. Note at + \f$\lambda^P=1\f$ the curve is a horizontal line + with a value of 0. + */ + void setPassiveTorqueAngleCurveBlendingVariable( + double blendingVariable); + + /** + @param blendingVariable [0-1] + The desired value of \f$\lambda^V\f$. Note at + \f$\lambda^V=1\f$ the curve is a horizontal line + with a value of 1. + */ + void setTorqueAngularVelocityCurveBlendingVariable( + double blendingVariable); + + /** + @returns the scaling parameter that is applied to the + domain of the active-torque-angle curve. Note that + a value greater than 1 means that the curve has + been stretched. + */ + double getActiveTorqueAngleCurveAngleScaling() const; + + /** + @param angleScaling + Scale the domain of the active torque angle curve. + This scaling is applied so that the angle at which + the curve peaks remains unchanged. Note that an + angleScaling > 1 means that the typically bell-shaped + curve will be wider. + */ + void setActiveTorqueAngleCurveAngleScaling(double angleScaling); + + + /** + @param fittedParameters: the structure returned after + one of the optimization-based fitting functions from + TorqueMuscleFittingToolkit has been called. If the + fitting was successful, calling this function will + update all of the adjusted parameters. + */ + void setFittedParameters ( + const TorqueMuscleParameterFittingData &fittedParameters); + + + /** + Prints 2 csv files: + -# 'fileName' + '_variableLengthfixedVelocity': All of the + fields in TorqueMuscleInfo are recorded to file as the + jointAngle varies but the jointAngularVelocity is zero. + -#'fileName' + '_fixedLengthVariableVelocity': All of the fields + in TorqueMuscleInfo are recorded to file as the jointAngle is + fixed but the jointAngularVelocity varies. + + Each column has a header, so that you can tell what each + piece of data means. + + @param path: the path to the destination folder. Don't put + an '\' on the end. + @param fileNameWithoutExtension: the name of the file, but + without an extension. + @param numberOfSamplePoints: the number of sample points to + use in the files. + */ + void printJointTorqueProfileToFile( + const std::string& path, + const std::string& fileNameWithoutExtension, + int numberOfSamplePoints); + + std::string getName(); + void setName(std::string& name); + + private: + + bool mMuscleCurvesAreDirty; + void updateTorqueMuscleCurves(); + TorqueMuscleInfo mTmInfo; + TorqueMuscleSummary mTmSummary; + + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction mTaCurve; + double mTaLambda; + static const double mTaLambdaMax;// = 1.0 -> defined in the cc file + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction mTpCurve; + double mTpLambda; + static const double mTpLambdaMax;// = 0.0 -> defined in the cc file + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction mTvCurve; + double mTvLambda; + + ///This is set to the maximum eccentric value of the + /// torque-velocity curve so that the optimization problem + /// of solving for mTaLambda, mTvLambda & + /// mMaxActiveIsometricTorque is convex. This parameter is + /// updated in updateTorqueMuscleCurves + double mTvLambdaMax; + + + RigidBodyDynamics::Math::VectorNd mAnderson2007c1c2c3c4c5c6; + RigidBodyDynamics::Math::VectorNd mAnderson2007b1k1b2k2; + RigidBodyDynamics::Math::VectorNd mGymnastParams; + + DataSet::item mDataSet; + + bool mUseTabularOmegaMax; + bool mUseTabularMaxActiveIsometricTorque; + bool mUseTabularTorqueVelocityMultiplierAtHalfOmegaMax; + + double mTorqueVelocityMultiplierAtHalfOmegaMax; + + double mMaxActiveIsometricTorque; + double mAngleAtOneNormActiveTorque; + double mTaAngleScaling; + double mOmegaMax; + double mAngleAtOneNormPassiveTorque; + double mAngleAtSmallestNormPassiveTorque; + double mPassiveTorqueScale; + double mPassiveCurveAngleOffset; + + double mBetaMax; //passive damping coefficient + + double mSubjectHeightInMeters; + double mSubjectMassInKg; + double mScaleFactorAnderson2007; + + double mSignOfJointAngle; + double mSignOfConcentricAnglularVelocity; + double mSignOfJointTorque; + double mAngleOffset; + + std::string mMuscleName; + + double calcJointAngle(double fiberAngle) const; + double calcFiberAngle(double jointAngle) const; + double calcFiberAngularVelocity( + double jointAngularVelocity) const; + double calcJointAngularVelocity( + double fiberAngularVelocity) const; + + //const static RigidBodyDynamics::Math::MatrixNd& + //getAnderson2007ParameterMatrix(); + static double const Anderson2007Table3Mean[36][14]; + static double const Anderson2007Table3Std[36][14]; + static double const GymnastWholeBody[22][12]; + + + void calcTorqueMuscleDataFeatures( + RigidBodyDynamics::Math::VectorNd const &jointTorque, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAangularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double torqueVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleDataFeatures &tmf) const; + + /** + This function will calculate the angle at which the blendable + passive torque angle curve develops a desired torque. Note + that if the blendingVariable is set to 1.0 then the curve is + y = 0, and a NaN will be returned. + + @param normPassiveFiberTorque: the normalized passive fiber torque + @param blendingVariable: A variable between [0,1] where 0 + is equivalent to using the default passive-torque-angle + curve and 1 will set this curve to 0. Any value between + 0-1 will smoothly interpolate between these extremes. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @returns fiber angle in radians at which the + blendable passive curve developes the desired curve. + */ + double calcFiberAngleGivenNormalizedPassiveTorque( + double normPassiveFiberTorque, + double blendingVariable, + double passiveTorqueAngleCurveOffset) const; + + /** + This function will calculate the value of a blended smooth + segmented curve \f$\mathbf{f}(\mathbf{c}, a,\lambda,\lambda^{max})\f$, + where \f$\mathbf{c}\f$ is a smooth-segmented-curve + \f[ + \mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}()) = \mathbf{c}(a) + (1-\lambda) + \lambda\lambda^{max} + \f] + + Note + At the moment this function only returns first derivatives. + + @param curveArgument: [\f$-\infty,infty\f$] + the argument \f$a\f$ to the smooth-segmented-curve + \f$\mathbf{c}()\f$. Units: depend on the curve. + + @param blendingVariable: [0-1]. + the value of \f$\lambda\f$ the blending variable. + Unitless + + @param maximumBlendingValue: the scalar value the blended function + tends to with a blending variable \f$\lambda=1\f$. + + @param derivativeOrderArgument: [0,1]. + The order of the derivative of the + SmoothSegmentedFunction w.r.t. + its scalar input argument. + + @param derivativeOrderBlendingVariable: [0,1]. + This parameter is set to choose the derivative order + with respect to the blending variable. + + @param curve: the SmoothSegmentedCurve to evaluate. + + @returns the value of the blended curve or its derivative + */ + double calcBlendedCurveDerivative( + double curveArgument, + double blendingVariable, + double maximumBlendingValue, + unsigned int derivativeOrderArgument, + unsigned int derivativeOrderBlendingVariable, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction + const &curve) const; + + + /** + This function will calculate the inverse value of a blended smooth + segmented curve, that is \f$a\f$, in the function + \f$\mathbf{f}(\mathbf{c}, a,\lambda,\lambda^{max})\f$, + where \f$\mathbf{c}\f$ is a smooth-segmented-curve + \f[ + \mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}()) = \mathbf{c}(a) + (1-\lambda) + \lambda\lambda^{max} + \f] + + Note + At the moment this function only returns first derivatives. + + + @param blendedCurveValue: + the value of \f$\mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}())\f$ + + @param argGuess: + an initial guess for the value of \f$a\f$, which is necessary + as not all blended functions are monotonic. + + @param blendingVariable: [0-1]. + the value of \f$\lambda\f$ the blending variable. + Unitless + + @param maximumBlendingValue: the scalar value the blended function + tends to with a blending variable \f$\lambda=1\f$. + + @param curve: the SmoothSegmentedCurve to evaluate. + + @returns the value of the argument \f$a\f$ which will evaluate + to the given value of \f$\mathbf{f}(a,\lambda,\lambda^{max},\mathbf{c}())\f$ + */ + double calcInverseBlendedCurveValue( + double blendedCurveValue, + double argGuess, + double blendingVariable, + double maximumBlendingValue, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction + const &curve) const; + + /** + This function evaluates all of the quantities needed to + compute the torque the muscle is generating but nothing extra. + + + @param activation: the activation of the muscle which can + range from 0-1. + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: the maximum isometric torque + @param updTms: the torque-muscle-summary structure to be + updated. + */ + void updTorqueMuscleSummary( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleSummary &updTms) const; + /** + This function evaluates the curves values that are a part of the + torque-muscle-summary structure. + + @param fiberAngle: the angle of the muscle fiber (radians) + @param normFiberAngularVelocity: the normalized angular + velocity of the fiber ((rad/s)/(rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param updTms: the torque-muscle-summary structure to be + updated. + */ + void updTorqueMuscleSummaryCurveValues( + double fiberAngle, + double normFiberAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + TorqueMuscleSummary &updTms) const; + + /** + This function evaluates the curves values and derivatives in + the TorqueMuscleInfo structue that are functions of fiberAngle + and the normalized fiber angular velocity. + + @param activation: the activation of the muscle [0-1] + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: + the maximum active isometric torque the muscle can + generate + @param updTmi: the torque-muscle-info structure to be + updated. + */ + void updTorqueMuscleInfo( + double activation, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleInfo &updTmi) const; + + /** + This function evaluates all of the quantities needed to + compute the activation of the muscle given its torque. In this + case the activation assigned to the TorqueMuscleSummary + structure is allowed to go outside of the bounds [0,1]. + + @param jointTorque: the torque generated by the muscle at the + joint (Nm) + @param jointAngle: the angle of the joint(radians) + @param jointAngularVelocity: the angular velocity of the joint + ((rad/s)) + @param activeTorqueAngleBlendingVariable: the blending variable + associated with the active-torque-angle curve + @param passiveTorqueAngleBlendingVariable: the blending variable + associated with the passive-torque-angle curve + @param activeTorqueAngularVelocityBlendingVariable: the blending + variable associated with the torque-angular velocity + curve. + @param activeTorqueAngleAngleScaling: + the scaling of the angle. By default this is set to 1.0, + but if desired, the active torque angle curve can be + re-scaled to make it wider or narrower. This scaling is + done such that the angular location of peak + active-torque-angle multiplier does not change. + @param activeTorqueAngleAtOneNormTorque: + the angle at which the torque-angle curve hits a + normalized value of 1.0. + @param passiveTorqueAngleCurveOffset: the angular shift of the + passive-torque-angle curve. + @param maxAngularVelocity: + the maximum angular velocity of the muscle - used + to normalize the angular velocity prior to evaluating + the force-velocity curve. + @param maxActiveIsometricTorque: the maximum isometric torque + @param updTms: the torque-muscle-summary structure to be + updated. + + */ + void updInvertTorqueMuscleSummary( + double jointTorque, + double jointAngle, + double jointAngularVelocity, + double activeTorqueAngleBlendingVariable, + double passiveTorqueAngleBlendingVariable, + double activeTorqueAngularVelocityBlendingVariable, + double activeTorqueAngleAngleScaling, + double activeTorqueAngleAtOneNormTorque, + double passiveTorqueAngleCurveOffset, + double maxAngularVelocity, + double maxActiveIsometricTorque, + TorqueMuscleSummary &updTms) const; + + + +}; + + + + +} +} +} + + + +#endif diff --git a/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc new file mode 100755 index 0000000..8c09b36 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.cc @@ -0,0 +1,927 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunctionFactory.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + /* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ + +//============================================================================= +// INCLUDES +//============================================================================= + +#include "MuscleFunctionFactory.h" +#include +#include +#include +#include +#include + +using namespace std; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +//============================================================================= +// STATICS +//============================================================================= +//using namespace std; + + +static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample + //each Bezier corner section + +static double SMOOTHING = 0; //The amount of smoothing to use when fitting + //3rd order splines to the quintic Bezier + //functions +static bool DEBUG = true; //When this is set to true, each function's debug + //routine will be called, which ususally results + //in a text file of its output being produced + +static double UTOL = (double)std::numeric_limits::epsilon()*1e2; + +static double INTTOL = (double)std::numeric_limits::epsilon()*1e4; + +static int MAXITER = 20; +//============================================================================= +// UTILITY FUNCTIONS +//============================================================================= + +//============================================================================= +// MUSCLE CURVE FITTING FUNCTIONS +//============================================================================= +void MuscleFunctionFactory::createFiberActiveForceLengthCurve( + double x0, + double x1, + double x2, + double x3, + double ylow, + double dydx, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + double rootEPS = sqrt(std::numeric_limits::epsilon()); + + if( (!(x0>=0 && x1>x0+rootEPS && x2>x1+rootEPS && x3>x2+rootEPS) ) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve: " + << curveName + << ": This must be true: 0 < lce0 < lce1 < lce2 < lce3" + << endl; + assert(0); + abort(); + + } + + + if( !(ylow >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + <<": shoulderVal must be greater than, or equal to 0" + << endl; + assert(0); + abort(); + + } + + double dydxUpperBound = (1-ylow)/(x2-x1); + + + if( !(dydx >= 0 && dydx < dydxUpperBound) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": plateauSlope must be greater than 0 and less than " + << dydxUpperBound + << endl; + assert(0); + abort(); + } + + if( !(curviness >= 0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberActiveForceLengthCurve:" + << curveName + << ": curviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberActiveForceLengthCurve"); + + + + //Translate the users parameters into Bezier curves + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //The active force length curve is made up of 5 elbow shaped sections. + //Compute the locations of the joining point of each elbow section. + + //Calculate the location of the shoulder + double xDelta = 0.05*x2; //half the width of the sarcomere 0.0259, + //but TM.Winter's data has a wider shoulder than + //this + + double xs = (x2-xDelta);//x1 + 0.75*(x2-x1); + + //Calculate the intermediate points located on the ascending limb + double y0 = 0; + double dydx0 = 0; + + double y1 = 1 - dydx*(xs-x1); + double dydx01= 1.25*(y1-y0)/(x1-x0);//(y1-y0)/(x1-(x0+xDelta)); + + double x01 = x0 + 0.5*(x1-x0); //x0 + xDelta + 0.5*(x1-(x0+xDelta)); + double y01 = y0 + 0.5*(y1-y0); + + //Calculate the intermediate points of the shallow ascending plateau + double x1s = x1 + 0.5*(xs-x1); + double y1s = y1 + 0.5*(1-y1); + double dydx1s= dydx; + + //double dydx01c0 = 0.5*(y1s-y01)/(x1s-x01) + 0.5*(y01-y0)/(x01-x0); + //double dydx01c1 = 2*( (y1-y0)/(x1-x0)); + //double dydx01(1-c)*dydx01c0 + c*dydx01c1; + + //x2 entered + double y2 = 1; + double dydx2 = 0; + + //Descending limb + //x3 entered + double y3 = 0; + double dydx3 = 0; + + double x23 = (x2+xDelta) + 0.5*(x3-(x2+xDelta)); //x2 + 0.5*(x3-x2); + double y23 = y2 + 0.5*(y3-y2); + + //double dydx23c0 = 0.5*((y23-y2)/(x23-x2)) + 0.5*((y3-y23)/(x3-x23)); + //double dydx23c1 = 2*(y3-y2)/(x3-x2); + double dydx23 = (y3-y2)/((x3-xDelta)-(x2+xDelta)); + //(1-c)*dydx23c0 + c*dydx23c1; + + //Compute the locations of the control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,ylow,dydx0,x01,y01,dydx01,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x01,y01,dydx01,x1s,y1s,dydx1s,c); + RigidBodyDynamics::Math::MatrixNd p2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1s,y1s,dydx1s,x2, y2, dydx2,c); + RigidBodyDynamics::Math::MatrixNd p3 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2, y2, dydx2,x23,y23,dydx23,c); + RigidBodyDynamics::Math::MatrixNd p4 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x23,y23,dydx23,x3,ylow,dydx3,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,5), mY(6,5); + mX.col(0) = p0.col(0); + mX.col(1) = p1.col(0); + mX.col(2) = p2.col(0); + mX.col(3) = p3.col(0); + mX.col(4) = p4.col(0); + + mY.col(0) = p0.col(1); + mY.col(1) = p1.col(1); + mY.col(2) = p2.col(1); + mY.col(3) = p3.col(1); + mY.col(4) = p4.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x3,ylow,ylow,0,0,curveName); +} + +void MuscleFunctionFactory::createFiberForceVelocityCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + + if( !(fmaxE > 1.0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": fmaxE must be greater than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxC >= 0.0 && dydxC < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxC must be greater than or equal to 0 " + <<" and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxNearC > dydxC && dydxNearC <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxNearC must be greater than or equal to 0 " + << "and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(dydxIso > 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + << ": dydxIso must be greater than (fmaxE-1)/1 (" + << ((fmaxE-1.0)/1.0) + << ")" + << endl; + assert(0); + abort(); + } + + if( !(dydxE >= 0.0 && dydxE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve: " + << curveName + <<": dydxE must be greater than or equal to 0 " + << "and less than fmaxE-1 (" + << (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(!(dydxNearE >= dydxE && dydxNearE < (fmaxE-1))){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve" + << curveName + << ": dydxNearE must be greater than or equal to dydxE " + << "and less than fmaxE-1 (" << (fmaxE-1) + << ")" + << endl; + assert(0); + abort(); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberForceVelocityCurve"); + + //Translate the users parameters into Bezier point locations + double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); + double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); + + //Compute the concentric control point locations + double xC = -1; + double yC = 0; + + double xNearC = -0.9; + double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); + + double xIso = 0; + double yIso = 1; + + double xE = 1; + double yE = fmaxE; + + double xNearE = 0.9; + double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE); + + + RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); + RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); + RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); + RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); + + RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); + mX.col(0) = concPts1.col(0); + mX.col(1) = concPts2.col(0); + mX.col(2) = eccPts1.col(0); + mX.col(3) = eccPts2.col(0); + + mY.col(0) = concPts1.col(1); + mY.col(1) = concPts2.col(1); + mY.col(2) = eccPts1.col(1); + mY.col(3) = eccPts2.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,xC,xE,yC,yE,dydxC,dydxE,curveName); +} + + +void MuscleFunctionFactory::createFiberForceVelocityInverseCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Ensure that the inputs are within a valid range + if(! (fmaxE > 1.0 )){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve: " + << curveName + << ": fmaxE must be greater than 1" + << endl; + assert(0); + abort(); + } + + double SimTKSignificantReal = + pow((double)std::numeric_limits::epsilon(), 7.0/8.0); + + if(! (dydxC > SimTKSignificantReal && dydxC < 1 )){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxC must be greater than 0" + << "and less than 1" + << endl; + assert(0); + abort(); + + } + + if(! (dydxNearC > dydxC && dydxNearC < 1 )){ + std::stringstream errMsg; + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << ": dydxNearC must be greater than 0 " + << curveName + << " and less than 1" + << endl; + assert(0); + abort(); + } + + if(! (dydxIso > 1)){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxIso must be greater than or equal to 1" + << endl; + assert(0); + abort(); + } + + //double SimTKSignificantReal = + // pow(std::numeric_limits::epsilon(), 7.0/8.0); + + if(! (dydxE > SimTKSignificantReal && dydxE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxE must be greater than or equal to 0" + << " and less than fmaxE-1 (" << (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(! (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": dydxNearE must be greater than or equal to dydxE" + << "and less than fmaxE-1 ("<< (fmaxE-1) << ")" + << endl; + assert(0); + abort(); + } + + if(! (concCurviness <= 1.0 && concCurviness >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": concCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + if(! (eccCurviness <= 1.0 && eccCurviness >= 0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceVelocityInverseCurve " + << curveName + << ": eccCurviness must be between 0 and 1" + << endl; + assert(0); + abort(); + } + + std::string name = curveName; + name.append(".createFiberForceVelocityInverseCurve"); + + //Translate the users parameters into Bezier point locations + double cC = SegmentedQuinticBezierToolkit::scaleCurviness(concCurviness); + double cE = SegmentedQuinticBezierToolkit::scaleCurviness(eccCurviness); + + //Compute the concentric control point locations + double xC = -1; + double yC = 0; + + double xNearC = -0.9; + double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC); + + double xIso = 0; + double yIso = 1; + + double xE = 1; + double yE = fmaxE; + + double xNearE = 0.9; + double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE); + + + RigidBodyDynamics::Math::MatrixNd concPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xC, yC, dydxC, + xNearC, yNearC,dydxNearC,cC); + RigidBodyDynamics::Math::MatrixNd concPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC, + xIso, yIso, dydxIso, cC); + RigidBodyDynamics::Math::MatrixNd eccPts1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( xIso, yIso, dydxIso, + xNearE, yNearE, dydxNearE, cE); + RigidBodyDynamics::Math::MatrixNd eccPts2 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE, + xE, yE, dydxE, cE); + + RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4); + mX.col(0) = concPts1.col(0); + mX.col(1) = concPts2.col(0); + mX.col(2) = eccPts1.col(0); + mX.col(3) = eccPts2.col(0); + + mY.col(0) = concPts1.col(1); + mY.col(1) = concPts2.col(1); + mY.col(2) = eccPts1.col(1); + mY.col(3) = eccPts2.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mY,mX,yC,yE,xC,xE,1/dydxC,1/dydxE, curveName); + +} + +void MuscleFunctionFactory::createFiberCompressiveForcePennationCurve( + double phi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(phi0>0 && phi0<(M_PI/2.0)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": phi0 must be greater than 0, and less than Pi/2" + << endl; + assert(0); + abort(); + } + + if( !(k > (1.0/(M_PI/2.0-phi0))) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": k must be greater than " << (1.0/(M_PI/2.0-phi0)) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForcePennationCurve " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string name=curveName; + name.append(".createFiberCompressiveForcePennationCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = phi0; + double y0 = 0; + double dydx0 = 0; + double x1 = M_PI/2.0; + double y1 = 1; + double dydx1 = k; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + +void MuscleFunctionFactory:: + createFiberCompressiveForceCosPennationCurve( + double cosPhi0, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(cosPhi0>0 && cosPhi0 < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": cosPhi0 must be greater than 0, and less than 1" + << endl; + assert(0); + abort(); + } + + if( !(k < 1/cosPhi0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve " + << curveName + << ": k must be less than 0" + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceCosPennationCurve" + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string name=curveName; + name.append(".createFiberCompressiveForceCosPennationCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0; + double y0 = 1; + double dydx0 = k; + double x1 = cosPhi0; + double y1 = 0; + double dydx1 = 0; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + +void MuscleFunctionFactory::createFiberCompressiveForceLengthCurve( + double lmax, + double k, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(lmax>0) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": l0 must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(k < -(1.0/lmax)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": k must be less than " + << -(1.0/lmax) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberCompressiveForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string caller = curveName; + caller.append(".createFiberCompressiveForceLength"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0.0; + double y0 = 1; + double dydx0 = k; + double x1 = lmax; + double y1 = 0; + double dydx1 = 0; + + RigidBodyDynamics::Math::MatrixNd ctrlPts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,1), mY(6,1); + mX.col(0) = ctrlPts.col(0); + mY.col(0) = ctrlPts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX,mY,x0,x1,y0,y1,dydx0,dydx1,curveName); +} + + +void MuscleFunctionFactory::createFiberForceLengthCurve( + double eZero, + double eIso, + double kLow, + double kIso, + double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(eIso > eZero) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": The following must hold: eIso > eZero" + << endl; + assert(0); + abort(); + } + + if( !(kIso > (1.0/(eIso-eZero))) ){ + std::stringstream errMsg; + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kiso must be greater than 1/(eIso-eZero) (" + << (1.0/(eIso-eZero)) << ")" + << endl; + assert(0); + abort(); + } + + if( !(kLow > 0.0 && kLow < 1/(eIso-eZero)) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": kLow must be greater than 0 and less than" + << 1.0/(eIso-eZero) + << endl; + assert(0); + abort(); + } + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createFiberForceLength " + << curveName + << ": curviness must be between 0.0 and 1.0" + << endl; + assert(0); + abort(); + } + + std::string callerName = curveName; + callerName.append(".createFiberForceLength"); + + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double xZero = 1+eZero; + double yZero = 0; + + double xIso = 1 + eIso; + double yIso = 1; + + double deltaX = std::min(0.1*(1.0/kIso), 0.1*(xIso-xZero)); + + double xLow = xZero + deltaX; + double xfoot = xZero + 0.5*(xLow-xZero); + double yfoot = 0; + double yLow = yfoot + kLow*(xLow-xfoot); + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xZero, yZero, 0, + xLow, yLow, kLow,c); + + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xLow, yLow, kLow, + xIso, yIso, kIso, c); + RigidBodyDynamics::Math::MatrixNd mX(6,2); + RigidBodyDynamics::Math::MatrixNd mY(6,2); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, xZero, xIso, yZero, yIso, 0.0, kIso, curveName); +} + + + + +void MuscleFunctionFactory:: + createTendonForceLengthCurve( double eIso, double kIso, + double fToe, double curviness, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(eIso>0) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": eIso must be greater than 0, but " + << eIso << " was entered" + << endl; + assert(0); + abort(); + + } + + if( !(fToe>0 && fToe < 1) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": fToe must be greater than 0 and less than 1, but " + << fToe + << " was entered" + << endl; + assert(0); + abort(); + } + + if( !(kIso > (1/eIso)) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << ": kIso must be greater than 1/eIso, (" + << (1/eIso) << "), but kIso (" + << kIso << ") was entered" + << endl; + assert(0); + abort(); + } + + + if( !(curviness>=0 && curviness <= 1) ){ + cerr << "MuscleFunctionFactory::" + << "createTendonForceLengthCurve " + << curveName + << " : curviness must be between 0.0 and 1.0, but " + << curviness << " was entered" + << endl; + assert(0); + abort(); + } + + std::string callerName = curveName; + callerName.append(".createTendonForceLengthCurve"); + + //Translate the user parameters to quintic Bezier points + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 1.0; + double y0 = 0; + double dydx0 = 0; + + double xIso = 1.0 + eIso; + double yIso = 1; + double dydxIso = kIso; + + //Location where the curved section becomes linear + double yToe = fToe; + double xToe = (yToe-1)/kIso + xIso; + + + //To limit the 2nd derivative of the toe region the line it tends to + //has to intersect the x axis to the right of the origin + double xFoot = 1.0+(xToe-1.0)/10.0; + double yFoot = 0; + double dydxToe = (yToe-yFoot)/(xToe-xFoot); + + //Compute the location of the corner formed by the average slope of the + //toe and the slope of the linear section + double yToeMid = yToe*0.5; + double xToeMid = (yToeMid-yIso)/kIso + xIso; + double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot); + + //Compute the location of the control point to the left of the corner + double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); + double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot); + + + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + xToeCtrl,yToeCtrl,dydxToeMid,c); + RigidBodyDynamics::Math::MatrixNd p1 = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); + RigidBodyDynamics::Math::MatrixNd mX(6,2); + RigidBodyDynamics::Math::MatrixNd mY(6,2); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, x0, xToe, y0, yToe, dydx0, dydxIso, curveName); + +} diff --git a/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h new file mode 100755 index 0000000..ece6344 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/MuscleFunctionFactory.h @@ -0,0 +1,832 @@ +#ifndef MUSCLEFUNCTIONFACTORY_H_ +#define MUSCLEFUNCTIONFACTORY_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: SmoothSegmentedFunctionFactory.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +/* + Update: + This is a port of the original code so that it will work with + the multibody code RBDL written by Martin Felis. + + Author: + Matthew Millard + + Date: + Nov 2015 + +*/ +#include "../geometry/SmoothSegmentedFunction.h" +#include "../geometry/SegmentedQuinticBezierToolkit.h" + +#include +#include +#include +#include + + + +/** +This is a class that acts as a user friendly wrapper to QuinticBezerCurveSet +to build specific kinds of physiologically plausible muscle curves using C2 +continuous sets of quintic Bezier curves. This class has been written there did +not exist a set of curves describing muscle characteristics that was: + +1. Physiologically Accurate +2. Continuous to the second derivative +3. Parameterized in a physically meaningful manner + +For example, the curves employed by Thelen (Thelen DG(2003). Adjustment of +Muscle Mechanics Model Parameters to Simulate Dynamic Contractions in Older +Adults. ASME Journal of Biomechanical Engineering (125).) are parameterized in a +physically meaningful manner, making them easy to use. However there are +many shortcomings of these curves: + +1. The tendon and parallel element are not C2 continuous, making them slow to + simulate and likely not physiologically accurate. +2. The active force length curve approaches does not achieve its minimum value + at a normalized fiber length of 0.5, and 1.5. +3. The force velocity curve is not C2 continuous at the origin. As it is + written in the paper the curve is impossible to use with an equilibrium model + because it is not invertible. In addition the force-velocity curve actually + increases in stiffness as activation drops - a very undesirable property given + that many muscles are inactive at any one time. + +The muscle curves used in the literature until 2012 have been hugely influenced +by Thelen's work, and thus similar comments can easily be applied to just about +every other musculoskeletal simulation. + +Another example is from Miller (Miller,RH(2011).Optimal Control of +Human Running. PhD Thesis). On pg 149 a physiolgically plausible force velocity +curve is specified that gives the user the ability to change the concentric +curvature to be consistent with a slow or a fast twitch musle. This curve is +not C2 continuous at the origin, but even worse, it contains singularities in +its parameter space. Since these parameters do not have a physical +interpretation this model is difficult to use without accidentically creating a +curve with a singularity. + +With this motivation I set out to develop a class that could generate muscle +characteristic curves with the following properties: + +1. Physiologically Accurate +2. Continuous to the second derivative +3. Parameterized in a physically meaningful manner +4. Monotonicity for monotonic curves +5. Computationally efficient + +These goals were surprisingly time consuming achieve, but these goals have been +achieved using sets of C2 continuous quintic Bezier curves. The resulting +muscle curve functions in this class take parameters that would be intuitive to +biomechanists who simulate human musculoskeletal systems, and returns a +SmoothSegmentedFunction which is capable of evaluating the value, and +derivatives of the desired function (or actually relation as +the case may be). + +Each curve is made up of one or more C2 quintic Bezier curves x(u), +and y(u), with linearily extrapolated ends as shown in the figure below. These +quintic curves span 2 points, and achieve the desired derivative at its end +points. The degree of curviness can be varied from 0 to 1 (0, 0.75 and 1.0 are +shown in the figure in grey, blue and black respectively), and will make the +curve approximate a line when set to 0 (grey), and approximate a curve that +hugs the intersection of the lines that are defined by the end points locations +and the slopes at the end of each curve segment (red lines). Although you do +not need to set all of this information directly, for some of the curves it is +useful to know that both the slope and the curviness parameter may need to be +altered to achieve the desired shape. + + +\image html fig_GeometryAddon_quinticCornerSections.png + + + +Computational Cost Details +All computational costs assume the following operation costs: + +\verbatim +Operation Type : #flops +*,+,-,=,Boolean Op : 1 + / : 10 + sqrt: 20 + trig: 40 +\endverbatim + +These relative weightings will vary processor to processor, and so any of +the quoted computational costs are approximate. + + RBDL Port Notes +The port of this code from OpenSim has been accompanied by a few changes: + +1. The 'calcIntegral' method has been removed. Why? This function + relied on having access to a variable-step error controlled + integrator. There is no such integrator built into RBDL. Rather + than add a dependency (by using Boost perhaps) this functionality + has been removed. + +2. The function name .printMuscleCurveToFile(...) has been changed + to .printCurveToFile(). + + + +@author Matt Millard +@version 0.0 + +*/ +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + +class MuscleFunctionFactory +{ + + + public: + + // friend class SmoothSegmentedFunction; + + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active force length curve. + + + @param lce0 Normalized fiber length at the left-most shoulder of the + active force-length curve. The value of the active force + length curve for lce < lce0 will be equal to the value + set in shoulderVal. Normally lce0 is approximately 0.5 + + @param lce1 Normalized fiber length at the transition point between + the ascending limb and the plateau region of the active + force length curve. + + @param lce2 Normalized fiber length at the maximum active force length + curve value of 1. Normally lce2 is by definition 1. + + @param lce3 Normalized fiber length of the at the right most shoulder + of the active-force length curve. The value of the active + force length curve for lce > lce2 will be equal to the + value of shoulderVal. Normally lce3 is approximately 1.5 + + @param minActiveForceLengthValue + The minimum value of the active force length + curve. A physiological non-equibrium muscle model + would have this value set to 0. An equilibrium + muscle model would have a non-zero lower bound on + this value of 0.1 typically. shoulderVal must be + greater than, or equal to 0. + + @param plateauSlope The slope of the plateau of the active force + length curve between lce1 and lce2. This parameter + can vary depending on the muscle model, but a + value of 0.8616 is a good place to start. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0 will yield an active + force length curve that is composed of slightly curved + line segments. A value of 1 will yield an active force + length curve that is smoothly rounded. + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberActiveForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + if these conditions aren't met + -0 < lce0 < lce1 < lce2 < lce3 + -shoulderVal >= 0 + -0 <= plateauSlope < (1/(lce3-lce2)) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_falCurve.png + + + Conditions: + + Computational Costs + \verbatim + ~20,500 flops + \endverbatim + + Example: + @code + double lce0 = 0.5; + double lce1 = 0.75; + double lce2 = 1; + double lce3 = 1.5; + double shoulderVal = 0.1; + double plateauSlope = 0.75; + double curviness = 0.9; + + SmoothSegmentedFunction fiberfalCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberActiveForceLengthCurve(lce0, lce1, lce2, lce3, + shoulderVal, plateauSlope, curviness,"test", fiberfalCurve); + fiberfalCurve.printCurveToFile(); + @endcode + + + */ + static void createFiberActiveForceLengthCurve( + double lce0, + double lce1, + double lce2, + double lce3, + double minActiveForceLengthValue, + double plateauSlope, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry + ::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continous (continuous to the second + derivative) force velocity curve of a single muscle fiber. The main + function of this element is to model the amount the force enhancement or + attenuation that is associated with contracting at a particular velocity. + + @param fmaxE The normalized maximum force the fiber can generate when + is being stretched. This value is reported to range + between 1.1 and 1.8 in the literature, though all values + are above 1. + + @param dydxC The slope of the fv(dlce(t)/dt) curve at the maximum + normalized concentric contraction velocity. Although + physiologically the value of dydxC at the maximum + concentric contracton velocity is by definition 0, a value + of 0 is often used. If you are using an equilbrium type + model this term must be positive and greater than zero so + that the fv curve can be inverted. +

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

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

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

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

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

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

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

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

+ + + @param concCurviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). This parameter affects only + the concentric side of the fv curve. + + @param eccCurviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). This parameter affects only + the eccentric side of the fv curve. + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberForceVelocityCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless these conditions are met + -0 <= dydxC < 1 + -dydxC < dydxNearC < 1 + -1 < dydxIso + -dydxE < (fmaxE-1) + -dydxE < dydxNearC < (fmaxE-1) + -0<= concCurviness <=0 + -0 <= eccCurviness <= 0 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png + + + + Computational Costs + \verbatim + ~8,200 flops + \endverbatim + + Example: + @code + double fmaxE = 1.8; + double dydxC = 0.1; + double dydxNearC = 0.25; + double dydxE = 0.1; + double dydxNearE = 0.15; + double dydxIso= 5; + double concCurviness = 0.1; + double eccCurviness = 0.75; + + SmoothSegmentedFunction fiberFVCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberForceVelocityCurve(fmaxE, + dydxC, dydxNearC, dydxIso, dydxE, dydxNearE, + concCurviness, eccCurviness,"test", fiberFVCurve); + fiberFVCurve.printCurveToFile(); + @endcode + */ + static void createFiberForceVelocityCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continuous (continuous to the 2nd + derivative) inverse curve that the function + createFiberForceVelocityCurve generates. The inverse force velocity + curve is required by every equilibrium muscle model in order to compute + the derivative of fiber velocity. To generate the inverse force velocity + curve simply call this function with EXACTLY the same parameter values + that you used to generate the force velocity curve. See the parameter + descriptions for createFiberForceVelocityCurve, as the parameters for + the inverse function are identical. The curve name should be different, + however, because this is an inverse curve + (e.g. "bicep_fiberForceVelocityInverseCurve") + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png + + */ + static void createFiberForceVelocityInverseCurve( + double fmaxE, + double dydxC, + double dydxNearC, + double dydxIso, + double dydxE, + double dydxNearE, + double concCurviness, + double eccCurviness, + const std::string& muscleName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This element will generate a C2 continuous (continuous to the 2nd + derivative) compressive force profile curve as a function of pennation. + A muscle model with this element usually places this element parallel to + the fiber.The main function of this element is to prevent the fiber from + achieving a pennation angle of pi/2 radians. This type of element is + necessary for a parallelogram pennated equilibrium muscle models because + without it, the muscle model can deform to the point where a pennation + angle of pi/2 radians is reached, which causes a singularity in the + model. + + + @param phi0 The pennation angle at which the compressive force element + starts to engage . When the pennation angle is greater than + phi0, the compressive element is generating a force. When the + pennation angle is less than phi0, the compressive element + generates no force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when the fiber is pennated by 90 degrees + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForcePennationCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -0 < phi0 < SimTK::Pi/2 + -kiso > 1/(SimTK::Pi/2-phi0) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double phi0 = (SimTK::Pi/2)*(8.0/9.0); + double kiso = 8.389863790885878; + double c = 0.0; + + SmoothSegmentedFunction fiberCEPhiCurve = SmoothSegmentedFunction(); + MuscleFunctionFactory:: + createFiberCompressiveForcePennationCurve(phi0,kiso,c,"test",fiberCEPhiCurve); + fiberCEPhiCurve.printCurveToFile(); + @endcode + */ + static void createFiberCompressiveForcePennationCurve( + double phi0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This element will generate a C2 continuous (continuous to the 2nd + derivative) compressive force profile curve as a function of + cos(pennation). + + A muscle model with this element usually places this element in line + with the tendon. The main function of this element is to prevent the + fiber from achieving a pennation angle of pi/2 radians. This type of + element is necessary for a parallelogram pennated muscle models because + without it, the muscle model can deform to the point where a pennation + angle of pi/2 radians is reached, which causes a singularity in the + model. + + + @param cosPhi0 The cosine of the pennation angle at which the + compressive force element starts to engage. When the + cos of the pennation angle is greater than cosPhi0, the + compressive element generates no force. When cos of the + pennation angle is less than cosPhi0, the compressive + element generates a compressive force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when cosPhi is zero. This parameter must be + negative + cos + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForceCosPennationCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met: + -0 < cosPhi0 + -kiso > 1/(cosPhi0) + -0 <= curviness <= 1 + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double cosPhi0 = cos( (80.0/90.0)*SimTK::Pi/2); + double kiso = -1.2/(cosPhi0); + double c = 0.5; + + SmoothSegmentedFunction fiberCECosPhiCurve = MuscleFunctionFactory:: + createFiberCompressiveForceCosPennationCurve(cosPhi0,kiso,c,"test"); + fiberCEPhiCurve.printCurveToFile(); + @endcode + + + + */ + static void createFiberCompressiveForceCosPennationCurve( + double cosPhi0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + /** + This element will generate a C2 continous (continuous to the second + derivative) curve that models a compressive force profile that is a + function of fiber length. The main function of + this element is to prevent the fiber from achieving an unrealistically + short length. This type of element is necessary for equilibrium-type + muscle models because of the editing that is done to the active force + length curve that endows an equilibrium model fiber with the ability to + to generate force when a physiological fiber cannot. + + + + @param l0 The normalized fiber length at which the compressive element + starts to engage. When the fiber is shorter than l0, the + compressive element is generating a force. When the fiber + length is longer than l0, the compressive element generates + no force. + + @param kiso This is the maximum stiffness of the compressive element, + which occurs when the fiber has a length of 0, under a load + of 1 maximum isometric unit of force. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it + (e.g. "bicep_fiberCompressiveForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -e0 > 0 + -kiso > 1/(e0) + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double lmax = 0.6; + double kiso = -8.389863790885878; + double c = 0.1;//0.0; + + SmoothSegmentedFunction fiberCECurve = MuscleFunctionFactory:: + createFiberCompressiveForceLengthCurve(lmax,kiso,c,"test"); + fiberCECurve.printCurveToFile(); + @endcode + + */ + static void createFiberCompressiveForceLengthCurve( + double l0, + double kiso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This function will generate a C2 continuous curve that fits a fiber's + tensile force length curve. + + @param eZero The fiber strain at which the fiber begins to develop force. + Thus an e0 of 0.0 means that the fiber will start to develop + passive force when it has a normalized length of 1.0. Note + that e0 can be postive or negative. + + @param eIso The fiber strain at which the fiber develops 1 unit of + normalized force (1 maximum isometric force). Note that the + '1' is left off. Thus an e0 of 0.6 means that the fiber + will develop an 1 normalized force unit when it is strained + by 60% of its resting length, or to a normalized length of + 1.6 + + @param kLow The normalized stiffness (or slope) of the fiber curve + close to the location where the force-length curve + approaches a normalized force of 0. This is usually + chosen to be a small, but non-zero fraction of kIso + (kLow = 0.025 kIso is typical). + + @param kIso The normalized stiffness (or slope) of the fiber curve + when the fiber is strained by eIso (or has a length of + 1+eIso) under a load of 1 maximum isometric unit of force. + + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_fiberForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met + -eIso > eZero + -kIso > 1/(eIso-eZero) + -0 < kLow < kIso + -0 <= curviness <= 1 + + \image html fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double eIso = 0.6; + double eZero = 0.0; + double kIso = 4.0/(eIso-eZero); + double kNearZero = 0.025*kIso + double c = 0.5; + + SmoothSegmentedFunction fiberFLCurve + = MuscleFunctionFactory:: + createFiberForceLengthCurve(eZero, eIso, + kLow, kIso, c,"test"); + fiberFLCurve.printCurveToFile(); + @endcode + + */ + static void createFiberForceLengthCurve( + double eZero, + double eIso, + double kLow, + double kIso, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + Will generate a C2 continous (continuous to the second derivative) + curve in a MuscleFunctionObject object that fits a tendon's tensile + force length curve. + + + + @param eIso The tendon strain at which the tendon develops 1 unit + of normalized force (1 maximum isometric force). Note that + the'1' is left off. Thus an e0 of 0.04 means that the tendon + will develop an 1 normalized force unit when it is strained + by 4% of its resting length, at a normalized length of + 1.04 + + @param kIso The normalized stiffness (or slope) of the tendon + curve when the tendon is strained by e0 + (or has a length of 1+e0) under a load of 1 maximum + isometric unit of force. + + @param fToe The normalized force at which the tendon smoothly + transitions from the curved low stiffness region to + the linear stiffness region. + + @param curviness The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow) + + @param curveName The name of the muscle this curve applies to. This + curve name should have the name of the muscle and the + curve in it (e.g. "bicep_tendonForceLengthCurve") + sothat if this curve ever causes an exception, a + userfriendly error message can be displayed to the + end user to help them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + \b aborts \b + unless the following conditions are met: + -0 < fToe < 1 + -e0 > 0 + -kiso > 1/e0 + -0 <= curviness <= 1 + + + \image html fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png + + + Computational Costs + \verbatim + ~4,100 flops + \endverbatim + + Example: + @code + double e0 = 0.04; + double kiso = 42.79679348815859; + double fToe = 1.0/3.0 + double c = 0.75; + + SmoothSegmentedFunction* tendonCurve = MuscleFunctionFactory:: + createTendonForceLengthCurve( + e0,kiso,fToe,c,"test"); + tendonCurve.printCurveToFile(); + @endcode + + + */ + static void createTendonForceLengthCurve(double eIso, + double kIso, + double fToe, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry:: + SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + + +}; + +} +} +} + +#endif //MUSCLEFUNCTIONFACTORY_H_ diff --git a/3rdparty/rbdl/addons/muscle/NOTICE b/3rdparty/rbdl/addons/muscle/NOTICE new file mode 100755 index 0000000..e12de15 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/NOTICE @@ -0,0 +1,22 @@ +Author: + Matthew Millard + +Date: + July 2016 + + +Notice: these files + + -MuscleFunctionFactory.cc + -MuscleFunctionFactory.h + -csvtools.h + -csvtools.cc + -tests/testMuscleFunctionFactory.cc + +were originally part of OpenSim and have been ported +over to RBDL with some modification. These files are licenced under the +APACHE 2.0 license which, like the zlib license, is quite liberal. The +full licence can be found in this folder in the file "LICENSE_APACHE-2.0.txt" +and online here: + +http://www.apache.org/licenses/LICENSE-2.0.txt diff --git a/3rdparty/rbdl/addons/muscle/README.md b/3rdparty/rbdl/addons/muscle/README.md new file mode 100755 index 0000000..5905145 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/README.md @@ -0,0 +1,55 @@ +@brief muscle - a set of functions and classes for simulation musculotendon + dynamics. This addon is maintained by Matthew Millard, so if + you have problems with it email him. + +@author Matthew Millard + +\copyright 2016 Matthew Millard + +\b Requirements +This addon depends on the geometry addon + +\b Description + This addon currently contains an optimized library for creating specialized + curves that describe phenomenological curves for line-type muscles + -fiber active-force-length curve + -fiber force-velocity curve + -fiber passive-force-length curve + -tendon force-length curve + and torque-type muscles + -active torque-angle curve + -active torque-velocity curve + -passive torque-angle curve + In addition, there is a class that can be used to package the memory and + functions required to model torque muscles: Millard2016TorqueMuscle. + +\b Future Development +In the near future this library will also contain + +1. Torque-type muscle models with + a. Elastic tendons + b. Short-range-stiffness + +2. An implementation of the line-type muscle Millard2012Equilibrium muscle model + which features the option of using rigid and elastic tendons, and a damped/ + undamped formulation. + +3. An novel implemenation of 2D muscle wrapping using an obstacle set of + smooth convex shapes described using Pythagorean hodographs. + +\b Licensing +The following files have been ported over from OpenSim and Simbody and as such +are licenced under the Apache 2.0 Licence: + +SmoothSegmentedFunctionFactory.h +SmoothSegmentedFunctionFactory.cc + +The Apache License is very similar to the zlib license and is quite liberal. +Licensed under the Apache License, Version 2.0 (the "License"); you may +not use this file except in compliance with the License. You may obtain a +copy of the License at http://www.apache.org/licenses/LICENSE-2.0. + +The remaining code has been written from scratch and is licenced under the +zlib license. See the LICENSE file for details. + + diff --git a/3rdparty/rbdl/addons/muscle/TorqueMuscleFittingToolkit.cc b/3rdparty/rbdl/addons/muscle/TorqueMuscleFittingToolkit.cc new file mode 100644 index 0000000..1f61fcb --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/TorqueMuscleFittingToolkit.cc @@ -0,0 +1,992 @@ +#include "TorqueMuscleFittingToolkit.h" + +#include "coin/IpIpoptApplication.hpp" + +using namespace Ipopt; +static double EPSILON = std::numeric_limits::epsilon(); +static double SQRTEPSILON = sqrt(EPSILON); + + +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; + + +void TorqueMuscleFittingToolkit:: + fitTorqueMuscleParameters( + Millard2016TorqueMuscle const &tqMcl, + VectorNd const &jointAngle, + VectorNd const &jointAngularVelocity, + VectorNd const &jointTorque, + double activationUpperBound, + double passiveTorqueAngleMultiplierUpperBound, + TorqueMuscleParameterFittingData ¶metersOfBestFit, + bool verbose) +{ + + if(tqMcl.mMuscleCurvesAreDirty){ + Millard2016TorqueMuscle* mutableTqMcl = + const_cast(&tqMcl); + mutableTqMcl->updateTorqueMuscleCurves(); + } + + + assert(jointAngle.rows() > 1); + assert(jointAngularVelocity.rows() > 1); + assert(jointTorque.rows() > 1); + + unsigned int nrows = jointAngle.rows(); + + double activationLowerBound = 0.0; + assert(jointAngularVelocity.rows() == nrows); + assert(jointTorque.rows() == nrows); + assert(activationUpperBound > 0 && activationUpperBound <= 1.0); + assert(activationLowerBound >= 0 && activationLowerBound= 0){ + jointTorqueSingleSided[i] = jointTorque[i]; + }else{ + jointTorqueSingleSided[i] = 0.0; + } + } + + double tiso = tqMcl.getMaximumActiveIsometricTorque(); + double omegaMax = tqMcl.getMaximumConcentricJointAngularVelocity(); + + double maxConcentricAngularVelocityData = 0; + for(unsigned int i=0; i fabs(maxConcentricAngularVelocityData)){ + maxConcentricAngularVelocityData = fabs(jointAngularVelocity[i]); + } + } + + //Set ta and tv blending variables to a small finite value so that + //it is possible to solve for an activation of finite value when + //calcTorqueMuscleDataFeatures is called + double taLambda = sqrt(SQRTEPSILON); + double tvLambda = sqrt(SQRTEPSILON); + if(tqMcl.getActiveTorqueAngleCurveBlendingVariable() > taLambda){ + taLambda = tqMcl.getActiveTorqueAngleCurveBlendingVariable(); + } + if(tqMcl.getTorqueAngularVelocityCurveBlendingVariable() > tvLambda){ + tvLambda = tqMcl.getTorqueAngularVelocityCurveBlendingVariable(); + } + + double tpLambda = 0.0; + + double taAngleScaling = tqMcl.mTaAngleScaling; + double taAngleAtOneNormTorque = tqMcl.mAngleAtOneNormActiveTorque; + double tvOmegaMaxScale = 1.0; + double tpOffset = 0.0; + + double objectiveValue = 0.0; + double constraintError = 0.0; + + TorqueMuscleDataFeatures tmf; + + omegaMax = fabs(tqMcl.getMaximumConcentricJointAngularVelocity() + *tvOmegaMaxScale); + + //If the optimization run fails, the values from tmf are passed out to + //the user to help them debug the problem. + tqMcl.calcTorqueMuscleDataFeatures( + jointTorqueSingleSided,jointAngle,jointAngularVelocity, + taLambda,tpLambda,tvLambda, + taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax, + tiso,tmf); + + + Millard2016TorqueMuscle* mutableTqMcl = + const_cast(&tqMcl); + + SmartPtr fittingProblem + = new FitTorqueMuscleParameters( + jointAngle, + jointAngularVelocity, + jointTorqueSingleSided, + activationUpperBound, + passiveTorqueAngleMultiplierUpperBound, + taLambda, + tvLambda, + *mutableTqMcl); + + SmartPtr app = IpoptApplicationFactory(); + + app->RethrowNonIpoptException(true); + double solnTol = SQRTEPSILON; + app->Options()->SetNumericValue("tol",solnTol); + app->Options()->SetStringValue("mu_strategy","adaptive"); + + if(!verbose){ + app->Options()->SetIntegerValue("print_level",0); + } + + + ApplicationReturnStatus status; + + objectiveValue = std::numeric_limits::infinity(); + constraintError= std::numeric_limits::infinity(); + bool converged = false; + + double taAngleScaleStart = 1.0; + double tvOmegaMaxScaleStart =fabs(1.1*maxConcentricAngularVelocityData + /tqMcl.getMaximumConcentricJointAngularVelocity()); + + double tisoScale = 1.0; + bool updateStart = false; + + status = app->Initialize(); + if(status == Solve_Succeeded){ + status = app->OptimizeTNLP(fittingProblem); + if(status == Solve_Succeeded){ + converged = true; + updateStart = true; + taAngleScaling = fittingProblem + ->getSolutionActiveTorqueAngleAngleScaling(); + tvOmegaMaxScale = fittingProblem + ->getSolutionTorqueAngularVelocityOmegaMaxScale(); + tpLambda = fittingProblem + ->getSolutionPassiveTorqueAngleBlendingParameter(); + tpOffset = fittingProblem + ->getSolutionPassiveTorqueAngleCurveOffset(); + tisoScale = fittingProblem + ->getSolutionMaximumActiveIsometricTorqueScale(); + + tiso = tisoScale * tiso; + omegaMax = fabs(tvOmegaMaxScale + *tqMcl.getMaximumConcentricJointAngularVelocity()); + + objectiveValue = fittingProblem->getObjectiveValue(); + constraintError= fittingProblem->getConstraintError().norm(); + + tqMcl.calcTorqueMuscleDataFeatures( + jointTorqueSingleSided,jointAngle,jointAngularVelocity, + taLambda,tpLambda,tvLambda, + taAngleScaling,taAngleAtOneNormTorque,tpOffset,omegaMax, + tiso,tmf); + } + } + + + if(converged){ + parametersOfBestFit.fittingConverged = true; + parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive); + + parametersOfBestFit.indexAtMaximumActivation=tmf.indexOfMaxActivation; + parametersOfBestFit.indexAtMinimumActivation=tmf.indexOfMinActivation; + parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier + =tmf.indexOfMaxPassiveTorqueAngleMultiplier; + + parametersOfBestFit.activeTorqueAngleBlendingVariable = taLambda; + parametersOfBestFit.torqueVelocityBlendingVariable = tvLambda; + parametersOfBestFit.passiveTorqueAngleBlendingVariable = tpLambda; + parametersOfBestFit.passiveTorqueAngleCurveOffset = tpOffset; + parametersOfBestFit.maximumActiveIsometricTorque = tiso; + parametersOfBestFit.activeTorqueAngleAngleScaling = taAngleScaling; + parametersOfBestFit.maximumAngularVelocity = fabs(omegaMax); + + parametersOfBestFit.summaryDataAtMaximumActivation + =tmf.summaryAtMaxActivation; + parametersOfBestFit.summaryDataAtMinimumActivation + =tmf.summaryAtMinActivation; + parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier + =tmf.summaryAtMaxPassiveTorqueAngleMultiplier; + }else{ + parametersOfBestFit.fittingConverged = false; + parametersOfBestFit.isTorqueMuscleActive = !(tmf.isInactive); + + parametersOfBestFit.indexAtMaximumActivation + =numeric_limits::signaling_NaN(); + parametersOfBestFit.indexAtMinimumActivation + =numeric_limits::signaling_NaN(); + parametersOfBestFit.indexAtMaxPassiveTorqueAngleMultiplier + =numeric_limits::signaling_NaN(); + + parametersOfBestFit.activeTorqueAngleBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.passiveTorqueAngleBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.passiveTorqueAngleCurveOffset + = numeric_limits::signaling_NaN(); + parametersOfBestFit.torqueVelocityBlendingVariable + = numeric_limits::signaling_NaN(); + parametersOfBestFit.maximumActiveIsometricTorque + = numeric_limits::signaling_NaN(); + parametersOfBestFit.activeTorqueAngleAngleScaling + = numeric_limits::signaling_NaN(); + parametersOfBestFit.maximumAngularVelocity + = numeric_limits::signaling_NaN(); + + parametersOfBestFit.summaryDataAtMaximumActivation + =tmf.summaryAtMaxActivation; + parametersOfBestFit.summaryDataAtMinimumActivation + =tmf.summaryAtMinActivation; + parametersOfBestFit.summaryDataAtMaximumPassiveTorqueAngleMultiplier + =tmf.summaryAtMaxPassiveTorqueAngleMultiplier; + } + +} + + + +FitTorqueMuscleParameters:: + FitTorqueMuscleParameters( + const RigidBodyDynamics::Math::VectorNd &jointAngle, + const RigidBodyDynamics::Math::VectorNd &jointAngularVelocity, + const RigidBodyDynamics::Math::VectorNd &jointTorque, + double maxActivation, + double maxPassiveTorqueAngleMultiplier, + double taLambda, + double tvLambda, + Millard2016TorqueMuscle &tqMcl): + mJointAngle(jointAngle), + mJointAngularVelocity(jointAngularVelocity), + mJointTorque(jointTorque), + mTqMcl(tqMcl) +{ + assert(jointTorque.rows() == jointAngle.rows()); + assert(jointTorque.rows() == jointAngularVelocity.rows()); + + //Initialize all member variables + mN = 5; + mM = 3*jointTorque.rows(); + + double angleMin = std::numeric_limits< double >::max(); + double omegaMin = std::numeric_limits< double >::max(); + double angleMax = -std::numeric_limits< double >::max(); + double omegaMax = -std::numeric_limits< double >::max(); + double tauMax = 0; + + for(unsigned int i=0; i angleMax){ + angleMax = mJointAngle[i]; + } + if(mJointAngularVelocity[i] < omegaMin){ + omegaMin = mJointAngularVelocity[i]; + } + if(mJointAngularVelocity[i] > omegaMax){ + omegaMax = mJointAngularVelocity[i]; + } + if(mJointTorque[i]*mTqMcl.getJointTorqueSign() > tauMax){ + tauMax = mJointTorque[i]*mTqMcl.getJointTorqueSign(); + } + } + + double tvScale = max(fabs(omegaMin),fabs(omegaMax)) + /fabs(mTqMcl.getMaximumConcentricJointAngularVelocity()); + double taScale = fabs(angleMax-angleMin) + /mTqMcl.getActiveTorqueAngleCurveWidth(); + + double tauScale = tauMax/fabs(mTqMcl.getMaximumActiveIsometricTorque()); + + + + + mConIdxTauActMaxStart = 0; + mConIdxTauActMaxEnd = jointTorque.rows()-1; + mConIdxTauActMinStart = jointTorque.rows(); + mConIdxTauActMinEnd = 2*jointTorque.rows()-1; + mConIdxTauPassiveStart = 2*jointTorque.rows(); + mConIdxTauPassiveEnd = 3*jointTorque.rows()-1; + + mMaxActivation = maxActivation; + mMinActivation = 0.0; + mMaxTp = maxPassiveTorqueAngleMultiplier; + mTauIso = mTqMcl.getMaximumActiveIsometricTorque(); + mTaLambda = taLambda; + mTvLambda = tvLambda; + mTaAngleAtOneNormTorque = mTqMcl.mAngleAtOneNormActiveTorque; + mOmegaMax = mTqMcl.mOmegaMax; + + mTaAngleScaleStart = max(taScale, + mTqMcl.getActiveTorqueAngleCurveAngleScaling()); + mTvOmegaMaxScaleStart = max(tvScale, 1.); + mTpLambdaStart = mTqMcl.getPassiveTorqueAngleCurveBlendingVariable(); + mTpAngleOffsetStart = mTqMcl.getPassiveCurveAngleOffset(); + mTauScalingStart = max(tauScale, 1.); + + mTaAngleScaleLB = 1.; + mTvOmegaMaxScaleLB = 1.; + mTpLambdaLB = 0.; + mTpAngleOffsetLB = -M_PI*0.5; + mTauScalingLB = 1.0; + + mTaAngleScaleUB = 2.0e19; + mTvOmegaMaxScaleUB = 2.0e19; + mTpLambdaUB = 1.0; + mTpAngleOffsetUB = M_PI*0.5; + mTauScalingUB = 2.0e19; + + mIndexTaAngleScale = 0; + mIndexTvOmegaMaxScale = 1; + mIndexTpLambda = 2; + mIndexTpOffset = 3; + mIndexTauScaling = 4; + + //Initialize vectors + mConstraintErrors.resize(mM); + mXOffset.resize(mN); + + mWeights.resize(mN); + for(unsigned int i=0; i= 0 + m = mM; + + //Constraint Jacobian + //This is a dense Jacobian + nnz_jac_g = mM*mN; + + //Hessian + //This is a symmetric matrix 5x5 matrix, but its densley populated so + //we must fill in the bottom left triangle. + nnz_h_lag = 15; + + index_style = Ipopt::TNLP::C_STYLE; + + return true; +} + + +bool FitTorqueMuscleParameters:: + get_bounds_info(Ipopt::Index n, + Ipopt::Number *x_l, + Ipopt::Number *x_u, + Ipopt::Index m, + Ipopt::Number *g_l, + Ipopt::Number *g_u) +{ + assert(n==mN); + assert(m==mM); + x_l[0] = mTaAngleScaleLB; + x_l[1] = mTvOmegaMaxScaleLB; + x_l[2] = mTpLambdaLB; + x_l[3] = mTpAngleOffsetLB; + x_l[4] = mTauScalingLB; + + x_u[0] = mTaAngleScaleUB; + x_u[1] = mTvOmegaMaxScaleUB; + x_u[2] = mTpLambdaUB; + x_u[3] = mTpAngleOffsetUB; + x_u[4] = mTauScalingUB; + + //The optimization routine will only strengthen + //the muscle to satisfy the constraint - but it + //will not weaken it. + + double tauActMaxLB = 0; + double tauActMaxUB = 0; + double tauActMinLB = 0; + double tauActMinUB = 0; + double tauPassiveLB = 0; + double tauPassiveUB = 0; + + + + if(mTqMcl.getJointTorqueSign() > 0){ + tauActMaxLB = 0; + tauActMaxUB = 2e19; + + tauActMinLB = -2e19; + tauActMinUB = 0; + + tauPassiveLB = -2e19; + tauPassiveUB = 0; + }else{ + tauActMaxLB = -2.e19; + tauActMaxUB = 0.; + + tauActMinLB = 0; + tauActMinUB = 2e19; + + tauPassiveLB = -2e19; + tauPassiveUB = 0; + } + + for(unsigned int i = mConIdxTauActMaxStart; i <= mConIdxTauActMaxEnd; ++i){ + g_l[i] = tauActMaxLB; + g_u[i] = tauActMaxUB; + } + for(unsigned int i = mConIdxTauActMinStart; i <= mConIdxTauActMinEnd; ++i){ + g_l[i] = tauActMinLB; + g_u[i] = tauActMinUB; + } + for(unsigned int i = mConIdxTauPassiveStart; i <= mConIdxTauPassiveEnd; ++i){ + g_l[i] = tauPassiveLB; + g_u[i] = tauPassiveUB; + } + + return true; +} + + +bool FitTorqueMuscleParameters:: + get_starting_point(Ipopt::Index n, + bool init_x, + Ipopt::Number *x, + bool init_z, + Ipopt::Number *z_L, + Ipopt::Number *z_U, + Ipopt::Index m, + bool init_lambda, + Ipopt::Number *lambda) +{ + assert(n==mN); + + x[0] = mTaAngleScaleStart; + x[1] = mTvOmegaMaxScaleStart; + x[2] = mTpLambdaStart; + x[3] = mTpAngleOffsetStart; + x[4] = mTauScalingStart; + + init_x = true; + init_z = false; + init_lambda = false; + + mDtp_Dx.resize(mN); + + return true; +} + +bool FitTorqueMuscleParameters:: + eval_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number &obj_value) +{ + assert(n==mN); + + obj_value = 0.; + double xUpd = 0.; + for(unsigned i=0; i +#include +#include "coin/IpTNLP.hpp" +#include "Millard2016TorqueMuscle.h" + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + + +class TorqueMuscleFittingToolkit { + public: + + + /** + \brief This function will adjust the parameters of the + muscle-torque-generator (MTG) so that the MTG is strong enough and + flexible enough that it can produce the measured output torque + across the vector of measured joint angles and angular + velocities without exceeding the desired activation upper + bound, nor the passive forces exceeding the prescribed upper bound. + This function requires that IPOPT is installed + and that cmake has been correctly configured to point at + IPOPT's install directory. For a detailed description of the method please + see : M.Millard, A.L.Kleesattel, M.Harant, & K.Mombaur. A reduced muscle + model and planar musculoskeletal model fit for the synthesis of whole body + movements. Journal of Biomechanics. (under review as of August 2018) + + The method adjusts five parameters: + + -# \f$s^A\f$: the scale of the domain (angle) of the + active-torque-angle-curve. This scaling is applied so that + the angle which corresponds to the peak value of the + active-torque-angle-curve remains unchanged. + -# \f$s^V\f$: the scaling applied to the maximum angular velocity at + which the concentric side of the torque-angular-velocity + curve goes to zero. + -# \f$\lambda^P\f$: the passive-torque-angle-blending-variable + -# \f$\Delta^P\f$: the passive-torque-angle-offset + -# \f$s^\tau\f$: the scaling applied to \f$\tau_{o}\f$ the + maximum active isometric torque + + These 5 parameters are a subset of the fitting variables that are available, + and are illustrated below: + + \image html fig_MuscleAddon_BlendableTorqueMuscle.png "Examples of the adjustable characteristic curves" + + + To peform the fitting, the user must provide vectors of the + joint angle, joint angular velocity, and the joint torque + for the joint that the muscle-torque-generator (MTG) actuates. The sign of + all of these quantities must be consistent with the multibody + and the torque muscle. IPOPT solves for the fitting parameters + \f$\mathbf{x}\f$ such that the final result is as close as possible to the + values the MTG has before the fitting is performed \f$\mathbf{x}_o\f$ + \f[ + (\mathbf{x}-\mathbf{x}_o)^T \mathbf{K} (\mathbf{x}-\mathbf{x}_o) + \f] + such that at every point \f$i\f$ in the vector of joint angles, velocities, + and torques the following constraints are satisfied + \f[ + \tau^M(a^{max},\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \ge 0 + \f] + \f[ + \tau^M(0,\theta_i,\dot{\theta}_i,\mathbf{x})-\tau_i \le 0 + \f] + \f[ + \mathbf{t}_{P}(\theta_i) - \mathbf{t}_{P}^{max} \le 0 + \f] + \f[ + s^A, s^V, s^{\tau} \ge 1 + \f] + \f[ + 1 \ge \lambda^P \ge 0 + \f] + \f[ + \pi/2 \ge \Delta^P \ge -\pi/2. + \f] + These constraints ensure that the muscle can deliver the + requested torque with an activation of \f$a^{max}\f$ or less, such that + the active forces of the muscle are never fighting the passive forces, + and that passive forces that are \f$\mathbf{t}_{P}^{max}\f$ or less. + The bound limits on the various optimization variables ensure that the + muscle is not made to be weaker or slower than the default settings for + the muscle. Thus if your subject is weaker than the default settings + for the muscle, you should first weaken the MTG and then run this function. + + Notes + -# This routine will return the parameters that + fit the data, even if they are unreasonable: it is your + responsibility to have a look at this data to sanity check it + before using it. + -# This function will not update the internal + parameters of the model - it just calculates what they need + to be and returns them in the + TorqueMuscleParametersFittingData structure. If you decide + that these parameters are parameters are suitable, then they + can be assigned to the model using the function + Millard2016TorqueMuscle.setFittedParameters. The solve and set functions + have been deliberately split up to reflect the inherentely + iterative, and interactive nature of fitting the strength + of a musculoskeletal model to data. + -# Before attempting to fit the strength of the muscles + to experimental data we recommend that you pre-process the + data first. Normally this involves filtering the joint angles + using a zero-phase 2nd order low-pass Butterworth filter with + a cutoff frequency of 8-10 Hz (for human data). This can be + achieved in Matlab using the functions 'butter' and + 'filtfilt'. Following this filtering step joint angular + velocities can be safely calculated numerically using a + central-difference method. If you fail to filter measured + joint angles it is likely that the joint angular velocities + you calculate will be noisy and will result in a terrible + fit. This function is not magic, and so the rule + garbage in, garbage out applies. + + @param tqMcl: + The Millard2016TorqueMuscle object that is to be fitted. + @param jointAngle (radians) + A vector of measured joint angles. The sign of this vector + must be consistent with the multibody model and this + torque muscle. + @param jointAngularVelocity (radians/s) + A vector of joint angular velocities. + The sign of this vector must be consistent with the + multibody model and this torque muscle. + @param jointTorque (Nm) + A vector of joint torques. Only the + values in this vector that have the same torque sign as + the muscle are considered during the fitting process. This + means, inherently, that the muscle fitting is done assuming that + there is no co-contraction. If you want co-contraction to be included + in the fitting process, you must preprocess this vector so that it just + contains the (estimated) single-signed torque of the muscle in question. + The sign of this vector must be consistent with the + multibody model and this torque muscle. + @param activationUpperBound (0,1] + The maximum activation that is permitted for the given + set of joint angles, joint angular velocities, and + joint torques. + @param passiveTorqueAngleMultiplierUpperBound (0, \f$\infty\f$] + The maximum passive torque angle multipler that you expect + to see when the muscle moves through the values in + jointAngle. Note that a value of 1 is quite large: this + corresponds to 1 maximum active isometric torque. + @param parametersOfBestFit + A structure that contains the parameters of best fit along + with a summary of the internal variables of the muscle + model at the points in the data where the peak passive + forces are developed and where the maximum activation is + developed. + @param verbose + Setting this to will print information from IPOPT to the screen + during the solution process. + + */ + static void fitTorqueMuscleParameters( + Millard2016TorqueMuscle const &tqMcl, + RigidBodyDynamics::Math::VectorNd const &jointAngle, + RigidBodyDynamics::Math::VectorNd const &jointAngularVelocity, + RigidBodyDynamics::Math::VectorNd const &jointTorque, + double activationUpperBound, + double passiveTorqueAngleMultiplierUpperBound, + TorqueMuscleParameterFittingData ¶metersOfBestFit, + bool verbose=false); + + +}; + + + + +//A class that defines the virtual functions needed +//to use Ipopt to solve the problem +class FitTorqueMuscleParameters : public Ipopt::TNLP{ + public: + FitTorqueMuscleParameters( + const RigidBodyDynamics::Math::VectorNd &jointAngle, + const RigidBodyDynamics::Math::VectorNd &jointAngularVelocity, + const RigidBodyDynamics::Math::VectorNd &jointTorque, + double maxActivation, + double maxPassiveTorqueAngleMultiplier, + double taLambda, + double tvLambda, + Millard2016TorqueMuscle &tqMcl); + + + //Manditory functions of the TNLP interface + virtual bool get_nlp_info(Ipopt::Index &n, + Ipopt::Index &m, + Ipopt::Index &nnz_jac_g, + Ipopt::Index &nnz_h_lag, + Ipopt::TNLP::IndexStyleEnum &index_style); + + + virtual bool get_bounds_info(Ipopt::Index n, + Ipopt::Number *x_l, + Ipopt::Number *x_u, + Ipopt::Index m, + Ipopt::Number *g_l, + Ipopt::Number *g_u); + + + virtual bool get_starting_point(Ipopt::Index n, + bool init_x, + Ipopt::Number *x, + bool init_z, + Ipopt::Number *z_L, + Ipopt::Number *z_U, + Ipopt::Index m, + bool init_lambda, + Ipopt::Number *lambda); + + virtual bool eval_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number &obj_value); + + virtual bool eval_grad_f(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number *grad_f); + + virtual bool eval_g(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Index m, + Ipopt::Number *g); + + virtual bool eval_jac_g(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Index m, + Ipopt::Index nele_jac, + Ipopt::Index *iRow, + Ipopt::Index *jCol, + Ipopt::Number *values); + + virtual void finalize_solution( + Ipopt::SolverReturn status, + Ipopt::Index n, + const Ipopt::Number *x, + const Ipopt::Number *z_L, + const Ipopt::Number *z_U, + Ipopt::Index m, + const Ipopt::Number *g, + const Ipopt::Number *lambda, + Ipopt:: Number obj_value, + const Ipopt::IpoptData *ip_data, + Ipopt::IpoptCalculatedQuantities *ip_cq); + + //Optional functions + virtual bool eval_h(Ipopt::Index n, + const Ipopt::Number *x, + bool new_x, + Ipopt::Number obj_factor, + Ipopt::Index m, + const Ipopt::Number *lambda, + bool new_lambda, + Ipopt::Index nele_hess, + Ipopt::Index *iRow, + Ipopt::Index *jCol, + Ipopt::Number *values); + + double getSolutionActiveTorqueAngleAngleScaling(); + double getSolutionPassiveTorqueAngleBlendingParameter(); + double getSolutionTorqueAngularVelocityOmegaMaxScale(); + double getSolutionPassiveTorqueAngleCurveOffset(); + double getSolutionMaximumActiveIsometricTorqueScale(); + double getObjectiveValue(); + RigidBodyDynamics::Math::VectorNd& getConstraintError(); + + void updOptimizationVariables(const Ipopt::Number *x); + private: + unsigned int mN, mM; + + double mMaxActivation; + double mMinActivation; + double mMaxTp; + double mTauIso; + + double mTaAngleAtOneNormTorque; + double mOmegaMax; + double mTaLambda; + double mTvLambda; + + + double mTaAngleScaleStart; + double mTvOmegaMaxScaleStart; + double mTpLambdaStart; + double mTpAngleOffsetStart; + double mTauScalingStart; + + + double mTaAngleScaleLB; + double mTvOmegaMaxScaleLB; + double mTpLambdaLB; + double mTpAngleOffsetLB; + double mTauScalingLB; + + + double mTaAngleScaleUB; + double mTvOmegaMaxScaleUB; + double mTpLambdaUB; + double mTpAngleOffsetUB; + double mTauScalingUB; + + + double mTaAngleScale; + double mTvOmegaMaxScale; + double mTpLambda; + double mTpAngleOffset; + double mTauScaling; + + unsigned int mIndexTaAngleScale; + unsigned int mIndexTvOmegaMaxScale; + unsigned int mIndexTpLambda; + unsigned int mIndexTpOffset; + unsigned int mIndexTauScaling; + + unsigned int mConIdxTauActMaxStart; + unsigned int mConIdxTauActMaxEnd; + unsigned int mConIdxTauActMinStart; + unsigned int mConIdxTauActMinEnd; + unsigned int mConIdxTauPassiveStart; + unsigned int mConIdxTauPassiveEnd; + + + const RigidBodyDynamics::Math::VectorNd &mJointAngle; + const RigidBodyDynamics::Math::VectorNd &mJointAngularVelocity; + const RigidBodyDynamics::Math::VectorNd &mJointTorque; + + RigidBodyDynamics::Math::VectorNd mXOffset; + RigidBodyDynamics::Math::VectorNd mDtp_Dx; + RigidBodyDynamics::Math::VectorNd mConstraintErrors; + double mObjValue; + RigidBodyDynamics::Math::VectorNd mWeights; + Millard2016TorqueMuscle &mTqMcl; + TorqueMuscleInfo mTmi; + TorqueMuscleSummary mTms; +}; + + + + + } + } +} + +#endif + diff --git a/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc new file mode 100755 index 0000000..056135a --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.cc @@ -0,0 +1,1503 @@ +/*------------------------------------------------------------------------- + OpenSim: SmoothSegmentedFunctionFactory.cpp + -------------------------------------------------------------------------- + The OpenSim API is a toolkit for musculoskeletal modeling and simulation. + See http:%opensim.stanford.edu and the NOTICE file for more information. + OpenSim is developed at Stanford University and supported by the US + National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA + through the Warrior Web program. + + Copyright (c) 2005-2012 Stanford University and the Authors + Author(s): Matthew Millard + + Licensed under the Apache License, Version 2.0 (the 'License'); you may + not use this file except in compliance with the License. You may obtain a + copy of the License at http:%www.apache.org/licenses/LICENSE-2.0. + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an 'AS IS' BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + -------------------------------------------------------------------------- + + Derivative work + Date : September 2016 + Authors(s): Millard + Updates : Made active torque-angle, passive-torque-angle, torque-velocity + and tendon-torque-angle curves based on the equivalent line-type + curves in OpenSim. +*/ + +#include "TorqueMuscleFunctionFactory.h" +#include +#include +#include +#include +#include + +using namespace std; +using namespace RigidBodyDynamics::Math; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; + +static double SQRTEPS = sqrt( (double)std::numeric_limits::epsilon()); + +//============================================================================= +// Anderson 2007 Active Torque Angle Curve +//============================================================================= + +void TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueAngleCurve( + double c2, + double c3, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(c2 > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007ActiveTorqueAngleCurve " + << curveName + << ": c2 must be greater than 0" + << endl; + assert(0); + abort(); + } + + + std::string name=curveName; + name.append(".createAnderson2007ActiveTorqueAngleCurve"); + + //For now these advanced paramters are hidden. They will only be + //uncovered if absolutely necessary. + double minValueAtShoulders = 0; + double minShoulderSlopeMagnitude = 0; + + double curviness = 0.5; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //Translate the user parameters to quintic Bezier points + double x0 = c3 - 1.05*(0.5*(M_PI/c2)); + double x1 = c3 - 0.95*(0.5*(M_PI/c2)); + double x2 = c3; + double x3 = c3 + 0.95*(0.5*(M_PI/c2)); + double x4 = c3 + 1.05*(0.5*(M_PI/c2)); + + double y0 = minValueAtShoulders; + double y1 = cos(c2*(x1-c3)); + double y2 = cos(c2*(x2-c3)); + double y3 = cos(c2*(x3-c3)); + double y4 = minValueAtShoulders; + + double dydx0 = minShoulderSlopeMagnitude; + double dydx1 = -sin(c2*(x1-c3))*c2; + double dydx2 = -sin(c2*(x2-c3))*c2; + double dydx3 = -sin(c2*(x3-c3))*c2; + double dydx4 = -minShoulderSlopeMagnitude; + + + //Compute the Quintic Bezier control points + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + + RigidBodyDynamics::Math::MatrixNd p3 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); + + RigidBodyDynamics::Math::MatrixNd mX(6,4); + RigidBodyDynamics::Math::MatrixNd mY(6,4); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + mX.col(3) = p3.col(0); + mY.col(3) = p3.col(1); + + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, x0, x4, y0, y4, dydx0, dydx4, curveName); +} + +//============================================================================= +// ANDERSON 2007 Active Torque Angular Velocity Curve +//============================================================================= +void TorqueMuscleFunctionFactory:: + createAnderson2007ActiveTorqueVelocityCurve( + double c4, + double c5, + double c6, + double minEccentricMultiplier, + double maxEccentricMultiplier, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + //Check the input arguments + if( !(c4 < c5) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than c5" + << endl; + assert(0); + abort(); + } + + if( !((c4 > 0)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c4 must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(c6 > 0.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": c6 must be greater than 1.0" + << endl; + assert(0); + abort(); + } + + if( !(minEccentricMultiplier > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": minEccentricMultiplier must be greater than 1.0" + << endl; + assert(0); + abort(); + } + + if( !(maxEccentricMultiplier > minEccentricMultiplier) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAndersonActiveTorqueVelocityCurve " + << curveName + << ": maxEccentricMultiplier must be greater than " + << " minEccentricMultiplier" + << endl; + assert(0); + abort(); + } + + //Advanced settings that we'll hide for now + double minShoulderSlopeMagnitude = 0; + double curviness = 0.75; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + //Go and get the value of the curve that is closest to + //the maximum contraction velocity by setting rhs of Eqn. 9 + //to 0 and solving + double dthMaxConc = abs( 2.0*c4*c5/(c5-3.0*c4) ); + + //Go and evaluate the concentric side of the Anderson curve + //at 1/2 of omega max - we need this to use the updated + //torque-velocity curve. + double wMid = dthMaxConc*0.50; + double tvMidDen = (2*c4*c5 + wMid*(2*c5-4*c4)); + double tvMid = (2*c4*c5 + wMid*(c5-3*c4))/tvMidDen; + + tvMid = min(tvMid,0.45); + double tvMaxEcc = 1.1 + c6*0.2; + + createTorqueVelocityCurve(tvMaxEcc, + tvMid, + curveName, + smoothSegmentedFunctionToUpdate); + + +} +//============================================================================= +// ANDERSON 2007 Passive Torque Angle Curve +//============================================================================= +void TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + double scale, + double c1, + double b1, + double k1, + double b2, + double k2, + const std::string& curveName, + SmoothSegmentedFunction& smoothSegmentedFunctionToUpdate) +{ + + if( !(scale > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": scale must be greater than 0" + << endl; + assert(0); + abort(); + } + + if( !(c1 > 0) ) { + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": c1 must be greater than 0" + << endl; + assert(0); + abort(); + } + + //Advanced settings that we'll hide for now + double curviness = 0.75; + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double minShoulderSlopeMagnitude = 0; + + //Zero out the coefficients associated with a + //the side of the curve that goes negative. + bool flag_oneSided = true; + + if(flag_oneSided){ + if(fabs(b1) > 0){ + if(b1 > 0){ + b2 = 0; + k2 = 0; + }else{ + b1 = 0; + k1 = 0; + } + + }else if(fabs(b2) > 0){ + if(b2 > 0){ + b1 = 0; + k1 = 0; + }else{ + b2 = 0; + k2 = 0; + } + } + } + //Divide up the curve into a left half + //and a right half, rather than 1 and 2. + //Why? These two different halves require different + // Bezier curves. + + double c1Scale = c1*scale; + double thL = 0.; //left + double thR = 0.; //right + double DtauDthL = 0.; + double DtauDthR = 0.; + double bL = 0.; + double kL = 0.; + double bR = 0.; + double kR = 0.; + + int curveType = 0; //flat curve + int flag_thL = 0; + int flag_thR = 0; + + if(fabs(k1)>0 && fabs(b1)>0){ + //The value of theta where the passive force generated by the + //muscle is equal to 1 maximum isometric contraction. + thL = (1/k1)*log(fabs( c1Scale/b1 )); + DtauDthL = b1*k1*exp(thL*k1); + bL = b1; + kL = k1; + flag_thL = 1; + } + + if(fabs(k2)>0 && fabs(b2)>0){ + //The value of theta where the passive force generated by the + //muscle is equal to 1 maximum isometric contraction. + thR = (1/k2)*log(fabs( c1Scale/b2 )); + DtauDthR = b2*k2*exp(thR*k2); + bR = b2; + kR = k2; + flag_thR = 1; + } + + //A 'left' curve has a negative slope, + //A 'right' curve has a positive slope. + if(DtauDthL > DtauDthR){ + double tmpD = thL; + thL = thR; + thR = tmpD; + + tmpD = bR; + bR = bL; + bL = tmpD; + + tmpD = kR; + kR = kL; + kL = tmpD; + + tmpD = DtauDthL; + DtauDthL = DtauDthR; + DtauDthR = tmpD; + + int tmpI = flag_thL; + flag_thL = flag_thR; + flag_thR = tmpI; + } + + + if(flag_thL){ + curveType = curveType + 1; + } + if(flag_thR){ + curveType = curveType + 2; + } + + RigidBodyDynamics::Math::MatrixNd mX(6,1); + RigidBodyDynamics::Math::MatrixNd mY(6,1); + + double xStart = 0; + double xEnd = 0; + double yStart = 0; + double yEnd = 0; + double dydxStart = 0; + double dydxEnd = 0; + + + switch(curveType){ + + case 0: + {//No curve - it's a flat line + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(0.,0.,0., + 1.,0.,0.,c); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + + }break; + case 1: + { + //Get a point on the curve that is close to 0. + double x1 = (1/kL)*log(fabs(0.01*c1Scale/bL) ); + double y1 = bL*exp(kL*x1); + double dydx1 = bL*kL*exp(kL*x1); + + //Get a point that is at 1 maximum isometric torque + double x3 = thL; + double y3 = bL*exp(kL*x3); + double dydx3 = bL*kL*exp(kL*x3); + + //Get a mid-point + double x2 = 0.5*(x1+x3); + double y2 = bL*exp(kL*x2); + double dydx2 = bL*kL*exp(kL*x2); + + //Past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + xStart = x3; + xEnd = x0; + yStart = y3; + yEnd = y0; + dydxStart = dydx3; + dydxEnd = dydx0; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x0,y0,dydx0,c); + + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + }break; + case 2: + { + //Get a point on the curve that is close to 0. + double x1 = (1/kR)*log(fabs(0.01*c1Scale/bR) ); + double y1 = bR*exp(kR*x1); + double dydx1 = bR*kR*exp(kR*x1); + + //Go just past the crossing point of the linear extrapolation + double x0 = x1 - 2*y1/dydx1; + double y0 = 0; + double dydx0 = minShoulderSlopeMagnitude*copysign(1.0,dydx1); + + //Get a point close to 1 maximum isometric torque + double x3 = thR; + double y3 = bR*exp(kR*x3); + double dydx3 = bR*kR*exp(kR*x3); + + //Get a mid point. + double x2 = 0.5*(x1+x3); + double y2 = bR*exp(kR*x2); + double dydx2 = bR*kR*exp(kR*x2); + + xStart = x0; + xEnd = x3; + yStart = y0; + yEnd = y3; + dydxStart = dydx0; + dydxEnd = dydx3; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + mX.resize(6,3); + mY.resize(6,3); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + + }break; + case 3: + { + //Only when flag_oneSided = false; + double x0 = thL; + double x4 = thR; + + double x2 = 0.5*(x0 + x4); + double x1 = 0.5*(x0 + x2); + double x3 = 0.5*(x2 + x4); + + double y0 = b1*exp(k1*x0) + + b2*exp(k2*x0); + double y1 = b1*exp(k1*x1) + + b2*exp(k2*x1); + double y2 = b1*exp(k1*x2) + + b2*exp(k2*x2); + double y3 = b1*exp(k1*x3) + + b2*exp(k2*x3); + double y4 = b1*exp(k1*x4) + + b2*exp(k2*x4); + + double dydx0 = b1*k1*exp(k1*x0) + + b2*k2*exp(k2*x0); + double dydx1 = b1*k1*exp(k1*x1) + + b2*k2*exp(k2*x1); + double dydx2 = b1*k1*exp(k1*x2) + + b2*k2*exp(k2*x2); + double dydx3 = b1*k1*exp(k1*x3) + + b2*k2*exp(k2*x3); + double dydx4 = b1*k1*exp(k1*x4) + + b2*k2*exp(k2*x4); + + xStart = x0; + xEnd = x4; + yStart = y0; + yEnd = y4; + dydxStart = dydx0; + dydxEnd = dydx4; + + RigidBodyDynamics::Math::MatrixNd p0 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0,y0,dydx0, + x1,y1,dydx1,c); + RigidBodyDynamics::Math::MatrixNd p1 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1,y1,dydx1, + x2,y2,dydx2,c); + RigidBodyDynamics::Math::MatrixNd p2 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x2,y2,dydx2, + x3,y3,dydx3,c); + RigidBodyDynamics::Math::MatrixNd p3 = + SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x3,y3,dydx3, + x4,y4,dydx4,c); + + mX.resize(6,4); + mY.resize(6,4); + + mX.col(0) = p0.col(0); + mY.col(0) = p0.col(1); + mX.col(1) = p1.col(0); + mY.col(1) = p1.col(1); + mX.col(2) = p2.col(0); + mY.col(2) = p2.col(1); + mX.col(3) = p3.col(0); + mY.col(3) = p3.col(1); + + }break; + default: + { + cerr << "TorqueMuscleFunctionFactory::" + << "createAnderson2007PassiveTorqueAngleCurve " + << curveName + << ": undefined curveType" + << endl; + assert(0); + abort(); + } + + }; + + //Normalize the y values. + mY = mY*(1/c1Scale); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + mX, mY, + xStart, xEnd, + yStart/c1Scale, yEnd/c1Scale, + dydxStart/c1Scale, dydxEnd/c1Scale, + curveName); +} + +//============================================================================= +// +// New torque-muscle characteristic curves +// +//============================================================================= + + +//============================================================================= +// torque-velocity curves +//============================================================================= + +void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) + { + + double slopeAtConcentricOmegaMax = 0.0; + double slopeNearEccentricOmegaMax = -0.025; + double slopeAtEccentricOmegaMax = 0.0; + double eccentricCurviness = 0.75; + + createTorqueVelocityCurve( + tvAtEccentricOmegaMax, + tvAtHalfConcentricOmegaMax, + slopeAtConcentricOmegaMax, + slopeNearEccentricOmegaMax, + slopeAtEccentricOmegaMax, + eccentricCurviness, + curveName, + smoothSegmentedFunctionToUpdate); + + } + +void TorqueMuscleFunctionFactory::createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + double slopeAtConcentricOmegaMax, + double slopeNearEccentricOmegaMax, + double slopeAtEccentricOmegaMax, + double eccentricCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + if( (tvAtEccentricOmegaMax < 1.05) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtEccentricOmegaMax must be greater than 1.05" + << endl; + assert(0); + abort(); + } + + if( (tvAtHalfConcentricOmegaMax < 0.05 + || tvAtHalfConcentricOmegaMax > 0.45) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": tvAtHalfOmegaMax must be in the interval [0.05,0.45]" + << endl; + assert(0); + abort(); + } + + if( (slopeAtConcentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtConcentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (slopeNearEccentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeNearEccentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (slopeAtEccentricOmegaMax > 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": slopeAtEccentricOmegaMax cannot be less than 0" + << endl; + assert(0); + abort(); + } + + if( (eccentricCurviness < 0 || eccentricCurviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": eccentricCurviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + + double omegaMax = 1.0; + double wmaxC = omegaMax; //In biomechanics the concentric side gets a + //a +'ve signed velocity + double wmaxE = -omegaMax; + + //----------------------------------------------------------------------- + // 1. Fit concentric Bezier curves to a Hill-type hyperbolic curve + //----------------------------------------------------------------------- + + /* + First we compute the terms that are consistent with a Hill-type concentric + contraction. Starting from Hill's hyperbolic equation + + f(w) = (fiso*b - a*w) / (b+w) + + + Taking a derivative w.r.t omega yields + + df(w)/dw = [ (fiso*b - a*w + a*(b+w))] / (b+w)^2 + + at w = wmaxC the numerator goes to 0 + + (fiso*b - a*wmaxC) / (b + wmaxC) = 0 + (fiso*b - a*wmaxC) = 0; + b = a*wmaxC/fiso; + + Subtituting this expression for b into the expression when + f(wHalf) = tvAtHalfOmegaMax yields this expression for parameter a + + a = tvAtHalfOmegaMax*w*fiso ... + / (wmaxC*tvAtHalfOmegaMax + fiso*wmaxC - fiso*w); + + */ + + double fiso = 1.0; + double w = 0.5*wmaxC; + double a = -tvAtHalfConcentricOmegaMax*w*fiso + / (wmaxC*tvAtHalfConcentricOmegaMax - fiso*wmaxC + fiso*w); + + double b = a*wmaxC/fiso; + double yCheck = (b*fiso-a*w)/(b+w); + + if( abs(yCheck-tvAtHalfConcentricOmegaMax) > SQRTEPS ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << "abs(yCheck-tvAtHalfOmegaMax) > sqrt(eps)" + << "Consult the maintainers of this addon." + << endl; + assert(0); + abort(); + } + + + w = 0*wmaxC; + double dydxIso = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w)); + + + w = 0.9*wmaxC; + double dydxNearC = (-(a)*(b+w) - (b*fiso-a*w)) / ((b+w)*(b+w)); + + + if( dydxNearC > slopeAtConcentricOmegaMax || abs(dydxNearC) > abs(1/wmaxC) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTorqueVelocityCurve " + << curveName + << ": Internal error fitting the concentric curve to Hill's " + << "hyperbolic curve. This error condition was true: " + << " dydxNearC < dydxC || dydxNearC > abs(1/wmaxC)" + << "Consult the maintainers of this addon." + << endl; + assert(0); + abort(); + } + + //---------------------------------------------------------------------------- + // Iterate over the curviness value to get a good match between Hill's + // hyperbolic curve and the Bezier curve + //---------------------------------------------------------------------------- + + double xNearC = 0.9*wmaxC; + double yNearC = (b*fiso-a*w)/(b+w); + double xIso = 0; + double yIso = 1.0; + + double cC = 0.5; + + MatrixNd pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso, + xNearC,yNearC, dydxNearC, + cC); + MatrixNd xpts(6,1); + xpts.col(0) = pts.col(0); + MatrixNd ypts(6,1); + ypts.col(0) = pts.col(1); + + SmoothSegmentedFunction fvCFcn = SmoothSegmentedFunction( + xpts,ypts, + xIso,xNearC, + yIso,yNearC, + dydxIso,dydxNearC, + "tvFcn"); + SmoothSegmentedFunction fvCFcnLeft = SmoothSegmentedFunction(); + SmoothSegmentedFunction fvCFcnRight = SmoothSegmentedFunction(); + + int nSample = 10; + double f = 0; + double yHill = 0; + + //Calculate the error of the Bezier curve with the starting curviness value + //of 0.5 + for(int j=0; j (0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque has a magnitude that is too " + << "large, it exceeds 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( abs(stiffnessAtOneNormTorque) + < (1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque has a magnitude that is too " + << "small, it is less than 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque*stiffnessAtLowTorque < 0.0 ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtLowTorque and stiffnessAtOneNormTorque must have the" + << " same sign." + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque + *(angleAtOneNormTorque-angleAtZeroTorque) < 0.0){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque must have the same sign as " + << "(angleAtOneNormTorque-angleAtZeroTorque)" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createPassiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + MatrixNd xM(6,2); + MatrixNd yM(6,2); + MatrixNd pts(6,2); + + double x0,x1,y0,y1,dydx0,dydx1; + if(angleAtZeroTorque < angleAtOneNormTorque){ + x0 = angleAtZeroTorque; + x1 = angleAtOneNormTorque; + y0 = 0.; + y1 = 1.; + dydx0 = 0.0; + dydx1 = stiffnessAtOneNormTorque; + }else{ + x0 = angleAtOneNormTorque; + x1 = angleAtZeroTorque; + y0 = 1.0; + y1 = 0.0; + dydx0 = stiffnessAtOneNormTorque; + dydx1 = 0.0; + } + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + if(abs(stiffnessAtOneNormTorque) > SQRTEPS){ + + double delta = min( 0.1*(1.0-abs(1.0/stiffnessAtOneNormTorque)), + 0.05*abs(x1-x0)); + if(stiffnessAtOneNormTorque < 0.){ + delta *= -1.0; + } + + double xLow = angleAtZeroTorque + delta; + double xFoot = angleAtZeroTorque + 0.5*(xLow-angleAtZeroTorque); + double yFoot = 0.0; + double yLow = yFoot + stiffnessAtLowTorque*(xLow-xFoot); + + pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( + x0, y0, dydx0, + xLow,yLow,stiffnessAtLowTorque, c); + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints( + xLow,yLow,stiffnessAtLowTorque, + x1, y1, dydx1, c); + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + }else{ + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + angleAtZeroTorque,0, 0, + angleAtOneNormTorque,0, 0, + c); + xM.col(0) = pts.col(0); + yM.col(1) = pts.col(1); + } + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + xM, yM, + x0, x1, + y0, y1, + dydx0, dydx1, + curveName); + + +} + +//============================================================================= +// active-torque angle curve +//============================================================================= + + +void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + 0.0, + 0.0, + 0.5, + curveName, + smoothSegmentedFunctionToUpdate ); + +} + + +void TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + double minSlopeAtShoulders, + double minValueAtShoulders, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + if( (angularStandardDeviation < SQRTEPS) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": angularStandardDeviation is less than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + if( (minValueAtShoulders < 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minValueAtShoulders is less than 0" + << endl; + assert(0); + abort(); + } + + + if( (minSlopeAtShoulders < 0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": minSlopeAtShoulders is less than 0" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createGaussianShapedActiveTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + double taCutoff = 1e-3; + if(taCutoff < minValueAtShoulders ){ + taCutoff = minValueAtShoulders; + } + double angularStandardDeviationSq = + angularStandardDeviation*angularStandardDeviation; + double thetaWidth = sqrt(-log(taCutoff)*2*angularStandardDeviationSq); + double thetaMin = -thetaWidth + angleAtOneNormTorque; + double thetaMax = thetaWidth + angleAtOneNormTorque; + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + + double x0 = thetaMin; + + double x1 = angleAtOneNormTorque - angularStandardDeviation; + double x2 = angleAtOneNormTorque; + double x3 = angleAtOneNormTorque + angularStandardDeviation; + + if( (angleAtOneNormTorque-thetaMin) < + (angleAtOneNormTorque-angularStandardDeviation)){ + x1 = angleAtOneNormTorque - 0.5*thetaMin; + x3 = angleAtOneNormTorque + 0.5*thetaMin; + } + + double x4 = thetaMax; + + double y0 = minValueAtShoulders; + double y1 = exp(-(x1-angleAtOneNormTorque)*(x1-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y2 = exp(-(x2-angleAtOneNormTorque)*(x2-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y3 = exp(-(x3-angleAtOneNormTorque)*(x3-angleAtOneNormTorque) + / (2*angularStandardDeviationSq) ); + double y4 = minValueAtShoulders; + + + double dydx1 = -(2*(x1-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y1; + double dydx2 = -(2*(x2-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y2; + double dydx3 = -(2*(x3-angleAtOneNormTorque) + / (2*angularStandardDeviationSq)) * y3; + + double dydx0 = minSlopeAtShoulders; + double dydx4 = minSlopeAtShoulders; + + if(dydx1 < 0){ + dydx0 *= -1.0; + } + if(dydx3 < 0){ + dydx4 *= -1.0; + } + + + MatrixNd xM(6,4); + MatrixNd yM(6,4); + MatrixNd pts(6,2); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x0,y0, dydx0, + x1,y1, dydx1, c); + + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x1,y1, dydx1, + x2,y2, dydx2, c); + + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x2,y2, dydx2, + x3,y3, dydx3, c); + + xM.col(2) = pts.col(0); + yM.col(2) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints( + x3,y3, dydx3, + x4,y4, dydx4, c); + + xM.col(3) = pts.col(0); + yM.col(3) = pts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction( + xM, yM, + x0, x4, + y0, y4, + dydx0,dydx4, + curveName); + +} + + +//============================================================================= +// tendon-torque angle curve +//============================================================================= + +void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + 1.5/angularStretchAtOneNormTorque, + 1.0/3.0, + 0.5, + curveName, + smoothSegmentedFunctionToUpdate ); + +} + + +void TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + double stiffnessAtOneNormTorque, + double normTorqueAtToeEnd, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ) +{ + + + if( angularStretchAtOneNormTorque < SQRTEPS ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": angularStretchAtOneNormTorque should be greater than sqrt(eps)" + << endl; + assert(0); + abort(); + } + + if( stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": stiffnessAtOneNormTorque should be greater " + << " than 1.1/angularStretchAtOneNormTorque" + << endl; + assert(0); + abort(); + } + + if( normTorqueAtToeEnd < SQRTEPS || normTorqueAtToeEnd > 0.99){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": normTorqueAtToeEnd must be in the inteval [sqrt(eps), 0.99]" + << endl; + assert(0); + abort(); + } + + if( (curviness < 0 || curviness > 1.0) ){ + cerr << "TorqueMuscleFunctionFactory::" + << "createTendonTorqueAngleCurve " + << curveName + << ": curviness must be in the interval [0,1]" + << endl; + assert(0); + abort(); + } + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(curviness); + double x0 = 0; + double y0 = 0; + double dydx0 = 0; + + double xIso = angularStretchAtOneNormTorque; + double yIso = 1; + double dydxIso = stiffnessAtOneNormTorque; + + //Location where the curved section becomes linear + double yToe = normTorqueAtToeEnd; + double xToe = (yToe-1)/stiffnessAtOneNormTorque + xIso; + + + //To limit the 2nd derivative of the toe region the line it tends to + //has to intersect the x axis to the right of the origin + double xFoot = (xToe)/10.0; + double yFoot = 0; + + + //Compute the location of the corner formed by the average slope of the + //toe and the slope of the linear section + double yToeMid = yToe*0.5; + double xToeMid = (yToeMid-yIso)/stiffnessAtOneNormTorque + xIso; + double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot); + + //Compute the location of the control point to the left of the corner + double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot); + double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot); + + MatrixNd xM(6,2); + MatrixNd yM(6,2); + MatrixNd pts(6,2); + + //Compute the Quintic Bezier control points + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0, y0, dydx0, + xToeCtrl,yToeCtrl,dydxToeMid, c); + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid, + xToe, yToe, dydxIso, c); + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM, + x0,xToe, + y0,yToe, + dydx0,dydxIso, + curveName); + +} + + +//============================================================================= +// damping blending curve +//============================================================================= + +void TorqueMuscleFunctionFactory::createDampingBlendingCurve( + double normAngularVelocityAtMaximumDamping, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate) +{ + if( abs(normAngularVelocityAtMaximumDamping) < SQRTEPS){ + cerr << "TorqueMuscleFunctionFactory::" + << "createDampingBlendingCurve " + << curveName + << ": |normAngularVelocityAtMaximumDamping| < SQRTEPS" + << endl; + assert(0); + abort(); + } + + double x0,x1,x2,y0,y1,y2,dydx0,dydx1,dydx2; + + if(normAngularVelocityAtMaximumDamping > 0){ + x0 = 0.; + x2 = normAngularVelocityAtMaximumDamping; + y0 = 0.; + y2 = 1.; + }else{ + x0 = normAngularVelocityAtMaximumDamping; + x2 = 0.; + y0 = 1.0; + y2 = 0.0; + } + x1 = 0.5*(x0+x2); + y1 = 0.5*(y0+y2); + + dydx0 = 0.; + dydx2 = 0.; + dydx1 = 2.0*(y2-y0)/(x2-x0); + + double c = SegmentedQuinticBezierToolkit::scaleCurviness(0.5); + + MatrixNd xM(6,2); + MatrixNd yM(6,2); + MatrixNd pts(6,2); + + //Compute the Quintic Bezier control points + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x0, y0, dydx0, + x1, y1, dydx1, c); + xM.col(0) = pts.col(0); + yM.col(0) = pts.col(1); + + pts = SegmentedQuinticBezierToolkit:: + calcQuinticBezierCornerControlPoints(x1, y1, dydx1, + x2, y2, dydx2, c); + xM.col(1) = pts.col(0); + yM.col(1) = pts.col(1); + + smoothSegmentedFunctionToUpdate.updSmoothSegmentedFunction(xM,yM, + x0,x2, + y0,y2, + dydx0,dydx2, + curveName); + +} + diff --git a/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h new file mode 100755 index 0000000..15644ff --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/TorqueMuscleFunctionFactory.h @@ -0,0 +1,704 @@ +#ifndef TORQUEMUSCLEFUNCTIONFACTORY_H_ +#define TORQUEMUSCLEFUNCTIONFACTORY_H_ +/*------------------------------------------------------------------------- + OpenSim: SmoothSegmentedFunctionFactory.cpp + -------------------------------------------------------------------------- + The OpenSim API is a toolkit for musculoskeletal modeling and simulation. + See http:%opensim.stanford.edu and the NOTICE file for more information. + OpenSim is developed at Stanford University and supported by the US + National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA + through the Warrior Web program. + + Copyright (c) 2005-2012 Stanford University and the Authors + Author(s): Matthew Millard + + Licensed under the Apache License, Version 2.0 (the 'License'); you may + not use this file except in compliance with the License. You may obtain a + copy of the License at http:%www.apache.org/licenses/LICENSE-2.0. + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an 'AS IS' BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + -------------------------------------------------------------------------- + + Derivative work + Date : September 2016 + Authors(s): Millard + Updates : Made active torque-angle, passive-torque-angle, torque-velocity + and tendon-torque-angle curves based on the equivalent line-type + curves in OpenSim. +*/ + +#include "../geometry/SmoothSegmentedFunction.h" +#include "../geometry/SegmentedQuinticBezierToolkit.h" + +#include +#include +#include +#include + +namespace RigidBodyDynamics { + namespace Addons { + namespace Muscle{ + +class TorqueMuscleFunctionFactory +{ + public: + + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active torque angle curve. This Bezier curve has been + fitted to match the active-torque-angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + but note that its range is normalized to [0,1]. + + @param c2 (radians) + The active-torque-angle width parameter. The parameter c2 + is defined by Anderson et al. as + + c2 = pi/(theta_max - theta_min). + + @param c3 : (radians) + Then angle which has the largest active-torque. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + +*/ +static void createAnderson2007ActiveTorqueAngleCurve( + double c2, + double c3, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This is a function that will produce a C2 (continuous to the second + derivative) active torque (angular) velocity curve. This Bezier curve + has been fitted to match the active-torque-angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + While the concentric side of the Bezier curve and the original + formulation match, the eccentric side does not: the equations + Anderson et al. chose decrease down to 0 rapidly. Since Anderson + et al. did not collect data at the higher eccentric velocities the + oddities in their chosen curves are likely due to the parameterization + they chose. The eccentric side of the Bezier curve will be fitted + so that, if possible, it passes close to the value of the original + curves for theta = -60 deg/s within the limits imposed by + minEccentricMultiplier and maxEccentricMultiplier. + + @param c4 (rads/s) + Angular velocity when the torque is 75% of the maximum + isometric torque. + + @param c5 (rads/s) + Angular velocity when the torque is 50% of the maximum + isometric torque. + + @param c6 + Multiplier that Anderson et al. uses to describe the + change in slope of the curve as the contraction velocity + changes sign from + to -. + + @param minEccentricMultiplier + The minimum value of the torque-(angular)-velocity curve + tends to at large eccentric contraction velocities. Note + minEccentricMultiplier > 1.0 + + @param maxEccentricMultiplier + The value of the torque-(angular)-velocity curve tends + to at large eccentric contraction velocities. Note + maxEccentricMultiplier > minEccentricMultiplier. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + */ + static void createAnderson2007ActiveTorqueVelocityCurve( + double c4, + double c5, + double c6, + double minEccentricMultiplier, + double maxEccentricMultiplier, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + /** + This is a function that will produce a C2 (continuous to the second + derivative) passive torque angle curve described in + + Anderson, Dennis E., Michael L. Madigan, and Maury A. Nussbaum. + "Maximum voluntary joint torque as a function of joint angle and + angular velocity: model development and application to the lower + limb." Journal of biomechanics 40, no. 14 (2007): 3105-3113. + + Note the following differences between this implementation and + the original equations presented in Anderson et al.: + + 1. This function will return a curve that is fitted to the + positive side of the curve defined by the coefficients + b1, k1, b2, and k2. Because of the sign convention employed by + Anderson et al. the positive side of the curve corresponds to + the passive curve generated by the torque actuator associated + with the rest of the coefficients. + + 2. This function has been normalized so that a value of 1.0 + corresponds to one-maximum-isometric-active-contraction + torque, or c1*subjectWeightInNewtons*subjectHeightInMeters. + + @param scale + The scaling factor used on the c1 column in Table 3 of + Anderson et al.: + + scale = subjectWeightInNewtons * subjectHeightInMeters + + @param c1 + The normalized c1 parameter listed in Tabel 3 of + Anderson et al. + + @param b1 + The passive torque angle curve parameter used in + Anderson et al.'s Eqn. 1: + + torquePassive = b1*exp(k1*theta) + b2*exp(k2*theta) + + @param k1 + The term k1 in Anderson et al.'s Eqn. 1. + + @param b2 + The term b2 in Anderson et al.'s Eqn. 1. + + @param k2 + The term k2 in Anderson et al.'s Eqn. 1. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + */ + static void createAnderson2007PassiveTorqueAngleCurve( + double scale, + double c1, + double b1, + double k1, + double b2, + double k2, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate); + + + /** + This function creates a normalized torque-velocity curve. The concentric + side of the curve is fitted to Hill's hyperbola that passes through + the value of tv-at-half-of-the-maximum-concentric-velocity (a parameter + supplied by the user). The eccentric side of the curve rapidly, but smoothly + approaches a terminal value of + tv-at-the-maximum-eccentric-contraction-velocity. Outside of the normalized + velocities of -1 to 1 the curve takes the values of 0, and + tvAtEccentricOmegaMax respectively with a slope of 0. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png + + @param tvAtEccentricOmegaMax + The value of the torque-velocity-multiplier at the maximum + eccentric contraction velocity. This value must be + + @param tvAtHalfConcentricOmegaMax + The value of the torque-velocity- + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + -tvAtEccentricOmegaMax < 1.05 + -tvAtHalfOmegaMax >= 0.45 + -tvAtHalfOmegaMax <= 0.05 + + References + Hill, A. V. (1938). The heat of shortening and the dynamic constants of + muscle. Proceedings of the Royal Society of London B: Biological Sciences, + 126(843), 136-195. + + */ + + static void createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + /** + This function creates a normalized torque-velocity curve. The concentric + side of the curve is fitted to Hill's hyperbola that passes through + the value of tv-at-half-of-the-maximum-concentric-velocity (a parameter + supplied by the user). The eccentric side of the curve rapidly, but smoothly + approaches a terminal value of + tv-at-the-maximum-eccentric-contraction-velocity. Shape of the eccentric + side of the curve can be changed using the slopeNearEccentricOmegaMax + and curviness variables. Outside of the normalized + velocities of -1 to 1 the curve takes the values of + slopeAtConcentricOmegaMax and slopeAtEccentricOmegaMax respectively. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png + + @param tvAtEccentricOmegaMax + The value of the torque-velocity-multiplier at the maximum + eccentric contraction velocity. This value must be + + @param tvAtHalfConcentricOmegaMax + The value of the torque-velocity- + + @param slopeAtConcentricOmegaMax + The slope of the curve at a normalized angular velocity of -1. This slope + is used to extrapolate \f$\mathbf{t}_V\f$ for normalized angular velocities + of less than -1. + + @param slopeNearEccentricOmegaMax + The slope of the eccentric side of the curve as the normalized angular + velocity approaches 1. + + @param slopeAtEccentricOmegaMax + The slope of the curve at a normalized angular velocity of 1. This slope + is used to extrapolate \f$\mathbf{t}_V\f$ for normalized angular velocities + of greater than 1. + + @param eccentricCurviness + This parameter controls the shape of the curve between the normalized + angular velocities of 0 and 1. An eccentricCurviness of 0 will flatten + the elbow so that the curve closely follows a line that begins at (0,1) + and ends at (1,tvAtEccentricOmegaMax). An eccentricCurviness of 1 will + give the curve a strong elbow so that it quickly approaches the line that + passes through the point (1,tvAtEccentricOmegaMax) and has a slope of + slopeNearEccentricOmegaMax. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + -tvAtEccentricOmegaMax < 1.05 + -tvAtHalfOmegaMax > 0.45 or tvAtHalfOmegaMax < 0.05 + -slopeAtConcentricOmegaMax < 0 + -slopeNearEccentricOmegaMax < 0 + -slopeAtEccentricOmegaMax < 0 + -eccentricCurviness < 0 or eccentricCurviness > 1 + + References + Hill, A. V. (1938). The heat of shortening and the dynamic constants of + muscle. Proceedings of the Royal Society of London B: Biological Sciences, + 126(843), 136-195. + + */ + static void createTorqueVelocityCurve( + double tvAtEccentricOmegaMax, + double tvAtHalfConcentricOmegaMax, + double slopeAtConcentricOmegaMax, + double slopeNearEccentricOmegaMax, + double slopeAtEccentricOmegaMax, + double eccentricCurviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + /** + This function creates a Bezier spline that closely follows the + exponential curves that are typically used to model the passive-torque-angle + characteristic of muscles. This curve has a value and a slope of zero + for angles that are less than abs(angleAtZeroTorque). For angles that + have an absolute magnitude larger than abs(angleAtOneNormTorque) the curve + is simply linearly extrapolated. + + Note that curves can be represented that increase left-to-right, or + decrease left-to-right by setting the variables angleAtOneNormTorque and + angleAtZeroTorque correctly. For example using (0,1) for + angleAtOneNormTorque and angleAtZeroTorque produces a curve that increases + left-to-right while using (-1,0) produces a curve that decreases left to + right. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png + + @param angleAtZeroTorque is the angle at which the curve transitions from + a flat line and begins curving upwards. (radians) + + @param angleAtOneNormTorque is the angle at which this curve achieves a + value of 1.0. (radians) + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - abs(angleAtOneNormTorque-angleAtZeroTorque) < sqrt(eps) + */ + static void createPassiveTorqueAngleCurve( + double angleAtZeroTorque, + double angleAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + +/** + This function creates a Bezier spline that closely follows the + exponential curves that are typically used to model the passive-torque-angle + characteristic of muscles. This curve has a value and a slope of zero + for angles that are less than abs(angleAtZeroTorque). For angles that + have an absolute magnitude larger than abs(angleAtOneNormTorque) the curve + is simply linearly extrapolated. + + Note that curves can be represented that increase left-to-right, or + decrease left-to-right by setting the variables angleAtOneNormTorque and + angleAtZeroTorque correctly. For example using (0,1) for + angleAtOneNormTorque and angleAtZeroTorque produces a curve that increases + left-to-right while using (-1,0) produces a curve that decreases left to + right. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png + + @param angleAtZeroTorque is the angle at which the curve transitions from + a flat line and begins curving upwards. (radians) + + @param angleAtOneNormTorque is the angle at which this curve achieves a + value of 1.0. (radians) + + @param stiffnessAtLowTorque + The normalized stiffness (or slope) of the curve achieves as it begins + to increase. This is usually chosen to be a small, but non-zero fraction + of stiffnessAtOneNormTorque + (stiffnessAtLowTorque = 0.025 stiffnessAtOneNormTorque is typical). + The sign of stiffnessAtLowTorque must be positive if + angleAtOneNormTorque > angleAtZeroPassiveTorque. The sign + of stiffnessAtLowTorque must be negative if + angleAtOneNormTorque < angleAtZeroPassiveTorque. + (Norm.Torque/radians) + + @param stiffnessAtOneNormTorque + The normalized stiffness (or slope) of the fiber curve + when the fiber is stretched by + angleAtOneNormTorque - angleAtZeroPassiveTorque. The sign + of stiffnessAtOneNormTorque must agree with stiffnessAtLowTorque. + (Norm.Torque/radians) + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - abs(angleAtOneNormTorque-angleAtZeroTorque) < sqrt(eps) + - sign(stiffnessAtLowTorque) != sign(angleAtOneNormTorque-angleAtLowTorque) + - sign(stiffnessAtOneNormTorque) != sign(stiffnessAtLowTorque) + - abs(stiffnessAtLowTorque) > 0.9/abs(angleAtOneNormTorque-angleAtZeroTorque) + - abs(stiffnessAtOneTorque) <= 1.1/abs(angleAtOneNormTorque-angleAtZeroTorque) + - curviness < 0 or curviness > 1 + + */ + static void createPassiveTorqueAngleCurve( + double angleAtZeroTorque, + double angleAtOneNormTorque, + double stiffnessAtLowTorque, + double stiffnessAtOneNormTorque, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate ); + + + /** + This function produces a Bezier curve fitted to a Gaussian function. As the + tails of the Gaussian curve become small this curve is simply extrapolated + as a line with a y-value of zero and a slope of zero. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png + + @param angleAtOneNormTorque The angle at which the Gaussian curve develops + a value of 1. + + @param angularStandardDeviation The angular deviation from + the mean at which the Gaussian curve reaches a value of \f$e^{-1/2}\f$. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - angularWidthOfOneStandardDeviation < sqrt(eps) + */ + static void createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + +/** + This function produces a C2 continuous Bezier curve fitted to a Gaussian + function. As the tails of the Gaussian curve become less than + minValueAtShoulders, the curve is linearly extrapolated at a sloe of + minSlopeOfShoulders. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png + + @param angleAtOneNormTorque The angle at which the Gaussian curve develops + a value of 1. + + @param angularStandardDeviation The angular deviation from + the mean at which the Gaussian curve reaches a value of \f$e^{-1/2}\f$. + + @param minSlopeAtShoulders The y-value at which the Bezier curve transitions + from having a shape like a Gaussian curve to being linearly extrapolated. + + @param minValueAtShoulders The slope of the linear extrapolation of the + Bezier curve for y-values that are less than minSlopeAtShoulders. The + sign of minValueAtShoulders is automatically set so that it matches the + curve near it (see the figure). + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - angularWidthOfOneStandardDeviation < sqrt(eps) + - minSlopeAtShoulders < 0 + - minValueAtShoulders < 0 + - curviness > 1 or curviness < 0 + + */ + static void createGaussianShapedActiveTorqueAngleCurve( + double angleAtOneNormTorque, + double angularStandardDeviation, + double minSlopeAtShoulders, + double minValueAtShoulders, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + + /** + This function produces a normalized tendon-torque-angle curve with a + toe region that is in the range of \f$\mathbf{t}_V\f$ [0,1./3,] after which + the curve is linearly extrapolated. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png + + @param angularStretchAtOneNormTorque The amount of angular stretch of the + joint as the tendon goes from developing zero torque at its slack length + to developing one maximum isometric torque. (radians) + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - angularWidthOfOneStandardDeviation < sqrt(eps) + + */ + static void createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + /** + This function produces a normalized tendon-torque-angle curve with a + toe region, final stiffness, and shape that can be controlled. + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png + + @param angularStretchAtOneNormTorque The amount of angular stretch of the + joint as the tendon goes from developing zero torque at its slack length + to developing one maximum isometric torque. (radians) + + @param stiffnessAtOneNormTorque The linear stiffness value of the tendon + that is used for all y-values greater than the toe region. + (Norm. Torque/rad) + + @param normTorqueAtToeEnd The normalized torque value which defines the + end of the nonlinear-stiffness region of the tendon and the beginning of + the linear stiffness region of the tendon. + + @param curviness + The dimensionless 'curviness' parameter that + can vary between 0 (a line) to 1 (a smooth, but + sharply bent elbow). A value of 0.5 is typical as it produces a + graceful curve. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + + aborts + + - angularStretchAtOneNormTorque < sqrt(eps) + - stiffnessAtOneNormTorque < 1.1/angularStretchAtOneNormTorque + - normTorqueAtToeEnd < sqrt(eps) or normTorqueAtToeEnd > 0.99 + - curviness < 0 or curviness > 1 + + */ + static void createTendonTorqueAngleCurve( + double angularStretchAtOneNormTorque, + double stiffnessAtOneNormTorque, + double normTorqueAtToeEnd, + double curviness, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + + /** + This function creates a C2 sigmoid function that varies + from 0 to 1 and is used to ramp up the muscle's passive + damping. The + blending variable starts to go from 0 at an angular + velocity of 0. It reaches a value of 1 at the + normAngularVelocityAtMaximumDamping + + \image html fig_MuscleAddon_TorqueMuscleFunctionFactory_DampingBlendingCurve.png + + @param normAngularVelocityAtMaximumDamping the normalized + angular velocity at which the blending function reaches + a value of 1. This parameter must have an absolute magnitude + greater than 0. + + @param curveName The name of the joint torque this curve applies to. This + curve name should have the name of the joint and the + direction (e.g. hipExtensionTorqueMuscle) so that if + this curve ever causes an exception, a user friendly + error message can be displayed to the end user to help + them debug their model. + + @param smoothSegmentedFunctionToUpdate + A SmoothSegmentedFunction object that will be erased and filled with + the coefficients that are defined by this curve. + */ + static void createDampingBlendingCurve( + double normAngularVelocityAtMaximumDamping, + const std::string& curveName, + RigidBodyDynamics::Addons::Geometry::SmoothSegmentedFunction& + smoothSegmentedFunctionToUpdate + ); + +}; +} +} +} +#endif //TORQUEMUSCLEFUNCTIONFACTORY_H_ diff --git a/3rdparty/rbdl/addons/muscle/csvtools.cc b/3rdparty/rbdl/addons/muscle/csvtools.cc new file mode 100755 index 0000000..ff440a8 --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/csvtools.cc @@ -0,0 +1,160 @@ +/* -------------------------------------------------------------------------- * + * csvtools.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2012 Stanford University and the Authors * + * Author(s): Matthew Millard * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +#include "csvtools.h" + + +std::vector > readMatrixFromFile( + const std::string& filename, + int startingRow) +{ + std::ifstream datafile; + datafile.open(filename.c_str()); + + + //SimTK::Matrix data; + std::vector > dataMatrix; + std::vector rowVector; + + if(datafile.is_open()) + { + std::string line; + std::string entry; + int row = 0; + int col = 0; + //int matrixRowNum = 0; + int matrixColNum = 1; + + //1. Size the matrix + + + getline(datafile,line); //get a line of text + + while(row < startingRow){ + getline(datafile,line); + row++; + } + + //parse it for all of the comma spots + unsigned pos1 = 0; + unsigned pos2 = 0; + do{ + pos2 = line.find(",",pos1); + //if this is the first time running this loop, count + //the number of columns + if(pos2 >= 0 && pos2 < line.length() && row == 0) + matrixColNum++; + + pos1 = pos2+1; + }while(pos2 >= 0 && pos2 < line.length()); + + //Initial matrix sizing + if(row == 0){ + //matrixRowNum = max(matrixColNum,20); + rowVector.resize(matrixColNum); + //dataMatrix.resize(matrixRowNum); + //data.resizeKeep(matrixRowNum, matrixColNum); + } + + + while(datafile.good()) + { + pos1 = 0; + pos2 = 0; + for(int i=0; i < matrixColNum; i++){ + pos2 = line.find(",",pos1); + if(pos2 < 0 && pos2 >= line.length()) + pos2 = line.length(); + entry = line.substr(pos1,pos2-pos1); + pos1 = pos2+1; + //data(row,i) = atof(entry.c_str()); + rowVector[i] = atof(entry.c_str()); + } + + //Resize the matrix if its too small for the next line + //if(row == matrixRowNum-1){ + // matrixRowNum = matrixRowNum*2; + // data.resizeKeep(matrixRowNum,matrixColNum); + //} + dataMatrix.push_back(rowVector); + + row++; + getline(datafile,line); + } + //data.resizeKeep(row,matrixColNum); + + } + datafile.close(); + return dataMatrix; +} + +void printMatrixToFile( + const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename) +{ + + std::ofstream datafile; + datafile.open(filename.c_str()); + datafile << std::scientific; + datafile.precision(16); + if(header.length() > 1) + datafile << header << "\n"; + + for(int i = 0; i < dataMatrix.size(); i++){ + for(int j = 0; j < dataMatrix[0].size(); j++){ + if(j >& dataMatrix, + const std::string& header, + const std::string& filename) +{ + + std::ofstream datafile; + datafile.open(filename.c_str()); + //datafile << std::scientific; + //datafile.precision(16); + if(header.length() > 1) + datafile << header << "\n"; + + for(int i = 0; i < dataMatrix.size(); i++){ + for(int j = 0; j < dataMatrix[0].size(); j++){ + if(j +#include +#include +#include +#include +#include + +/** +This function will print cvs file of the matrix + data + +@params data: A vector of state vectors +@params filename: The name of the file to print +*/ +void printMatrixToFile( const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename); + +void printMatrixToFile( const std::vector >& dataMatrix, + const std::string& header, + const std::string& filename); +/** +This function will read in a cvs file assuming that all entries are numbers. + +@params filename: The name of the file to print +@params startingRow: the index of first row that contains numeric data. + +@return: A matrix of data +*/ +std::vector > readMatrixFromFile( + const std::string& filename, + int startingRow); diff --git a/3rdparty/rbdl/addons/muscle/muscle.h b/3rdparty/rbdl/addons/muscle/muscle.h new file mode 100755 index 0000000..a40fcfa --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/muscle.h @@ -0,0 +1,16 @@ +//============================================================================== +/* + * RBDL - Rigid Body Dynamics Library: Addon : muscle + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ +#ifndef MUSCLE_H_ +#define MUSCLE_H_ + +#include "Millard2016TorqueMuscle.h" +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + #include "TorqueMuscleFittingToolkit.h" +#endif + +#endif diff --git a/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt b/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt new file mode 100755 index 0000000..dc9307a --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/tests/CMakeLists.txt @@ -0,0 +1,123 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) + +#CMAKE_POLICY(SET CMP0048 NEW) +#CMAKE_POLICY(SET CMP0040 NEW) + +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR 2 ) +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR 0 ) +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH 0 ) + +SET ( RBDL_ADDON_MUSCLE_TESTS_VERSION + ${RBDL_ADDON_MUSCLE_TESTS_VERSION_MAJOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_MINOR}.${RBDL_ADDON_MUSCLE_TESTS_VERSION_PATCH} +) + + +PROJECT (RBDL_MUSCLE_TESTS VERSION ${RBDL_ADDON_MUSCLE_TESTS_VERSION}) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) + +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR} + ${IPOPT_INCLUDE_DIR}) +ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) + INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) +ENDIF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) + + SET ( MUSCLE_TESTS_SRCS + testMillard2016TorqueMuscle.cc + testMuscleFunctionFactory.cc + testTorqueMuscleFunctionFactory.cc + ../muscle.h + ../MuscleFunctionFactory.h + ../MuscleFunctionFactory.cc + ../TorqueMuscleFunctionFactory.h + ../TorqueMuscleFunctionFactory.cc + ../Millard2016TorqueMuscle.h + ../Millard2016TorqueMuscle.cc + ../TorqueMuscleFittingToolkit.cc + ../TorqueMuscleFittingToolkit.h + ../csvtools.h + ../csvtools.cc + ../../geometry/geometry.h + ../../geometry/SegmentedQuinticBezierToolkit.h + ../../geometry/SmoothSegmentedFunction.h + ../../geometry/SegmentedQuinticBezierToolkit.cc + ../../geometry/SmoothSegmentedFunction.cc + ../../geometry/tests/numericalTestFunctions.cc + ../../geometry/tests/numericalTestFunctions.h + ) + +ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) + + SET ( MUSCLE_TESTS_SRCS + testMillard2016TorqueMuscle.cc + testMuscleFunctionFactory.cc + testTorqueMuscleFunctionFactory.cc + ../muscle.h + ../MuscleFunctionFactory.h + ../MuscleFunctionFactory.cc + ../TorqueMuscleFunctionFactory.h + ../TorqueMuscleFunctionFactory.cc + ../Millard2016TorqueMuscle.h + ../Millard2016TorqueMuscle.cc + ../csvtools.h + ../csvtools.cc + ../../geometry/geometry.h + ../../geometry/SegmentedQuinticBezierToolkit.h + ../../geometry/SmoothSegmentedFunction.h + ../../geometry/SegmentedQuinticBezierToolkit.cc + ../../geometry/SmoothSegmentedFunction.cc + ../../geometry/tests/numericalTestFunctions.cc + ../../geometry/tests/numericalTestFunctions.h + ) + + +ENDIF(RBDL_BUILD_ADDON_MUSCLE_FITTING) + + + + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +ADD_EXECUTABLE ( rbdl_muscle_tests ${MUSCLE_TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_muscle_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runMuscleTests + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +IF (RBDL_BUILD_ADDON_MUSCLE_FITTING) +TARGET_LINK_LIBRARIES ( rbdl_muscle_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ${IPOPT_LIBRARY} + ) + +ELSE (RBDL_BUILD_ADDON_MUSCLE_FITTING) +TARGET_LINK_LIBRARIES ( rbdl_muscle_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} +) +ENDIF (RBDL_BUILD_ADDON_MUSCLE_FITTING) +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) +ADD_CUSTOM_COMMAND (TARGET rbdl_muscle_tests + POST_BUILD + COMMAND ./runMuscleTests + COMMENT "Running automated addon muscle tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc b/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc new file mode 100755 index 0000000..503ba2c --- /dev/null +++ b/3rdparty/rbdl/addons/muscle/tests/testMillard2016TorqueMuscle.cc @@ -0,0 +1,1719 @@ +/* * + * + * Copyright (c) 2016 Matthew Millard + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +//Some particularly aggressive experimental data +unsigned int TorqueMuscleFittingHardTestCaseRows = 100; +unsigned int TorqueMuscleFittingHardTestCaseCols = 4; +double const TorqueMuscleFittingHardTestCase[100][4] = + {{0.,1.9148,-17.618,107.25 }, + {0.0044242,1.8318,-18.277,108.39 }, + {0.0088485,1.7485,-18.949,110.49 }, + {0.013273,1.6636,-19.618,114.57 }, + {0.017697,1.5761,-20.243,121.32 }, + {0.022121,1.486,-20.758,130.97 }, + {0.026545,1.3938,-21.085,143.31 }, + {0.03097,1.3004,-21.144,157.68 }, + {0.035394,1.2074,-20.866,173.11 }, + {0.039818,1.1164,-20.199,188.42 }, + {0.044242,1.0292,-19.118,202.36 }, + {0.048667,0.94777,-17.626,213.77 }, + {0.053091,0.87376,-15.762,221.64 }, + {0.057515,0.80872,-13.594,225.25 }, + {0.061939,0.75378,-11.22,224.18 }, + {0.066364,0.70958,-8.7587,218.42 }, + {0.070788,0.67624,-6.3387,208.34 }, + {0.075212,0.65329,-4.0821,194.73 }, + {0.079636,0.63975,-2.0906,178.7 }, + {0.084061,0.63427,-0.43189,161.6 }, + {0.088485,0.63528,0.87052,144.75 }, + {0.092909,0.6412,1.8387,129.18 }, + {0.097333,0.6507,2.5306,115.24 }, + {0.10176,0.66277,3.015,102.36 }, + {0.10618,0.67685,3.3393,88.889 }, + {0.11061,0.69264,3.517,72.481 }, + {0.11503,0.71007,3.5756,50.979 }, + {0.11945,0.72927,3.6372,23.504 }, + {0.12388,0.75048,3.852,-8.9796 }, + {0.1283,0.77332,4.1461,-43.995 }, + {0.13273,0.79611,4.1835,-78.49 }, + {0.13715,0.81635,3.6883,-109.71 }, + {0.14158,0.83172,2.6498,-135.63 }, + {0.146,0.84066,1.2202,-154.98 }, + {0.15042,0.84231,-0.42406,-167.16 }, + {0.15485,0.83637,-2.1355,-172.03 }, + {0.15927,0.82297,-3.8027,-169.8 }, + {0.1637,0.80254,-5.3408,-160.95 }, + {0.16812,0.7758,-6.6819,-146.16 }, + {0.17255,0.74371,-7.771,-126.31 }, + {0.17697,0.70746,-8.5684,-102.56 }, + {0.18139,0.66837,-9.0566,-76.369 }, + {0.18582,0.62778,-9.2487,-49.623 }, + {0.19024,0.58691,-9.1941,-24.563 }, + {0.19467,0.54667,-8.9737,-3.6038 }, + {0.19909,0.50763,-8.6741,11.165 }, + {0.20352,0.47001,-8.3313,18.713 }, + {0.20794,0.43418,-7.8528,19.766 }, + {0.21236,0.40128,-6.9609,16.835 }, + {0.21679,0.37397,-5.266,12.889 }, + {0.22121,0.35642,-2.5349,9.4338 }, + {0.22564,0.35296,1.0546,6.2364 }, + {0.23006,0.36634,4.9935,2.8315 }, + {0.23448,0.39689,8.7586,-0.69647 }, + {0.23891,0.44304,11.998,-4.1689 }, + {0.24333,0.50198,14.522,-7.5873 }, + {0.24776,0.57037,16.269,-11.01 }, + {0.25218,0.64488,17.306,-14.511 }, + {0.25661,0.72269,17.793,-18.219 }, + {0.26103,0.8018,17.929,-22.301 }, + {0.26545,0.8811,17.902,-26.91 }, + {0.26988,0.96017,17.846,-32.128 }, + {0.2743,1.0391,17.834,-37.93 }, + {0.27873,1.1181,17.879,-44.158 }, + {0.28315,1.1973,17.961,-50.527 }, + {0.28758,1.277,18.039,-56.666 }, + {0.292,1.3569,18.078,-62.174 }, + {0.29642,1.4368,18.054,-66.692 }, + {0.30085,1.5165,17.955,-69.98 }, + {0.30527,1.5956,17.784,-71.983 }, + {0.3097,1.6738,17.552,-72.861 }, + {0.31412,1.7509,17.272,-72.976 }, + {0.31855,1.8266,16.952,-72.834 }, + {0.32297,1.9008,16.594,-72.982 }, + {0.32739,1.9733,16.191,-73.899 }, + {0.33182,2.0439,15.724,-75.903 }, + {0.33624,2.1123,15.162,-79.088 }, + {0.34067,2.1779,14.47,-83.322 }, + {0.34509,2.24,13.609,-88.273 }, + {0.34952,2.2979,12.55,-93.472 }, + {0.35394,2.3507,11.283,-98.369 }, + {0.35836,2.3974,9.8194,-102.39 }, + {0.36279,2.4373,8.1988,-105.01 }, + {0.36721,2.4698,6.4835,-105.77 }, + {0.37164,2.4947,4.7502,-104.37 }, + {0.37606,2.512,3.0759,-100.74 }, + {0.38048,2.5221,1.5245,-95.063 }, + {0.38491,2.5257,0.13371,-87.783 }, + {0.38933,2.5235,-1.0913,-79.537 }, + {0.39376,2.5163,-2.178,-71.042 }, + {0.39818,2.5045,-3.1764,-62.958 }, + {0.40261,2.4884,-4.1428,-55.768 }, + {0.40703,2.468,-5.122,-49.731 }, + {0.41145,2.443,-6.136,-44.898 }, + {0.41588,2.4132,-7.1827,-41.179 }, + {0.4203,2.3784,-8.2437,-38.413 }, + {0.42473,2.3383,-9.2979,-36.422 }, + {0.42915,2.2933,-10.332,-35.021 }, + {0.43358,2.2443,-11.347,-34.007 }, + {0.438,2.193,-12.35,-33.158 }}; + +//============================================================================== +// INCLUDES +//============================================================================== + + + +#include "../Millard2016TorqueMuscle.h" +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + #include "../TorqueMuscleFittingToolkit.h" +#endif +#include "../csvtools.h" +#include "../../geometry/tests/numericalTestFunctions.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace std; +/* + Constructor tests: + 1. Coefficients are copied over correctly. + 2. Curves are made correctly + + calcTorqueMuscleInfo test + stiffness calculation + power calculation + +*/ + + +TEST(ConstructorRegularCallCheck) +{ + + + //Check that the 3 constructors when called properly + //do not abort + Millard2016TorqueMuscle test0 = Millard2016TorqueMuscle(); + + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + + Millard2016TorqueMuscle test2 = + Millard2016TorqueMuscle( + DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + 0.0, + 1.0, + 1.0, + "test_easyConstructor"); + + CHECK(fabs( test2.getPassiveTorqueScale()-1.0) < TOL); + +} + + + +TEST(calcJointTorqueCorrectnessTests){ + + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double err = 0.0; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Anderson2007, + subjectInfo, + Anderson2007::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + + bool flagMakeTestVector = false; + if(flagMakeTestVector){ + Millard2016TorqueMuscle tqG = + Millard2016TorqueMuscle(DataSet::Gymnast, + subjectInfo, + Gymnast::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + TorqueMuscleInfo tmiG; + tqG.calcTorqueMuscleInfo(M_PI/3.0,0.1,0.77,tmiG); + + printf("%f\n",tmiG.fiberAngle); + printf("%f\n",tmiG.fiberAngularVelocity); + printf("%f\n",tmiG.activation); + printf("%f\n",tmiG.fiberTorque); + printf("%f\n",tmiG.fiberStiffness); + printf("%f\n",tmiG.fiberPassiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberActiveTorqueAngleMultiplier); + printf("%f\n",tmiG.fiberTorqueAngularVelocityMultiplier); + printf("%f\n",tmiG.fiberPassiveTorque); + printf("%f\n",tmiG.fiberActiveTorque); + printf("%f\n",tmiG.fiberDampingTorque); + printf("%f\n",tmiG.fiberNormDampingTorque); + printf("%f\n",tmiG.fiberActivePower); + printf("%f\n",tmiG.fiberPassivePower); + printf("%f\n",tmiG.fiberPower); + printf("%f\n",tmiG.DjointTorque_DjointAngle); + printf("%f\n",tmiG.DjointTorque_DjointAngularVelocity); + printf("%f\n",tmiG.DjointTorque_Dactivation); + + } + + + //Zero out the passive forces so that calcMuscleTorque reports + //just the active force - this allows us to test its correctness. + tq.setPassiveTorqueScale(0.0); + double tmp = tq.calcJointTorque(0,0,1.0); + + //Test that the get and set functions work for + //maximum isometric torque + double tauMaxOld = tq.getMaximumActiveIsometricTorque(); + double tauMax = tauMaxOld*10.0; + tq.setMaximumActiveIsometricTorque(tauMax); + tmp = tq.calcJointTorque(0,0,1.0); + //ensures updateTorqueMuscleCurves is called + CHECK(fabs( tq.getMaximumActiveIsometricTorque()-tauMax) + < TOL ); + + double omegaMaxOld = tq.getMaximumConcentricJointAngularVelocity(); + double omegaMax = 2.0*fabs(omegaMaxOld); + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + tmp = tq.calcJointTorque(0,0,1.0); + //ensures updateTorqueMuscleCurves is called + CHECK(fabs( fabs(tq.getMaximumConcentricJointAngularVelocity())-omegaMax) + < TOL ); + + + double taAngleScalingOld = tq.getActiveTorqueAngleCurveAngleScaling(); + double taAngleScaling = 2.0*taAngleScalingOld; + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScaling); + + CHECK(fabs(taAngleScaling-tq.getActiveTorqueAngleCurveAngleScaling()) = 23); + + //For joint-torque related constrains that use x = [a,v,p,o] + //(see the code for the parts of calcTorqueMuscleInfo that evaluate + //the second derivatives for further detail. + //Test the first column of the Hessian + //0: D^2 tau / D lambdaTa^2 + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda+h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtaLambda2 = + (tmiR.DjointTorque_DactiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DactiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambda2 + - tmi.fittingInfo[0]); + CHECK(err < SQRTEPSILON*100); + + + //1: Test D^2 tau / D TaLambda D TvLambda + double D2tau_DtaLambdaDtvLambda = + (tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable + - tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtvLambda + -tmi.fittingInfo[1]); + CHECK(err < SQRTEPSILON*100); + + //3: Test D^2 tau/ D TaLambda D TpLambda + double D2tau_DtaLambdaDtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtpLambda + -tmi.fittingInfo[3]); + CHECK(err < SQRTEPSILON*100); + + //6: Test D^2 tau/ D TaLambda DTpOffset + double D2tau_DtaLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtaLambdaDtpOffset - + tmi.fittingInfo[6]); + + //Check the 2nd column of the Hessian + //2: Test D^2 tau/ D Tvlambda^2 + tq.setActiveTorqueAngleCurveBlendingVariable( taLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setTorqueAngularVelocityCurveBlendingVariable( tvLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtvLambda2 = + (tmiR.DjointTorque_DtorqueAngularVelocityBlendingVariable + - tmiL.DjointTorque_DtorqueAngularVelocityBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtvLambda2 + - tmi.fittingInfo[2]); + CHECK(err < SQRTEPSILON*100); + + //4: Test D^2 tau/ D TvLambda D Tplambda + double D2tau_DtvLambdaDtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtvLambdaDtpLambda + - tmi.fittingInfo[4]); + CHECK(err < SQRTEPSILON*100); + + //7: Test D^2 tau/ D TvLambda D TpOffset + double D2tau_DtvLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtvLambdaDtpOffset + - tmi.fittingInfo[7]); + CHECK(err < SQRTEPSILON*100); + + //Check the 3rd column of the Hessian. + //5: D^2 tau / D TpLambda^2 + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtpLambda2 = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtpLambda2 + - tmi.fittingInfo[5]); + CHECK(err < SQRTEPSILON*100); + + //8: D^2 tau/ D TpLambda DTpOffset + double D2tau_DtpLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpLambdaDtpOffset + - tmi.fittingInfo[8]); + CHECK(err < SQRTEPSILON*100); + + //Check the 4th column of the Hessian. + //9: D^2 tau/ D TpOffset^2 + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + + tq.setPassiveCurveAngleOffset(0.); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tau_DtpOffset2 = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpOffset2 + - tmi.fittingInfo[9]); + CHECK(err < SQRTEPSILON*1000); + + +// For constraints on the value of the passive element +// 10: d2p/dp2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda+h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tp_DtpLambda2 = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DblendingVariable + - tmiL.DfiberPassiveTorqueAngleMultiplier_DblendingVariable)/(2.0*h); + + err = fabs(D2tp_DtpLambda2 + - tmi.fittingInfo[10]); + CHECK(err < SQRTEPSILON*1000); + + // 11: d2p/dpdo + double D2tp_DtpLambda_DangleOffset = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset + -tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h); + + err = fabs(D2tp_DtpLambda_DangleOffset + -tmi.fittingInfo[11]); + CHECK(err < SQRTEPSILON*1000); + +// 12: d2p/do2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setPassiveCurveAngleOffset(0.); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + double D2tp_DtpOffset2 = + (tmiR.DfiberPassiveTorqueAngleMultiplier_DangleOffset + - tmiL.DfiberPassiveTorqueAngleMultiplier_DangleOffset)/(2.0*h); + + err = fabs(D2tp_DtpOffset2 + - tmi.fittingInfo[12]); + CHECK(err < SQRTEPSILON*1000); + + +// For joint-torque related constraints that use x = [s,m,p,o] +// 13: d2t/ds2 + tq.setPassiveTorqueAngleCurveBlendingVariable( 0.); + tq.setTorqueAngularVelocityCurveBlendingVariable( 0.); + tq.setActiveTorqueAngleCurveBlendingVariable( 0.); + + jointAngleAtOneNormTorque = tq.getJointAngleAtMaximumActiveIsometricTorque(); + + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmi); + tq.setActiveTorqueAngleCurveAngleScaling(1.0-h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiL); + tq.setActiveTorqueAngleCurveAngleScaling(1.0+h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque+0.5,omegaMid,1.0,tmiR); + + double D2tau_DtaAngleScaling2 = + (tmiR.DjointTorque_DactiveTorqueAngleAngleScaling + - tmiL.DjointTorque_DactiveTorqueAngleAngleScaling)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling2 + - tmi.fittingInfo[13]); + CHECK(err < SQRTEPSILON*1000); + +// 14: d2t/dsdm + double D2tau_DtaAngleScaling_DomegaMax = + (tmiR.DjointTorque_DmaximumAngularVelocity + - tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DomegaMax + - tmi.fittingInfo[14]); + CHECK(err < SQRTEPSILON*100); + + // 16: d2t/dsdp + double D2tau_DtaAngleScaling_DtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DtpLambda + -tmi.fittingInfo[16]); + CHECK(err < SQRTEPSILON*100); + // 19: d2t/dsdo + double D2tau_DtaAngleScaling_DtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtaAngleScaling_DtpOffset + -tmi.fittingInfo[19]); + CHECK(err < SQRTEPSILON*100); + +// 15: d2t/dm2 + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + omegaMax = tq.getMaximumConcentricJointAngularVelocity(); + omegaMid = 0.5*omegaMax; + + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmi); + tq.setMaximumConcentricJointAngularVelocity(omegaMax-h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiL); + tq.setMaximumConcentricJointAngularVelocity(omegaMax+h); + tq.calcTorqueMuscleInfo(jointAngleAtOneNormTorque,omegaMid,1.0,tmiR); + + double D2tau_DomegaMax2 = + (tmiR.DjointTorque_DmaximumAngularVelocity + - tmiL.DjointTorque_DmaximumAngularVelocity)/(2.0*h); + + err = fabs(D2tau_DomegaMax2 + - tmi.fittingInfo[15]); + CHECK(err < SQRTEPSILON*100); + + // 17: d2t/dmdp + double D2tau_DomegaMax_DtpLambda = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + -tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + err = fabs(D2tau_DomegaMax_DtpLambda + - tmi.fittingInfo[17]); + CHECK(err < SQRTEPSILON*100); + + // 20: d2t/dmdo + double D2tau_DomegaMax_DtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + -tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + err = fabs(D2tau_DomegaMax_DtpOffset + - tmi.fittingInfo[20]); + CHECK(err < SQRTEPSILON*100); + +// 18: d2t/dp2 + tq.setActiveTorqueAngleCurveBlendingVariable(taLambda); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + tq.setPassiveCurveAngleOffset(0.0); + + tq.setActiveTorqueAngleCurveAngleScaling(1.0); + tq.setMaximumConcentricJointAngularVelocity(omegaMax); + + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda - h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveTorqueAngleCurveBlendingVariable( tpLambda + h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + D2tau_DtpLambda2 = + (tmiR.DjointTorque_DpassiveTorqueAngleBlendingVariable + - tmiL.DjointTorque_DpassiveTorqueAngleBlendingVariable)/(2.0*h); + + err = fabs(D2tau_DtpLambda2 + - tmi.fittingInfo[18]); + CHECK(err < SQRTEPSILON*100); + + // 21: d2t/dpdo + + D2tau_DtpLambdaDtpOffset = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpLambdaDtpOffset + - tmi.fittingInfo[21]); + CHECK(err < SQRTEPSILON*100); + + + // 22: d2t/do2 + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setPassiveCurveAngleOffset(0.); + + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmi); + tq.setPassiveCurveAngleOffset(-h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiL); + tq.setPassiveCurveAngleOffset( h); + tq.calcTorqueMuscleInfo(jointAngleTpMid,omegaMid,1.0,tmiR); + + D2tau_DtpOffset2 = + (tmiR.DjointTorque_DpassiveTorqueAngleCurveAngleOffset + - tmiL.DjointTorque_DpassiveTorqueAngleCurveAngleOffset)/(2.0*h); + + err = fabs(D2tau_DtpOffset2 + - tmi.fittingInfo[22]); + CHECK(err < SQRTEPSILON*1000); + + + +#endif + +} + + +#ifdef RBDL_BUILD_ADDON_MUSCLE_FITTING + +TEST(fittingEasyTest){ + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + double signOfJointVelocity = signOfJointTorque; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + Millard2016TorqueMuscle tq = + Millard2016TorqueMuscle(DataSet::Gymnast, + subjectInfo, + Gymnast::HipExtension, + jointAngleOffset, + signOfJointAngle, + signOfJointTorque, + name); + +// Testing approach +// 1. Generate a data set consistent with a muscle that is more flexible, +// is stronger, and has modified curves from the default. +// 2. Return the muscle back to its default values of flexibility, maximum +// isometric torque and blending variables. +// 3. Run the fitting algorithm to see if the parameters from step 1 can be +// identified. + + + double tisoOriginal = tq.getMaximumActiveIsometricTorque(); + double tisoUpd = 1.2*tisoOriginal; + double omegaMaxOrig = tq.getMaximumConcentricJointAngularVelocity(); + double omegaMaxUpd = 2.5*omegaMaxOrig; + + double taAngleScalingOrig = tq.getActiveTorqueAngleCurveAngleScaling(); + double taAngleScalingUpd = taAngleScalingOrig*1.35; + + double taLambda = 0.0; + double tpLambda = 0.0; + double tvLambda = 0.0; + + + double angleToOneOrig = + tq.getJointAngleAtOneNormalizedPassiveIsometricTorque(); + + tq.setActiveTorqueAngleCurveBlendingVariable(taLambda); + tq.setPassiveTorqueAngleCurveBlendingVariable(tpLambda); + tq.setTorqueAngularVelocityCurveBlendingVariable(tvLambda); + tq.setMaximumActiveIsometricTorque(tisoUpd); + tq.setMaximumConcentricJointAngularVelocity(omegaMaxUpd); + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingUpd); + + unsigned int npts = 100; + RigidBodyDynamics::Math::VectorNd angle, angularVelocity, torque; + angle.resize(npts); + angularVelocity.resize(npts); + torque.resize(npts); + + //Generate the data set. + double activationUpperBound = 0.95; + double passiveTorqueAngleMultiplierUpperBound = 0.; + TorqueMuscleInfo tmi; + TorqueMuscleSummary tms; + + double angularRange = M_PI/3.0; + double angleTpOne = + tq.getJointAngleAtOneNormalizedPassiveIsometricTorque(); + double angularOffset = angleTpOne-angularRange; + double time = 0.; + double normTime = 0.; + double omega = omegaMaxUpd/angularRange; + for(unsigned int i = 0; i passiveTorqueAngleMultiplierUpperBound){ + passiveTorqueAngleMultiplierUpperBound = + tmi.fiberPassiveTorqueAngleMultiplier; + } + } + //Return the muscle back to its defaults. + tq.setActiveTorqueAngleCurveBlendingVariable(0.0); + tq.setPassiveTorqueAngleCurveBlendingVariable(0.0); + tq.setTorqueAngularVelocityCurveBlendingVariable(0.0); + tq.setMaximumActiveIsometricTorque(tisoOriginal); + tq.setMaximumConcentricJointAngularVelocity(omegaMaxOrig); + tq.setActiveTorqueAngleCurveAngleScaling(taAngleScalingOrig); + + //Run the fitting routine. + passiveTorqueAngleMultiplierUpperBound *= 0.75; + TorqueMuscleParameterFittingData tmFittingData; + + + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters( + tq, angle,angularVelocity,torque, + activationUpperBound, + passiveTorqueAngleMultiplierUpperBound, + tmFittingData, false); + //Now to test the result, update the parameters of the + //model and run through all of the test vectors. The muscle + //should produce a torque that is >= the desired torque + //of the same sign. There should be one value where it + //,to numerical precision produces just the required torque + //and no more. + + CHECK(tmFittingData.fittingConverged == true); + + + tq.setFittedParameters(tmFittingData); + + double minActivation = 1.e20; + double maxActivation = -1.e20; + double maxPassiveTorqueAngleMultiplier = -1.0e20; + for(unsigned int i=0; i 0){ + tq.calcActivation(angle[i],angularVelocity[i],torque[i],tms); + + if(tms.activation < minActivation){ + minActivation = tms.activation; + } + if(tms.activation > maxActivation){ + maxActivation = tms.activation; + } + + if(tms.fiberPassiveTorqueAngleMultiplier + > maxPassiveTorqueAngleMultiplier){ + maxPassiveTorqueAngleMultiplier = tms.fiberPassiveTorqueAngleMultiplier; + } + } + } + double err = maxActivation-activationUpperBound; + CHECK(err <= 10.0*SQRTEPSILON); + + err = minActivation; + CHECK(err >= -10.0*SQRTEPSILON); + + err = (maxPassiveTorqueAngleMultiplier + - passiveTorqueAngleMultiplierUpperBound); + CHECK(err < SQRTEPSILON); + +} + +TEST(fittingHardTest) +{ + //std::vector< std::vector< double > > data; + //std::string fileName("TorqueMuscleFittingToolkitHardTestCase_TimeQQDotTau.csv"); + //data = readMatrixFromFile(fileName, 0); + + std::string name("hardTest"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + double angleOffset = 0.; + double angleSign = 1; + double torqueSign =-1; + + Millard2016TorqueMuscle kneeExt(DataSet::Gymnast, + subjectInfo, + Gymnast::KneeExtension, + angleOffset, + angleSign, + torqueSign, + name); + + RigidBodyDynamics::Math::VectorNd q, qDot, tau; + q.resize( TorqueMuscleFittingHardTestCaseRows); + qDot.resize(TorqueMuscleFittingHardTestCaseRows); + tau.resize( TorqueMuscleFittingHardTestCaseRows); + + double tauMax = 0; + double omegaMax = 0; + + for(unsigned int i=0; iomegaMax){ + omegaMax = fabs(qDot[i]); + } + if(fabs(tau[i])>tauMax){ + tauMax = fabs(tau[i]); + } + } + + + double activationUB = 0.9; + double tpUB = 0.75; + bool verbose = false; + + + TorqueMuscleParameterFittingData fittingData; + TorqueMuscleFittingToolkit::fitTorqueMuscleParameters(kneeExt,q,qDot,tau, + activationUB,tpUB, + fittingData,verbose); + CHECK(fittingData.fittingConverged == true); +} + +#endif + + +TEST(exampleUsage){ + + + bool printCurves = false; + bool printAllCurves = false; + + //int dataSet = DataSetAnderson2007; + + //int gender = 0; //male + //int age = 0; //young + + double jointAngleOffset = 0; + double signOfJointAngle = 1; + double signOfJointTorque = 1; + + std::string name("test"); + + SubjectInformation subjectInfo; + subjectInfo.gender = GenderSet::Male; + subjectInfo.ageGroup = AgeGroupSet::Young18To25; + subjectInfo.heightInMeters = 1.732; + subjectInfo.massInKg = 69.0; + + std::vector< Millard2016TorqueMuscle > muscleVector; + + bool exception = false; + + double angleTorqueSigns[][2] = {{-1, 1}, + {-1,-1}, + { 1,-1}, + { 1, 1}, + {-1, 1}, + {-1,-1}}; + + Millard2016TorqueMuscle tqMuscle; + std::stringstream tqName; + int tqIdx; + + for(int i=0; i < Anderson2007::LastJointTorque; ++i){ + + tqName.str(""); + tqName << DataSet.names[0] + < +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; +//using namespace OpenSim; +//using namespace SimTK; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + + + +TEST(tendonCurve) +{ + //cout <<"**************************************************"< +#include +#include +#include +#include +#include +#include + +using namespace RigidBodyDynamics::Addons::Geometry; +using namespace RigidBodyDynamics::Addons::Muscle; +using namespace std; + +/* +static double EPSILON = numeric_limits::epsilon(); + +static bool FLAG_PLOT_CURVES = false; +static string FILE_PATH = ""; +static double TOL_DX = 5e-3; +static double TOL_DX_BIG = 1e-2; +static double TOL_BIG = 1e-6; +static double TOL_SMALL = 1e-12; +*/ + +TEST(Anderson2007ActiveTorqueAngleCurve) +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + + + //cout < tauMax){ + double tmp = tauMin; + tauMin = tauMax; + tauMax = tmp; + tauMinAngle = angleMax; + } + + CHECK( abs(tauMin) < TOL_SMALL); + CHECK( abs(tauMax - 1.0) < TOL_SMALL); + CHECK( abs(andersonTpCurve.calcDerivative(tauMinAngle,1)) < TOL_SMALL); + + //cout << " passed " << endl; + + //cout << " Continuity and Smoothness Testing " << endl; + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + andersonTpCurve, + andersonTpCurveSample, + TOL_DX); + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous(andersonTpCurve, + andersonTpCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(andersonTpCurveSample); + CHECK(curveIsMonotonic); + //cout << " passed " << endl; + + + } + + SmoothSegmentedFunction andersonTpCurve = SmoothSegmentedFunction(); + TorqueMuscleFunctionFactory:: + createAnderson2007PassiveTorqueAngleCurve( + scale, + c1, + b1, + k1, + b2, + k2, + "test_passiveTorqueAngleCurve", + andersonTpCurve); + + if(FLAG_PLOT_CURVES){ + andersonTpCurve.printCurveToCSVFile( + FILE_PATH, + "anderson2007PassiveTorqueAngleCurve", + andersonTpCurve.getCurveDomain()[0]-0.1, + andersonTpCurve.getCurveDomain()[1]+0.1); + } + +} + +TEST(Anderson2007ActiveTorqueVelocityCurve) +{ + double subjectWeight = 75.0*9.81; + double subjectHeight = 1.75; + double scale = subjectHeight*subjectWeight; + + //These parameters are taken from table 3 for hip extension for + //men between the ages of 18-25 + double c1 = 0.161; //normalized maximum hip joint torque + double c2 = 0.958; // pi/(theta_max - theta_min) + double c3 = 0.932; //theta_max_iso_torque + double c4 = 1.578; //omega_1: angular velocity at 75% tq_iso_max + double c5 = 3.190; //omega_2: angular velocity at 50% tq_iso_max + double c6 = 0.242; //E, where eccentric slope = (1+E)*concentricSlope + //Passive torque angle curve parameters + double b1 =-1.210; // torque_passive = b1*exp(k1*theta) + double k1 =-6.351; // +b2*exp(k2*theta) + double b2 = 0.476; + double k2 = 5.910; + + //cout <= minEccentricMultiplier); + CHECK(maxTv <= maxEccentricMultiplier); + + CHECK(abs(andersonTvCurve.calcDerivative + (angularVelocityMax/angularVelocityMax,1)) wmaxC){ + CHECK( abs( tv.calcValue(w) ) < TOL_SMALL); + CHECK( abs( tv.calcDerivative(w,1) ) < TOL_SMALL); + CHECK( abs(tvX.calcDerivative(w,1) + - slopeAtConcentricOmegaMax ) < TOL_BIG); + }else if(w > 0 && w <= wmaxC){ + tvHill = (b*fiso-a*w)/(b+w); + errHill = abs(tv.calcValue(w)-tvHill); + //printf("%i. Err %f, ",i,errHill); + CHECK( errHill < 0.02); + errHill = abs(tvX.calcValue(w)-tvHill); + //printf(" Err %f\n",errHill); + CHECK( errHill < 0.02); + }else if(w < 0 & w > wmaxE){ + CHECK(tv.calcValue(w) > 1.0); + }else if(w < wmaxE){ + CHECK(abs( tv.calcValue(w) - tvAtEccentricOmegaMax ) < TOL_SMALL); + CHECK(abs( tv.calcDerivative(w,1) - 0.0 ) < TOL_SMALL); + //CHECK(abs( tvX.calcValue(w) - tvAtEccentricOmegaMax ) ); + CHECK(abs( tvX.calcDerivative(w,1) + - slopeAtEccentricOmegaMax ) < TOL_SMALL ); + } + } + + RigidBodyDynamics::Math::VectorNd curveDomain = tv.getCurveDomain(); + + double angularVelocityMin = curveDomain[0]; + double angularVelocityMax = curveDomain[1]; + + + RigidBodyDynamics::Math::MatrixNd tvCurveSample + = tv.calcSampledCurve( 6, + angularVelocityMin-0.1, + angularVelocityMax+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tv, + tvCurveSample, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + bool curveIsContinuous = isCurveC2Continuous( tv, + tvCurveSample, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tvCurveSample); + CHECK(curveIsMonotonic); + + if(FLAG_PLOT_CURVES){ + tv.printCurveToCSVFile( + FILE_PATH, + "millard2016TorqueVelocityCurve", + -1.1, + 1.1); + } + +} + +//============================================================================== +TEST(PassiveTorqueAngleCurve) +{ + SmoothSegmentedFunction tp = SmoothSegmentedFunction(); + SmoothSegmentedFunction tpX = SmoothSegmentedFunction(); + double angleAtZeroTorque0 = 0; + double angleAtOneNormTorque0 = -M_PI; + + double angleAtZeroTorque1 = 0; + double angleAtOneNormTorque1 = M_PI; + + double stiffnessAtOneNormTorque1 = + 5.6/(angleAtOneNormTorque1-angleAtZeroTorque1); + double stiffnessAtLowTorque1 = + 0.05*stiffnessAtOneNormTorque1; + double curviness1 = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque0, + angleAtOneNormTorque0, + curveName0, + tp); + + TorqueMuscleFunctionFactory::createPassiveTorqueAngleCurve( + angleAtZeroTorque1, + angleAtOneNormTorque1, + stiffnessAtLowTorque1, + stiffnessAtOneNormTorque1, + curviness1, + curveName1, + tpX); + + CHECK( abs(tp.calcValue(angleAtZeroTorque0)) < TOL_SMALL); + CHECK( abs(tp.calcValue(angleAtOneNormTorque0)-1.0) < TOL_SMALL); + + CHECK( abs(tpX.calcValue(angleAtZeroTorque1)) < TOL_SMALL); + CHECK( abs(tpX.calcValue(angleAtOneNormTorque1) - 1.0) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtZeroTorque1,1)) < TOL_SMALL); + CHECK( abs(tpX.calcDerivative(angleAtOneNormTorque1,1) + -stiffnessAtOneNormTorque1) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tp.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = tpX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd tpSample0 + = tp.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd tpSample1 + = tpX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tp, + tpSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tpX, + tpSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tp, + tpSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(tpX, + tpSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(tpSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(tpSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tp.printCurveToCSVFile( + FILE_PATH, + "millard2016PassiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } +} + +//============================================================================== +TEST(ActiveTorqueAngleCurve) +{ + SmoothSegmentedFunction ta = SmoothSegmentedFunction(); + SmoothSegmentedFunction taX = SmoothSegmentedFunction(); + double angleAtOneNormTorque = 1.5; + double angularStandardDeviation = 1.0; + double angularStandardDeviationSq = + angularStandardDeviation*angularStandardDeviation; + + double minSlopeAtShoulders = 0.2; + double minValueAtShoulders = 0.1; + double xTrans = sqrt(-log(minValueAtShoulders)*2*angularStandardDeviationSq); + double delta = abs(xTrans+angleAtOneNormTorque); + double curviness = 0.75; + + std::string curveName0("tpTest0"); + std::string curveName1("tpTest1"); + + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + curveName0, + ta); + + TorqueMuscleFunctionFactory::createGaussianShapedActiveTorqueAngleCurve( + angleAtOneNormTorque, + angularStandardDeviation, + minSlopeAtShoulders, + minValueAtShoulders, + curviness, + curveName1, + taX); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ta.calcValue(angleAtOneNormTorque + +10.0*angularStandardDeviation)) < TOL_SMALL); + CHECK(abs(ta.calcValue(angleAtOneNormTorque + -10.0*angularStandardDeviation)) < TOL_SMALL); + + CHECK(abs(taX.calcValue(angleAtOneNormTorque)-1.0) < TOL_SMALL); + + double err = abs(taX.calcDerivative(angleAtOneNormTorque + delta,1) + + minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + err = abs(taX.calcDerivative(angleAtOneNormTorque - delta,1) + - minSlopeAtShoulders); + CHECK(err < TOL_SMALL); + + + RigidBodyDynamics::Math::VectorNd curveDomain0 = ta.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = taX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd taSample0 + = ta.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd taSample1 + = taX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ta, + taSample0, + TOL_DX); + + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + taX, + taSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( ta, + taSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(taX, + taSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + if(FLAG_PLOT_CURVES){ + ta.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + } + +} + +//============================================================================== +TEST(TendonTorqueAngleCurve) +{ + SmoothSegmentedFunction tt = SmoothSegmentedFunction(); + SmoothSegmentedFunction ttX = SmoothSegmentedFunction(); + + double angularStretchAtOneNormTorque = M_PI/2.0; + double stiffnessAtOneNormTorque = 2.5/angularStretchAtOneNormTorque; + double normTorqueAtToeEnd = 2.0/3.0; + double curviness = 0.5; + + std::string curveName0("ttTest0"); + std::string curveName1("ttTest1"); + + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + curveName0, + tt); + + TorqueMuscleFunctionFactory::createTendonTorqueAngleCurve( + angularStretchAtOneNormTorque, + stiffnessAtOneNormTorque, + normTorqueAtToeEnd, + curviness, + curveName1, + ttX); + + CHECK(abs(tt.calcValue(0)) < TOL_SMALL); + CHECK(abs(ttX.calcValue(0)) < TOL_SMALL); + + CHECK(abs(tt.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + CHECK(abs(ttX.calcValue(angularStretchAtOneNormTorque)-1.0) < TOL_SMALL); + + CHECK(abs(ttX.calcDerivative(angularStretchAtOneNormTorque,1) + - stiffnessAtOneNormTorque) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomain0 = tt.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomain1 = ttX.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd ttSample0 + = tt.calcSampledCurve( 6, + curveDomain0[0]-0.1, + curveDomain0[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd ttSample1 + = ttX.calcSampledCurve( 6, + curveDomain1[0]-0.1, + curveDomain1[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + tt, + ttSample0, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + ttX, + ttSample1, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( tt, + ttSample0, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(ttX, + ttSample1, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(ttSample0); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(ttSample1); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + tt.printCurveToCSVFile( + FILE_PATH, + "millard2016ActiveTorqueAngleCurve", + curveDomain0[0]-0.1, + angularStretchAtOneNormTorque); + } + +} + +//============================================================================== +TEST(DampingBlendingCurve) +{ + SmoothSegmentedFunction negOne = SmoothSegmentedFunction(); + SmoothSegmentedFunction posOne = SmoothSegmentedFunction(); + + std::string curveName0("neg1"); + std::string curveName1("pos1"); + + + TorqueMuscleFunctionFactory::createDampingBlendingCurve( + -1.0,curveName0,negOne); + + TorqueMuscleFunctionFactory::createDampingBlendingCurve( + 1.0,curveName0,posOne); + + + CHECK(abs(negOne.calcValue(0)) < TOL_SMALL); + CHECK(abs(posOne.calcValue(0)) < TOL_SMALL); + + CHECK(abs(negOne.calcValue(-1.0)-1.0) < TOL_SMALL); + CHECK(abs(posOne.calcValue(1.0)-1.0) < TOL_SMALL); + + CHECK(abs(negOne.calcDerivative( 0,1)) < TOL_SMALL); + CHECK(abs(negOne.calcDerivative(-1,1)) < TOL_SMALL); + + CHECK(abs(posOne.calcDerivative( 0,1)) < TOL_SMALL); + CHECK(abs(posOne.calcDerivative( 1,1)) < TOL_SMALL); + + RigidBodyDynamics::Math::VectorNd curveDomainNegOne = negOne.getCurveDomain(); + RigidBodyDynamics::Math::VectorNd curveDomainPosOne = posOne.getCurveDomain(); + + RigidBodyDynamics::Math::MatrixNd sampleNegOne + = negOne.calcSampledCurve( 6, + curveDomainNegOne[0]-0.1, + curveDomainNegOne[1]+0.1); + + RigidBodyDynamics::Math::MatrixNd samplePosOne + = posOne.calcSampledCurve( 6, + curveDomainPosOne[0]-0.1, + curveDomainPosOne[1]+0.1); + + bool areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + negOne, + sampleNegOne, + TOL_DX); + + CHECK(areCurveDerivativesGood); + + areCurveDerivativesGood = + areCurveDerivativesCloseToNumericDerivatives( + posOne, + samplePosOne, + TOL_DX); + + CHECK(areCurveDerivativesGood); + bool curveIsContinuous = isCurveC2Continuous( negOne, + sampleNegOne, + TOL_BIG); + CHECK(curveIsContinuous); + curveIsContinuous = isCurveC2Continuous(posOne, + samplePosOne, + TOL_BIG); + CHECK(curveIsContinuous); + + bool curveIsMonotonic = isCurveMontonic(sampleNegOne); + CHECK(curveIsMonotonic); + + curveIsMonotonic = isCurveMontonic(samplePosOne); + CHECK(curveIsMonotonic); + + + if(FLAG_PLOT_CURVES){ + negOne.printCurveToCSVFile( + FILE_PATH, + "millard2016DampingBlendingCurve", + curveDomainNegOne[0]-0.1, + curveDomainNegOne[1]+0.1); + } + +} diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake new file mode 100644 index 0000000..7ce4e85 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/FindURDF.cmake @@ -0,0 +1,88 @@ +# - Try to find URDF +# +# + +SET (URDF_FOUND FALSE) +SET (CONSOLE_BRIDGE_FOUND FALSE) +SET (URDFDOM_HEADERS_FOUND FALSE) +SET (URDFDOM_FOUND FALSE) + +FIND_PATH (CONSOLE_BRIDGE_DIR console_bridge/console.h + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (CONSOLE_BRIDGE_LIBRARY NAMES console_bridge PATHS + /usr/local/include + /usr/include + ) + +FIND_PATH (URDFDOM_HEADERS_DIR urdf_model/model.h + /usr/local/include + /usr/include + ) + +FIND_PATH (URDFDOM_DIR urdf_parser/urdf_parser.h + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (URDFDOM_MODEL_LIBRARY NAMES urdfdom_model PATHS + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (URDFDOM_WORLD_LIBRARY NAMES urdfdom_world PATHS + /usr/local/include + /usr/include + ) + +IF (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + MESSAGE(ERROR "Could not find URDF: console_bridge not found") +ELSE (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + SET (CONSOLE_BRIDGE_FOUND TRUE) +ENDIF (NOT CONSOLE_BRIDGE_DIR OR NOT CONSOLE_BRIDGE_LIBRARY) + +IF (NOT URDFDOM_HEADERS_DIR) + MESSAGE(ERROR "Could not find URDF: urdfdom_headers not found") +ELSE (NOT URDFDOM_HEADERS_DIR) + SET (URDFDOM_HEADERS_FOUND TRUE) +ENDIF (NOT URDFDOM_HEADERS_DIR) + +IF (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + MESSAGE(ERROR "Could not find URDF: urdfdom_model or urdfdom_world library not found") +ELSE (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + SET (URDFDOM_FOUND TRUE) +ENDIF (NOT URDFDOM_DIR OR NOT URDFDOM_MODEL_LIBRARY OR NOT URDFDOM_WORLD_LIBRARY) + +IF (CONSOLE_BRIDGE_FOUND AND URDFDOM_HEADERS_FOUND AND URDFDOM_FOUND) + SET (URDF_FOUND TRUE) +ENDIF (CONSOLE_BRIDGE_FOUND AND URDFDOM_HEADERS_FOUND AND URDFDOM_FOUND) + +IF (URDF_FOUND) + IF (NOT URDF_FIND_QUIETLY) + MESSAGE(STATUS "Found URDF console_bridge: ${CONSOLE_BRIDGE_LIBRARY}") + MESSAGE(STATUS "Found URDF urdfdom_headers: ${URDFDOM_HEADERS_DIR}") + MESSAGE(STATUS "Found URDF urdfdom: ${URDFDOM_LIBRARY}") + ENDIF (NOT URDF_FIND_QUIETLY) + SET (URDF_INCLUDE_DIRS + ${CONSOLE_BRIDGE_DIR} + ${URDFDOM_HEADERS_DIR} + ${URDFDOM_DIR} + ) + SET (URDF_LIBRARIES + ${CONSOLE_BRIDGE_LIBRARY} + ${URDFDOM_WORLD_LIBRARY} + ${URDFDOM_MODEL_LIBRARY} + ) + MESSAGE (STATUS "URDF Libraries: ${URDF_LIBRARIES}") +ELSE (URDF_FOUND) + IF (URDF_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find URDF") + ENDIF (URDF_FIND_REQUIRED) +ENDIF (URDF_FOUND) + +MARK_AS_ADVANCED ( + ${URDF_INCLUDE_DIRS} + ${URDF_LIBRARIES} + ) diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake new file mode 100644 index 0000000..113ca7b --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/pkg-config.cmake @@ -0,0 +1,369 @@ +# Copyright (C) 2010 Thomas Moulard, JRL, CNRS/AIST. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +INCLUDE(CMake/shared-library.cmake) + +FIND_PACKAGE(PkgConfig) + +# Additional pkg-config variables whose value will be imported +# during the dependency check. +SET(PKG_CONFIG_ADDITIONAL_VARIABLES datarootdir pkgdatarootdir docdir doxygendocdir) + +# _SETUP_PROJECT_PKG_CONFIG +# ------------------------- +# +# Prepare pkg-config pc file generation step. +# +MACRO(_SETUP_PROJECT_PKG_CONFIG) + # Pkg-config related commands. + SET(PKG_CONFIG_PREFIX "${CMAKE_INSTALL_PREFIX}") + SET(PKG_CONFIG_EXEC_PREFIX "${PKG_CONFIG_PREFIX}") + SET(PKG_CONFIG_LIBDIR "${PKG_CONFIG_EXEC_PREFIX}/lib") + SET(PKG_CONFIG_INCLUDEDIR "${PKG_CONFIG_PREFIX}/include") + SET(PKG_CONFIG_DATAROOTDIR "${PKG_CONFIG_PREFIX}/share") + SET(PKG_CONFIG_PKGDATAROOTDIR "${PKG_CONFIG_PREFIX}/share/${PROJECT_NAME}") + SET(PKG_CONFIG_DOCDIR "${PKG_CONFIG_DATAROOTDIR}/doc/${PROJECT_NAME}") + SET(PKG_CONFIG_DOXYGENDOCDIR "${PKG_CONFIG_DOCDIR}/doxygen-html") + + SET(PKG_CONFIG_PROJECT_NAME "${PROJECT_NAME}") + SET(PKG_CONFIG_DESCRIPTION "${PROJECT_DESCRIPTION}") + SET(PKG_CONFIG_URL "${PROJECT_URL}") + SET(PKG_CONFIG_VERSION "${PROJECT_VERSION}") + SET(PKG_CONFIG_REQUIRES "") + SET(PKG_CONFIG_CONFLICTS "") + SET(PKG_CONFIG_LIBS "${LIBDIR_KW}${CMAKE_INSTALL_PREFIX}/lib") + SET(PKG_CONFIG_LIBS_PRIVATE "") + SET(PKG_CONFIG_CFLAGS "-I${CMAKE_INSTALL_PREFIX}/include") + + SET(PKG_CONFIG_EXTRA "") + + # Where to install the pkg-config file? + SET(PKG_CONFIG_DIR "${PKG_CONFIG_LIBDIR}/pkgconfig") + + # Watch variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES + PKG_CONFIG_FOUND + PKG_CONFIG_EXECUTABLE + PKG_CONFIG_PREFIX + PKG_CONFIG_EXEC_PREFIX + PKG_CONFIG_LIBDIR + PKG_CONFIG_INCLUDEDIR + PKG_CONFIG_DATAROOTDIR + PKG_CONFIG_PKGDATAROOTDIR + PKG_CONFIG_DOCDIR + PKG_CONFIG_DOXYGENDOCDIR + PKG_CONFIG_PROJECT_NAME + PKG_CONFIG_DESCRIPTION + PKG_CONFIG_URL + PKG_CONFIG_VERSION + PKG_CONFIG_REQUIRES + PKG_CONFIG_CONFLICTS + PKG_CONFIG_LIBS + PKG_CONFIG_LIBS_PRIVATE + PKG_CONFIG_CFLAGS + PKG_CONFIG_EXTRA + ) + + # Install it. + INSTALL( + FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc" + DESTINATION lib/pkgconfig + PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE) +ENDMACRO(_SETUP_PROJECT_PKG_CONFIG) + + +# _SETUP_PROJECT_PKG_CONFIG_FINALIZE +# ---------------------------------- +# +# Post-processing of the pkg-config step. +# +# The pkg-config file has to be generated at the end to allow end-user +# defined variables replacement. +# +MACRO(_SETUP_PROJECT_PKG_CONFIG_FINALIZE) + # Generate the pkg-config file. + CONFIGURE_FILE( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/pkg-config.pc.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.pc" + ) +ENDMACRO(_SETUP_PROJECT_PKG_CONFIG_FINALIZE) + + +# ADD_DEPENDENCY(PREFIX P_REQUIRED PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Fail if the package cannot +# be found. +# +# P_REQUIRED : if set to 1 the package is required, otherwise it consider +# as optional. +# WARNING for optional package: +# if the package is detected its compile +# and linking options are still put in the required fields +# of the generated pc file. Indeed from the binary viewpoint +# the package becomes required. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_DEPENDENCY P_REQUIRED PKG_CONFIG_STRING) + # Retrieve the left part of the equation to get package name. + STRING(REGEX MATCH "[^<>= ]+" LIBRARY_NAME "${PKG_CONFIG_STRING}") + # And transform it into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${LIBRARY_NAME}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Force redetection each time CMake is launched. + # Rationale: these values are *NEVER* manually set, so information is never + # lost by overriding them. Moreover, changes in the pkg-config files are + # not seen as long as the cache is not destroyed, even if the .pc file + # is changed. This is a BAD behavior. + SET(${PREFIX}_FOUND 0) + + # Search for the package. + IF(${P_REQUIRED}) + MESSAGE(STATUS "${PKG_CONFIG_STRING} is required.") + PKG_CHECK_MODULES("${PREFIX}" REQUIRED "${PKG_CONFIG_STRING}") + ELSE(${P_REQUIRED}) + MESSAGE(STATUS "${PKG_CONFIG_STRING} is optional.") + PKG_CHECK_MODULES("${PREFIX}" "${PKG_CONFIG_STRING}") + ENDIF(${P_REQUIRED}) + + # Watch variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES + ${PREFIX}_FOUND + ${PREFIX}_LIBRARIES + ${PREFIX}_LIBRARY_DIRS + ${PREFIX}_LDFLAGS + ${PREFIX}_LDFLAGS_OTHER + ${PREFIX}_INCLUDE_DIRS + ${PREFIX}_CFLAGS + ${PREFIX}_CFLAGS_OTHER + ${PREFIX} + ${PREFIX}_STATIC + ${PREFIX}_VERSION + ${PREFIX}_PREFIX + ${PREFIX}_INCLUDEDIR + ${PREFIX}_LIBDIR + ) + + # Get the values of additional variables. + FOREACH(VARIABLE ${PKG_CONFIG_ADDITIONAL_VARIABLES}) + # Upper-case version of the variable for CMake variable generation. + STRING(TOUPPER "${VARIABLE}" "VARIABLE_UC") + EXEC_PROGRAM( + "${PKG_CONFIG_EXECUTABLE}" ARGS + "--variable=${VARIABLE}" "${LIBRARY_NAME}" + OUTPUT_VARIABLE "${PREFIX}_${VARIABLE_UC}") + + # Watch additional variables. + LIST(APPEND LOGGING_WATCHED_VARIABLES ${PREFIX}_${VARIABLE_UC}) + ENDFOREACH(VARIABLE) + + #FIXME: spaces are replaced by semi-colon by mistakes, revert the change. + #I cannot see why CMake is doing that... + STRING(REPLACE ";" " " PKG_CONFIG_STRING "${PKG_CONFIG_STRING}") + + # Add the package to the dependency list if found + IF(${${PREFIX}_FOUND}) + _ADD_TO_LIST(PKG_CONFIG_REQUIRES "${PKG_CONFIG_STRING}" ",") + ENDIF() + + # Add the package to the cmake dependency list + # if cpack has been included. + # This is likely to disappear when Ubuntu 8.04 will + # disappear. + IF(COMMAND ADD_CMAKE_DEPENDENCY) + ADD_CMAKE_DEPENDENCY(${PKG_CONFIG_STRING}) + ENDIF(COMMAND ADD_CMAKE_DEPENDENCY) + + IF(${${PREFIX}_FOUND}) + MESSAGE(STATUS + "Pkg-config module ${LIBRARY_NAME} v${${PREFIX}_VERSION}" + " has been detected with success.") + ENDIF() + +ENDMACRO(ADD_DEPENDENCY) + +# ADD_REQUIRED_DEPENDENCY(PREFIX PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Fail if the package cannot +# be found. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_REQUIRED_DEPENDENCY PKG_CONFIG_STRING) + ADD_DEPENDENCY(1 ${PKG_CONFIG_STRING}) +ENDMACRO(ADD_REQUIRED_DEPENDENCY) + +# ADD_OPTIONAL_DEPENDENCY(PREFIX PKGCONFIG_STRING) +# ------------------------------------------------ +# +# Check for a dependency using pkg-config. Quiet if the package cannot +# be found. +# +# PKG_CONFIG_STRING : string passed to pkg-config to check the version. +# Typically, this string looks like: +# ``my-package >= 0.5'' +# +MACRO(ADD_OPTIONAL_DEPENDENCY PKG_CONFIG_STRING) + ADD_DEPENDENCY(0 ${PKG_CONFIG_STRING}) +ENDMACRO(ADD_OPTIONAL_DEPENDENCY) + +# PKG_CONFIG_APPEND_LIBRARY_DIR +# ----------------------------- +# +# This macro adds library directories in a portable way +# into the CMake file. +MACRO(PKG_CONFIG_APPEND_LIBRARY_DIR DIRS) + FOREACH(DIR ${DIRS}) + IF(DIR) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIBDIR_KW}${DIR}") + ENDIF(DIR) + ENDFOREACH(DIR ${DIRS}) +ENDMACRO(PKG_CONFIG_APPEND_LIBRARY_DIR DIR) + + +# PKG_CONFIG_APPEND_CFLAGS +# ------------------------ +# +# This macro adds CFLAGS in a portable way into the pkg-config file. +# +MACRO(PKG_CONFIG_APPEND_CFLAGS FLAGS) + FOREACH(FLAG ${FLAGS}) + IF(FLAG) + SET(PKG_CONFIG_CFLAGS "${PKG_CONFIG_CFLAGS} ${FLAG}") + ENDIF(FLAG) + ENDFOREACH(FLAG ${FLAGS}) +ENDMACRO(PKG_CONFIG_APPEND_CFLAGS) + + +# PKG_CONFIG_APPEND_LIBS_RAW +# ---------------------------- +# +# This macro adds raw value in the "Libs:" into the pkg-config file. +# +# Exception for mac OS X: +# In addition to the classical static and dynamic libraries (handled like +# unix does), mac systems can link against frameworks. +# Frameworks are directories gathering headers, libraries, shared resources... +# +# The syntax used to link with a framework is particular, hence a filter is +# added to convert the absolute path to a framework (e.g. /Path/to/Sample.framework) +# into the correct flags (-F/Path/to/ -framework Sample). +# +MACRO(PKG_CONFIG_APPEND_LIBS_RAW LIBS) + FOREACH(LIB ${LIBS}) + IF(LIB) + IF( APPLE AND ${LIB} MATCHES .framework) + GET_FILENAME_COMPONENT(framework_PATH ${LIB} PATH) + GET_FILENAME_COMPONENT(framework_NAME ${LIB} NAME_WE) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} -F${framework_PATH} -framework ${framework_NAME}") + ELSE( APPLE AND ${LIB} MATCHES .framework) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIB}") + ENDIF( APPLE AND ${LIB} MATCHES .framework) + ENDIF(LIB) + ENDFOREACH(LIB ${LIBS}) + STRING(REPLACE "\n" "" PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS}") +ENDMACRO(PKG_CONFIG_APPEND_LIBS_RAW) + + +# PKG_CONFIG_APPEND_LIBS +# ---------------------- +# +# This macro adds libraries in a portable way into the pkg-config +# file. +# +# Library prefix and suffix is automatically added. +# +MACRO(PKG_CONFIG_APPEND_LIBS LIBS) + FOREACH(LIB ${LIBS}) + IF(LIB) + SET(PKG_CONFIG_LIBS "${PKG_CONFIG_LIBS} ${LIBINCL_KW}${LIB}${LIB_EXT}") + ENDIF(LIB) + ENDFOREACH(LIB ${LIBS}) +ENDMACRO(PKG_CONFIG_APPEND_LIBS) + + +# PKG_CONFIG_USE_DEPENDENCY(TARGET DEPENDENCY) +# -------------------------------------------- +# +# This macro changes the target properties to properly search for +# headers, libraries and link against the required shared libraries +# when using a dependency detected through pkg-config. +# +# I.e. PKG_CONFIG_USE_DEPENDENCY(my-binary my-package) +# +MACRO(PKG_CONFIG_USE_DEPENDENCY TARGET DEPENDENCY) + # Transform the dependency into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${DEPENDENCY}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Make sure we search for a previously detected package. + IF(NOT DEFINED ${PREFIX}_FOUND) + MESSAGE(FATAL_ERROR + "The package ${DEPENDENCY} has not been detected correctly.\n" + "Have you called ADD_REQUIRED_DEPENDENCY/ADD_OPTIONAL_DEPENDENCY?") + ENDIF() + IF(NOT ${PREFIX}_FOUND) + MESSAGE(FATAL_ERROR + "The package ${DEPENDENCY} has not been found.") + ENDIF() + + # Make sure we do not override previous flags. + GET_TARGET_PROPERTY(CFLAGS ${TARGET} COMPILE_FLAGS) + GET_TARGET_PROPERTY(LDFLAGS ${TARGET} LINK_FLAGS) + + # If there were no previous flags, get rid of the XYFLAGS-NOTFOUND + # in the variables. + IF(NOT CFLAGS) + SET(CFLAGS "") + ENDIF() + IF(NOT LDFLAGS) + SET(LDFLAGS "") + ENDIF() + + # Transform semi-colon seperated list in to space separated list. + FOREACH(FLAG ${${PREFIX}_CFLAGS}) + SET(CFLAGS "${CFLAGS} ${FLAG}") + ENDFOREACH() + + FOREACH(FLAG ${${PREFIX}_LDFLAGS}) + SET(LDFLAGS "${LDFLAGS} ${FLAG}") + ENDFOREACH() + + # Update the flags. + SET_TARGET_PROPERTIES(${TARGET} + PROPERTIES COMPILE_FLAGS "${CFLAGS}" LINK_FLAGS "${LDFLAGS}") + + IF(UNIX AND NOT APPLE) + TARGET_LINK_LIBRARIES(${TARGET} ${${PREFIX}_LDFLAGS}) + TARGET_LINK_LIBRARIES(${TARGET} ${${PREFIX}_LDFLAGS_OTHER}) + ENDIF(UNIX AND NOT APPLE) + + # Include/libraries paths seems to be filtered on Linux, add paths + # again. + INCLUDE_DIRECTORIES(${${PREFIX}_INCLUDE_DIRS}) + LINK_DIRECTORIES(${${PREFIX}_LIBRARY_DIRS}) +ENDMACRO(PKG_CONFIG_USE_DEPENDENCY TARGET DEPENDENCY) + diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake new file mode 100644 index 0000000..2a7c358 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/ros.cmake @@ -0,0 +1,92 @@ +# This file was taken from: +# https://github.com/jrl-umi3218/jrl-cmakemodules/blob/master/ros.cmake + +MACRO(ADD_ROSPACK_DEPENDENCY PKG) + IF(PKG STREQUAL "") + MESSAGE(FATAL_ERROR "ADD_ROS_DEPENDENCY invalid call.") + ENDIF() + + # Transform package name into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${PKG}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + SET(${PREFIX}_FOUND 0) + + FIND_PROGRAM(ROSPACK rospack) + IF(NOT ROSPACK) + MESSAGE(FATAL_ERROR "failed to find the rospack binary. Is ROS installed?") + ENDIF() + + MESSAGE(STATUS "Looking for ${PKG} using rospack...") + EXEC_PROGRAM("${ROSPACK} find ${PKG}" OUTPUT_VARIABLE ${PKG}_ROS_PREFIX) + IF(NOT ${PKG}_ROS_PREFIX) + MESSAGE(FATAL_ERROR "Failed to detect ${PKG}.") + ENDIF() + + SET(${PREFIX}_FOUND 1) + EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=cflags -q ${PKG}" + OUTPUT_VARIABLE "${PREFIX}_CFLAGS") + EXEC_PROGRAM("${ROSPACK} export --lang=cpp --attrib=lflags -q ${PKG}" + OUTPUT_VARIABLE "${PREFIX}_LIBS") + + # Add flags to package pkg-config file. + PKG_CONFIG_APPEND_CFLAGS (${${PREFIX}_CFLAGS}) + PKG_CONFIG_APPEND_LIBS_RAW (${${PREFIX}_LIBS}) +ENDMACRO() + +MACRO(ROSPACK_USE_DEPENDENCY TARGET PKG) + IF(PKG STREQUAL "") + MESSAGE(FATAL_ERROR "ADD_ROS_DEPENDENCY invalid call.") + ENDIF() + + # Transform package name into a valid variable prefix. + # 1. replace invalid characters into underscores. + STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" PREFIX "${PKG}") + # 2. make it uppercase. + STRING(TOUPPER "${PREFIX}" "PREFIX") + + # Make sure we do not override previous flags. + GET_TARGET_PROPERTY(CFLAGS "${TARGET}" COMPILE_FLAGS) + GET_TARGET_PROPERTY(LDFLAGS "${TARGET}" LINK_FLAGS) + + # If there were no previous flags, get rid of the XYFLAGS-NOTFOUND + # in the variables. + IF(NOT CFLAGS) + SET(CFLAGS "") + ENDIF() + IF(NOT LDFLAGS) + SET(LDFLAGS "") + ENDIF() + + # Transform semi-colon seperated list in to space separated list. + FOREACH(FLAG ${${PREFIX}_CFLAGS}) + SET(CFLAGS "${CFLAGS} ${FLAG}") + ENDFOREACH() + + FOREACH(FLAG ${${PREFIX}_LDFLAGS}) + SET(LDFLAGS "${LDFLAGS} ${FLAG}") + ENDFOREACH() + + # Filter out end of line in new flags. + STRING(REPLACE "\n" "" ${PREFIX}_CFLAGS "${${PREFIX}_CFLAGS}") + STRING(REPLACE "\n" "" ${PREFIX}_LIBS "${${PREFIX}_LIBS}") + + # Append new flags. + SET(CFLAGS "${CFLAGS} ${${PREFIX}_CFLAGS}") + SET(LDFLAGS "${LDFLAGS} ${${PREFIX}_LIBS}") + + # MESSAGE (STATUS "Linkerflags for ${TARGET}: ${LDFLAGS}") + + # Explicitly link against the shared object file + EXEC_PROGRAM("${ROSPACK} export find ${PKG}" + OUTPUT_VARIABLE "${PKG_FULL_PATH}") + + # SET (LDFLAGS "${LDFLAGS} ${PKG_FULL_PATH}/lib/lib${PKG}.so") + + # Update the flags. + SET_TARGET_PROPERTIES(${TARGET} + PROPERTIES COMPILE_FLAGS "${CFLAGS}" LINK_FLAGS "${LDFLAGS}") +ENDMACRO() + diff --git a/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake b/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake new file mode 100644 index 0000000..c8dbbf5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMake/shared-library.cmake @@ -0,0 +1,28 @@ +# Copyright (C) 2010 Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +# Shared library related constants +# (used for pkg-config file generation). +# FIXME: can't we get these information from CMake directly? +IF(WIN32) + SET(LIBDIR_KW "/LIBPATH:") + SET(LIBINCL_KW "") + SET(LIB_EXT ".lib") +ELSEIF(UNIX) + SET(LIBDIR_KW "-L") + SET(LIBINCL_KW "-l") + SET(LIB_EXT "") +ENDIF(WIN32) + diff --git a/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt b/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt new file mode 100644 index 0000000..9ec3b49 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/CMakeLists.txt @@ -0,0 +1,114 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMake ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX +) + +INCLUDE_DIRECTORIES ( + ${CMAKE_CURRENT_BINARY_DIR}/include/rbdl + ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/ +) + +SET ( URDFREADER_SOURCES + urdfreader.cc + ) + +IF (DEFINED ENV{ROS_ROOT}) + MESSAGE (STATUS "ROS found: $ENV{ROS_ROOT}") + find_package(catkin REQUIRED COMPONENTS urdf) + OPTION (RBDL_USE_ROS_URDF_LIBRARY "Use the URDF library provided by ROS" ON) +ELSE () + SET (RBDL_USE_ROS_URDF_LIBRARY FALSE) +ENDIF () + +IF (RBDL_USE_ROS_URDF_LIBRARY) + # find_package(Boost REQUIRED COMPONENTS system) + SET (URDFREADER_DEPENDENCIES + rbdl + ${urdf_LIBRARIES} + # ${Boost_SYSTEM_LIBRARY} + ) +ELSE() + SET (URDFREADER_SOURCES + ${URDFREADER_SOURCES} + thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp + thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp + thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h + thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h + thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h + thirdparty/tinyxml/tinystr.cpp + thirdparty/tinyxml/tinyxml.cpp + thirdparty/tinyxml/tinyxmlerror.cpp + thirdparty/tinyxml/tinyxmlparser.cpp + thirdparty/urdf/boost_replacement/lexical_cast.h + thirdparty/urdf/boost_replacement/shared_ptr.h + thirdparty/urdf/boost_replacement/printf_console.cpp + thirdparty/urdf/boost_replacement/printf_console.h + thirdparty/urdf/boost_replacement/string_split.cpp + thirdparty/urdf/boost_replacement/string_split.h + ) + SET (URDFREADER_DEPENDENCIES + rbdl + ) +ENDIF() + +ADD_EXECUTABLE (rbdl_urdfreader_util rbdl_urdfreader_util.cc) + +IF (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${URDFREADER_SOURCES} ) + + IF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES PREFIX "lib") + ENDIF (NOT WIN32) + SET_TARGET_PROPERTIES ( rbdl_urdfreader-static PROPERTIES OUTPUT_NAME "rbdl_urdfreader") + + TARGET_LINK_LIBRARIES (rbdl_urdfreader-static + rbdl-static + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader_util + rbdl_urdfreader-static + rbdl-static + ) + + INSTALL (TARGETS rbdl_urdfreader-static rbdl_urdfreader_util + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_BINDIR} + ) +ELSE (RBDL_BUILD_STATIC) + ADD_LIBRARY ( rbdl_urdfreader SHARED ${URDFREADER_SOURCES} ) + SET_TARGET_PROPERTIES ( rbdl_urdfreader PROPERTIES + VERSION ${RBDL_VERSION} + SOVERSION ${RBDL_SO_VERSION} + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader + ${URDFREADER_DEPENDENCIES} + ) + + TARGET_LINK_LIBRARIES (rbdl_urdfreader_util + rbdl_urdfreader + ) + + INSTALL (TARGETS rbdl_urdfreader rbdl_urdfreader_util + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) +ENDIF (RBDL_BUILD_STATIC) + +FILE ( GLOB headers + "${CMAKE_CURRENT_SOURCE_DIR}/*.h" + ) + +INSTALL ( FILES ${headers} + DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/rbdl/addons/urdfreader + ) diff --git a/3rdparty/rbdl/addons/urdfreader/README.md b/3rdparty/rbdl/addons/urdfreader/README.md new file mode 100644 index 0000000..eb8f4d8 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/README.md @@ -0,0 +1,50 @@ +urdfreader - load models from (URDF Unified Robot Description Format) files +Copyright (c) 2012-2018 Martin Felis + +Requirements +============ + +This addon depends on urdfdom to load and access the model data in the URDF +files. + +See https://github.com/ros/urdfdom for more details on how to +install urdfdom. + +Warning +======= + +This code is not properly tested as I do not have a proper urdf robot +model. If anyone has one and also some reference values that should come +out for the dynamics computations, please let me know. + +Licensing +========= + +This code is published under the zlib license, however some parts of the +CMake scripts are taken from other projects and are licensed under +different terms. + +Full license text: + +------- +urdfreader - load models from URDF (Unified Robot Description Format) files +Copyright (c) 2012-2018 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. diff --git a/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc b/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc new file mode 100644 index 0000000..3b3098f --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/rbdl_urdfreader_util.cc @@ -0,0 +1,110 @@ +#include +#include + +#include "urdfreader.h" + +#include +#include + +using namespace std; + +bool verbose = false; +bool floatbase = false; +string filename = ""; + +using namespace std; + +using namespace RigidBodyDynamics::Math; + +void usage (const char* argv_0) { + cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] " << endl; + cerr << " -v | --verbose enable additional output" << endl; + cerr << " -d | --dof-overview print an overview of the degress of freedom" << endl; + cerr << " -f | --floatbase set the first mobile body as floating base" << endl; + cerr << " -m | --model-hierarchy print the hierarchy of the model" << endl; + cerr << " -o | --body-origins print the origins of all bodies that have names" << endl; + cerr << " -c | --center_of_mass print center of mass for bodies and full model" << endl; + cerr << " -h | --help print this help" << endl; + exit (1); +} + +int main (int argc, char *argv[]) { + if (argc < 2) { + cerr << "Error: not enough arguments!" << endl; + usage(argv[0]); + } + + bool verbose = false; + bool dof_overview = false; + bool model_hierarchy = false; + bool body_origins = false; + bool center_of_mass = false; + + string filename = argv[1]; + + for (int i = 1; i < argc; i++) { + if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose") + verbose = true; + else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview") + dof_overview = true; + else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy") + model_hierarchy = true; + else if (string(argv[i]) == "-f" || string (argv[i]) == "--floatbase") + floatbase = true; + else if (string(argv[i]) == "-o" || string (argv[i]) == "--body-origins") + body_origins = true; + else if (string(argv[i]) == "-c" || string (argv[i]) == "--center-of-mass") + center_of_mass = true; + else if (string(argv[i]) == "-h" || string (argv[i]) == "--help") + usage(argv[0]); + else + filename = argv[i]; + } + + RigidBodyDynamics::Model model; + + if (!RigidBodyDynamics::Addons::URDFReadFromFile(filename.c_str(), &model, floatbase, verbose)) { + cerr << "Loading of urdf model failed!" << endl; + return -1; + } + + cout << "Model loading successful!" << endl; + + if (dof_overview) { + cout << "Degree of freedom overview:" << endl; + cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model); + } + + if (model_hierarchy) { + cout << "Model Hierarchy:" << endl; + cout << RigidBodyDynamics::Utils::GetModelHierarchy (model); + } + + if (body_origins) { + cout << "Body Origins:" << endl; + cout << RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(model); + } + + if (center_of_mass) { + VectorNd q_zero (VectorNd::Zero (model.q_size)); + VectorNd qdot_zero (VectorNd::Zero (model.qdot_size)); + RigidBodyDynamics::UpdateKinematics (model, q_zero, qdot_zero, qdot_zero); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.mBodies[i].mIsVirtual) + continue; + + SpatialRigidBodyInertia rbi_base = model.X_base[i].apply(model.I[i]); + Vector3d body_com = rbi_base.h / rbi_base.m; + cout << setw(12) << model.GetBodyName (i) << ": " << setw(10) << body_com.transpose() << endl; + } + + Vector3d model_com; + double mass; + RigidBodyDynamics::Utils::CalcCenterOfMass (model, q_zero, qdot_zero, NULL, mass, model_com); + cout << setw(14) << "Model COM: " << setw(10) << model_com.transpose() << endl; + cout << setw(14) << "Model mass: " << mass << endl; + } + + return 0; +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md b/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md new file mode 100644 index 0000000..cd7a0d9 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/README.md @@ -0,0 +1,5 @@ +Code from the urdf directory was taken from the Bullet 3 source code +repository https://github.com/bulletphysics/bullet3 from the directory +examples/ThirdPartyLibs/urdf. + +The tinyxml code was obtained from http://sourceforge.net/projects/tinyxml/ diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt new file mode 100644 index 0000000..15b51bd --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/changes.txt @@ -0,0 +1,299 @@ +Changes in version 1.0.1: +- Fixed comment tags which were outputing as ' include. Thanks + to Steve Lhomme for that. + +Changes in version 2.0.0 BETA +- Made the ToXXX() casts safe if 'this' is null. + When "LoadFile" is called with a filename, the value will correctly get set. + Thanks to Brian Yoder. +- Fixed bug where isalpha() and isalnum() would get called with a negative value for + high ascii numbers. Thanks to Alesky Aksenov. +- Fixed some errors codes that were not getting set. +- Made methods "const" that were not. +- Added a switch to enable or disable the ignoring of white space. ( TiXmlDocument::SetIgnoreWhiteSpace() ) +- Greater standardization and code re-use in the parser. +- Added a stream out operator. +- Added a stream in operator. +- Entity support, of predefined entites. &#x entities are untouched by input or output. +- Improved text out formatting. +- Fixed ReplaceChild bug, thanks to Tao Chen. + +Changes in version 2.0.1 +- Fixed hanging on loading a 0 length file. Thanks to Jeff Scozzafava. +- Fixed crashing on InsertBeforeChild and InsertAfterChild. Also possibility of bad links being + created by same function. Thanks to Frank De prins. +- Added missing licence text. Thanks to Lars Willemsens. +- Added include, at the suggestion of Steve Walters. + +Changes in version 2.1.0 +- Yves Berquin brings us the STL switch. The forum on SourceForge, and various emails to + me, have long debated all out STL vs. no STL at all. And now you can have it both ways. + TinyXml will compile either way. + +Changes in version 2.1.1 +- Compilation warnings. + +Changes in version 2.1.2 +- Uneeded code is not compiled in the STL case. +- Changed headers so that STL can be turned on or off in tinyxml.h + +Changes in version 2.1.3 +- Fixed non-const reference in API; now uses a pointer. +- Copy constructor of TiXmlString not checking for assignment to self. +- Nimrod Cohen found a truly evil bug in the STL implementation that occurs + when a string is converted to a c_str and then assigned to self. Search for + STL_STRING_BUG for a full description. I'm asserting this is a Microsoft STL + bug, since &string and string.c_str() should never be the same. Nevertheless, + the code works around it. +- Urivan Saaib pointed out a compiler conflict, where the C headers define + the isblank macro, which was wiping out the TiXmlString::isblank() method. + The method was unused and has been removed. + +Changes in version 2.1.4 +- Reworked the entity code. Entities were not correctly surving round trip input and output. + Will now automatically create entities for high ascii in output. + +Changes in version 2.1.5 +- Bug fix by kylotan : infinite loop on some input (tinyxmlparser.cpp rev 1.27) +- Contributed by Ivica Aracic (bytelord) : 1 new VC++ project to compile versions as static libraries (tinyxml_lib.dsp), + and an example usage in xmltest.dsp + (Patch request ID 678605) +- A suggestion by Ronald Fenner Jr (dormlock) to add #include and for Apple's Project Builder + (Patch request ID 697642) +- A patch from ohommes that allows to parse correctly dots in element names and attribute names + (Patch request 602600 and kylotan 701728) +- A patch from hermitgeek ( James ) and wasteland for improper error reporting +- Reviewed by Lee, with the following changes: + - Got sick of fighting the STL/non-STL thing in the windows build. Broke + them out as seperate projects. + - I have too long not included the dsw. Added. + - TinyXmlText had a protected Print. Odd. + - Made LinkEndChild public, with docs and appropriate warnings. + - Updated the docs. + +2.2.0 +- Fixed an uninitialized pointer in the TiXmlAttributes +- Fixed STL compilation problem in MinGW (and gcc 3?) - thanks Brian Yoder for finding this one +- Fixed a syntax error in TiXmlDeclaration - thanks Brian Yoder +- Fletcher Dunn proposed and submitted new error handling that tracked the row and column. Lee + modified it to not have performance impact. +- General cleanup suggestions from Fletcher Dunn. +- In error handling, general errors will no longer clear the error state of specific ones. +- Fix error in documentation : comments starting with ">) has now + been fixed. + +2.5.2 +- Lieven, and others, pointed out a missing const-cast that upset the Open Watcom compiler. + Should now be fixed. +- ErrorRow and ErrorCol should have been const, and weren't. Fixed thanks to Dmitry Polutov. + +2.5.3 +- zloe_zlo identified a missing string specialization for QueryValueAttribute() [ 1695429 ]. Worked + on this bug, but not sure how to fix it in a safe, cross-compiler way. +- increased warning level to 4 and turned on detect 64 bit portability issues for VC2005. + May address [ 1677737 ] VS2005: /Wp64 warnings +- grosheck identified several problems with the Document copy. Many thanks for [ 1660367 ] +- Nice catch, and suggested fix, be Gilad Novik on the Printer dropping entities. + "[ 1600650 ] Bug when printing xml text" is now fixed. +- A subtle fix from Nicos Gollan in the tinystring initializer: + [ 1581449 ] Fix initialiser of TiXmlString::nullrep_ +- Great catch, although there isn't a submitter for the bug. [ 1475201 ] TinyXML parses entities in comments. + Comments should not, in fact, parse entities. Fixed the code path and added tests. +- We were not catching all the returns from ftell. Thanks to Bernard for catching that. + +2.5.4 +- A TiXMLDocument can't be a sub-node. Block this from happening in the 'replace'. Thanks Noam. +- [ 1714831 ] TiXmlBase::location is not copied by copy-ctors, fix reported and suggested by Nicola Civran. +- Fixed possible memory overrun in the comment reading code - thanks gcarlton77 + +2.5.5 +- Alex van der Wal spotted incorrect types (lf) being used in print and scan. robertnestor pointed out some problems with the simple solution. Types updated. +- Johannes Hillert pointed out some bug typos. +- Christian Mueller identified inconsistent error handling with Attributes. +- olivier barthelemy also reported a problem with double truncation, also related to the %lf issue. +- zaelsius came up with a great (and simple) suggestion to fix QueryValueAttribute truncating strings. +- added some null pointer checks suggested by hansenk +- Sami Väisänen found a (rare) buffer overrun that could occur in parsing. +- vi tri filed a bug that led to a refactoring of the attribute setting mess (as well as adding a missing SetDoubleAttribute() ) +- removed TIXML_ERROR_OUT_OF_MEMORY. TinyXML does not systematically address OOO, and the notion it does is misleading. +- vanneto, keithmarshall, others all reported the warning from IsWhiteSpace() usage. Cleaned this up - many thanks to everyone who reported this one. +- tibur found a bug in end tag parsing + + +2.6.2 +- Switched over to VC 2010 +- Fixed up all the build issues arising from that. (Lots of latent build problems.) +- Removed the old, now unmaintained and likely not working, build files. +- Fixed some static analysis issues reported by orbitcowboy from cppcheck. +- Bayard 95 sent in analysis from a different analyzer - fixes applied from that as well. +- Tim Kosse sent a patch fixing an infinite loop. +- Ma Anguo identified a doc issue. +- Eddie Cohen identified a missing qualifier resulting in a compilation error on some systems. +- Fixed a line ending bug. (What year is this? Can we all agree on a format for text files? Please? ...oh well.) + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt new file mode 100644 index 0000000..89d9e8d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/readme.txt @@ -0,0 +1,530 @@ +/** @mainpage + +

TinyXML

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

What it does.

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

What it doesn't do.

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

Tutorials.

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

Code Status.

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

Related Projects

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

Features

+ +

Using STL

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

UTF-8

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

Entities

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

Printing

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

Streams

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

White space

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

Handles

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

Row and Column tracking

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

Using and Installing

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

Windows project file for VC6

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

Makefile

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

To Use in an Application:

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

How TinyXML works.

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

Documentation

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

License

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

References

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

Contributors, Contacts, and a Brief History

+ +Thanks very much to everyone who sends suggestions, bugs, ideas, and +encouragement. It all helps, and makes this project fun. A special thanks +to the contributors on the web pages that keep it lively. + +So many people have sent in bugs and ideas, that rather than list here +we try to give credit due in the "changes.txt" file. + +TinyXML was originally written by Lee Thomason. (Often the "I" still +in the documentation.) Lee reviews changes and releases new versions, +with the help of Yves Berquin, Andrew Ellerton, and the tinyXml community. + +We appreciate your suggestions, and would love to know if you +use TinyXML. Hopefully you will enjoy it and find it useful. +Please post questions, comments, file bugs, or contact us at: + +www.sourceforge.net/projects/tinyxml + +Lee Thomason, Yves Berquin, Andrew Ellerton +*/ diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp new file mode 100644 index 0000000..0665768 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.cpp @@ -0,0 +1,111 @@ +/* +www.sourceforge.net/projects/tinyxml + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TIXML_USE_STL + +#include "tinystr.h" + +// Error value for find primitive +const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1); + + +// Null rep. +TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; + + +void TiXmlString::reserve (size_type cap) +{ + if (cap > capacity()) + { + TiXmlString tmp; + tmp.init(length(), cap); + memcpy(tmp.start(), data(), length()); + swap(tmp); + } +} + + +TiXmlString& TiXmlString::assign(const char* str, size_type len) +{ + size_type cap = capacity(); + if (len > cap || cap > 3*(len + 8)) + { + TiXmlString tmp; + tmp.init(len); + memcpy(tmp.start(), str, len); + swap(tmp); + } + else + { + memmove(start(), str, len); + set_size(len); + } + return *this; +} + + +TiXmlString& TiXmlString::append(const char* str, size_type len) +{ + size_type newsize = length() + len; + if (newsize > capacity()) + { + reserve (newsize + capacity()); + } + memmove(finish(), str, len); + set_size(newsize); + return *this; +} + + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) +{ + TiXmlString tmp; + tmp.reserve(a.length() + b.length()); + tmp += a; + tmp += b; + return tmp; +} + +TiXmlString operator + (const TiXmlString & a, const char* b) +{ + TiXmlString tmp; + TiXmlString::size_type b_len = static_cast( strlen(b) ); + tmp.reserve(a.length() + b_len); + tmp += a; + tmp.append(b, b_len); + return tmp; +} + +TiXmlString operator + (const char* a, const TiXmlString & b) +{ + TiXmlString tmp; + TiXmlString::size_type a_len = static_cast( strlen(a) ); + tmp.reserve(a_len + b.length()); + tmp.append(a, a_len); + tmp += b; + return tmp; +} + + +#endif // TIXML_USE_STL diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h new file mode 100644 index 0000000..89cca33 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinystr.h @@ -0,0 +1,305 @@ +/* +www.sourceforge.net/projects/tinyxml + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TIXML_USE_STL + +#ifndef TIXML_STRING_INCLUDED +#define TIXML_STRING_INCLUDED + +#include +#include + +/* The support for explicit isn't that universal, and it isn't really + required - it is used to check that the TiXmlString class isn't incorrectly + used. Be nice to old compilers and macro it here: +*/ +#if defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + #define TIXML_EXPLICIT explicit +#elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + #define TIXML_EXPLICIT explicit +#else + #define TIXML_EXPLICIT +#endif + + +/* + TiXmlString is an emulation of a subset of the std::string template. + Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. + Only the member functions relevant to the TinyXML project have been implemented. + The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase + a string and there's no more room, we allocate a buffer twice as big as we need. +*/ +class TiXmlString +{ + public : + // The size type used + typedef size_t size_type; + + // Error value for find primitive + static const size_type npos; // = -1; + + + // TiXmlString empty constructor + TiXmlString () : rep_(&nullrep_) + { + } + + // TiXmlString copy constructor + TiXmlString ( const TiXmlString & copy) : rep_(0) + { + init(copy.length()); + memcpy(start(), copy.data(), length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) + { + init( static_cast( strlen(copy) )); + memcpy(start(), copy, length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) + { + init(len); + memcpy(start(), str, len); + } + + // TiXmlString destructor + ~TiXmlString () + { + quit(); + } + + TiXmlString& operator = (const char * copy) + { + return assign( copy, (size_type)strlen(copy)); + } + + TiXmlString& operator = (const TiXmlString & copy) + { + return assign(copy.start(), copy.length()); + } + + + // += operator. Maps to append + TiXmlString& operator += (const char * suffix) + { + return append(suffix, static_cast( strlen(suffix) )); + } + + // += operator. Maps to append + TiXmlString& operator += (char single) + { + return append(&single, 1); + } + + // += operator. Maps to append + TiXmlString& operator += (const TiXmlString & suffix) + { + return append(suffix.data(), suffix.length()); + } + + + // Convert a TiXmlString into a null-terminated char * + const char * c_str () const { return rep_->str; } + + // Convert a TiXmlString into a char * (need not be null terminated). + const char * data () const { return rep_->str; } + + // Return the length of a TiXmlString + size_type length () const { return rep_->size; } + + // Alias for length() + size_type size () const { return rep_->size; } + + // Checks if a TiXmlString is empty + bool empty () const { return rep_->size == 0; } + + // Return capacity of string + size_type capacity () const { return rep_->capacity; } + + + // single char extraction + const char& at (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // [] operator + char& operator [] (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // find a char in a string. Return TiXmlString::npos if not found + size_type find (char lookup) const + { + return find(lookup, 0); + } + + // find a char in a string from an offset. Return TiXmlString::npos if not found + size_type find (char tofind, size_type offset) const + { + if (offset >= length()) return npos; + + for (const char* p = c_str() + offset; *p != '\0'; ++p) + { + if (*p == tofind) return static_cast< size_type >( p - c_str() ); + } + return npos; + } + + void clear () + { + //Lee: + //The original was just too strange, though correct: + // TiXmlString().swap(*this); + //Instead use the quit & re-init: + quit(); + init(0,0); + } + + /* Function to reserve a big amount of data when we know we'll need it. Be aware that this + function DOES NOT clear the content of the TiXmlString if any exists. + */ + void reserve (size_type cap); + + TiXmlString& assign (const char* str, size_type len); + + TiXmlString& append (const char* str, size_type len); + + void swap (TiXmlString& other) + { + Rep* r = rep_; + rep_ = other.rep_; + other.rep_ = r; + } + + private: + + void init(size_type sz) { init(sz, sz); } + void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } + char* start() const { return rep_->str; } + char* finish() const { return rep_->str + rep_->size; } + + struct Rep + { + size_type size, capacity; + char str[1]; + }; + + void init(size_type sz, size_type cap) + { + if (cap) + { + // Lee: the original form: + // rep_ = static_cast(operator new(sizeof(Rep) + cap)); + // doesn't work in some cases of new being overloaded. Switching + // to the normal allocation, although use an 'int' for systems + // that are overly picky about structure alignment. + const size_type bytesNeeded = sizeof(Rep) + cap; + const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); + rep_ = reinterpret_cast( new int[ intsNeeded ] ); + + rep_->str[ rep_->size = sz ] = '\0'; + rep_->capacity = cap; + } + else + { + rep_ = &nullrep_; + } + } + + void quit() + { + if (rep_ != &nullrep_) + { + // The rep_ is really an array of ints. (see the allocator, above). + // Cast it back before delete, so the compiler won't incorrectly call destructors. + delete [] ( reinterpret_cast( rep_ ) ); + } + } + + Rep * rep_; + static Rep nullrep_; + +} ; + + +inline bool operator == (const TiXmlString & a, const TiXmlString & b) +{ + return ( a.length() == b.length() ) // optimization on some platforms + && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare +} +inline bool operator < (const TiXmlString & a, const TiXmlString & b) +{ + return strcmp(a.c_str(), b.c_str()) < 0; +} + +inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } +inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } +inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } +inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } + +inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } +inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } +inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } +inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); +TiXmlString operator + (const TiXmlString & a, const char* b); +TiXmlString operator + (const char* a, const TiXmlString & b); + + +/* + TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. + Only the operators that we need for TinyXML have been developped. +*/ +class TiXmlOutStream : public TiXmlString +{ +public : + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const TiXmlString & in) + { + *this += in; + return *this; + } + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const char * in) + { + *this += in; + return *this; + } + +} ; + +#endif // TIXML_STRING_INCLUDED +#endif // TIXML_USE_STL diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp new file mode 100644 index 0000000..9c161df --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.cpp @@ -0,0 +1,1886 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include + +#ifdef TIXML_USE_STL +#include +#include +#endif + +#include "tinyxml.h" + +FILE* TiXmlFOpen( const char* filename, const char* mode ); + +bool TiXmlBase::condenseWhiteSpace = true; + +// Microsoft compiler security +FILE* TiXmlFOpen( const char* filename, const char* mode ) +{ + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + FILE* fp = 0; + errno_t err = fopen_s( &fp, filename, mode ); + if ( !err && fp ) + return fp; + return 0; + #else + return fopen( filename, mode ); + #endif +} + +void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) +{ + int i=0; + + while( i<(int)str.length() ) + { + unsigned char c = (unsigned char) str[i]; + + if ( c == '&' + && i < ( (int)str.length() - 2 ) + && str[i+1] == '#' + && str[i+2] == 'x' ) + { + // Hexadecimal character reference. + // Pass through unchanged. + // © -- copyright symbol, for example. + // + // The -1 is a bug fix from Rob Laveaux. It keeps + // an overflow from happening if there is no ';'. + // There are actually 2 ways to exit this loop - + // while fails (error case) and break (semicolon found). + // However, there is no mechanism (currently) for + // this function to return an error. + while ( i<(int)str.length()-1 ) + { + outString->append( str.c_str() + i, 1 ); + ++i; + if ( str[i] == ';' ) + break; + } + } + else if ( c == '&' ) + { + outString->append( entity[0].str, entity[0].strLength ); + ++i; + } + else if ( c == '<' ) + { + outString->append( entity[1].str, entity[1].strLength ); + ++i; + } + else if ( c == '>' ) + { + outString->append( entity[2].str, entity[2].strLength ); + ++i; + } + else if ( c == '\"' ) + { + outString->append( entity[3].str, entity[3].strLength ); + ++i; + } + else if ( c == '\'' ) + { + outString->append( entity[4].str, entity[4].strLength ); + ++i; + } + else if ( c < 32 ) + { + // Easy pass at non-alpha/numeric/symbol + // Below 32 is symbolic. + char buf[ 32 ]; + + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); + #else + sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); + #endif + + //*ME: warning C4267: convert 'size_t' to 'int' + //*ME: Int-Cast to make compiler happy ... + outString->append( buf, (int)strlen( buf ) ); + ++i; + } + else + { + //char realc = (char) c; + //outString->append( &realc, 1 ); + *outString += (char) c; // somewhat more efficient function call. + ++i; + } + } +} + + +TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() +{ + parent = 0; + type = _type; + firstChild = 0; + lastChild = 0; + prev = 0; + next = 0; +} + + +TiXmlNode::~TiXmlNode() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } +} + + +void TiXmlNode::CopyTo( TiXmlNode* target ) const +{ + target->SetValue (value.c_str() ); + target->userData = userData; + target->location = location; +} + + +void TiXmlNode::Clear() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } + + firstChild = 0; + lastChild = 0; +} + + +TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) +{ + assert( node->parent == 0 || node->parent == this ); + assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); + + if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + delete node; + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + node->parent = this; + + node->prev = lastChild; + node->next = 0; + + if ( lastChild ) + lastChild->next = node; + else + firstChild = node; // it was an empty list. + + lastChild = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) +{ + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + + return LinkEndChild( node ); +} + + +TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) +{ + if ( !beforeThis || beforeThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->next = beforeThis; + node->prev = beforeThis->prev; + if ( beforeThis->prev ) + { + beforeThis->prev->next = node; + } + else + { + assert( firstChild == beforeThis ); + firstChild = node; + } + beforeThis->prev = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) +{ + if ( !afterThis || afterThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->prev = afterThis; + node->next = afterThis->next; + if ( afterThis->next ) + { + afterThis->next->prev = node; + } + else + { + assert( lastChild == afterThis ); + lastChild = node; + } + afterThis->next = node; + return node; +} + + +TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) +{ + if ( !replaceThis ) + return 0; + + if ( replaceThis->parent != this ) + return 0; + + if ( withThis.ToDocument() ) { + // A document can never be a child. Thanks to Noam. + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = withThis.Clone(); + if ( !node ) + return 0; + + node->next = replaceThis->next; + node->prev = replaceThis->prev; + + if ( replaceThis->next ) + replaceThis->next->prev = node; + else + lastChild = node; + + if ( replaceThis->prev ) + replaceThis->prev->next = node; + else + firstChild = node; + + delete replaceThis; + node->parent = this; + return node; +} + + +bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) +{ + if ( !removeThis ) { + return false; + } + + if ( removeThis->parent != this ) + { + assert( 0 ); + return false; + } + + if ( removeThis->next ) + removeThis->next->prev = removeThis->prev; + else + lastChild = removeThis->prev; + + if ( removeThis->prev ) + removeThis->prev->next = removeThis->next; + else + firstChild = removeThis->next; + + delete removeThis; + return true; +} + +const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = firstChild; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = lastChild; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild(); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling(); + } +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild( val ); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling( val ); + } +} + + +const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = next; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = prev; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +void TiXmlElement::RemoveAttribute( const char * name ) +{ + #ifdef TIXML_USE_STL + TIXML_STRING str( name ); + TiXmlAttribute* node = attributeSet.Find( str ); + #else + TiXmlAttribute* node = attributeSet.Find( name ); + #endif + if ( node ) + { + attributeSet.Remove( node ); + delete node; + } +} + +const TiXmlElement* TiXmlNode::FirstChildElement() const +{ + const TiXmlNode* node; + + for ( node = FirstChild(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = FirstChild( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement() const +{ + const TiXmlNode* node; + + for ( node = NextSibling(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = NextSibling( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlDocument* TiXmlNode::GetDocument() const +{ + const TiXmlNode* node; + + for( node = this; node; node = node->parent ) + { + if ( node->ToDocument() ) + return node->ToDocument(); + } + return 0; +} + + +TiXmlElement::TiXmlElement (const char * _value) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} + + +#ifdef TIXML_USE_STL +TiXmlElement::TiXmlElement( const std::string& _value ) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} +#endif + + +TiXmlElement::TiXmlElement( const TiXmlElement& copy) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + copy.CopyTo( this ); +} + + +TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) +{ + ClearThis(); + base.CopyTo( this ); + return *this; +} + + +TiXmlElement::~TiXmlElement() +{ + ClearThis(); +} + + +void TiXmlElement::ClearThis() +{ + Clear(); + while( attributeSet.First() ) + { + TiXmlAttribute* node = attributeSet.First(); + attributeSet.Remove( node ); + delete node; + } +} + + +const char* TiXmlElement::Attribute( const char* name ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( node ) + return node->Value(); + return 0; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( attrib ) + return &attrib->ValueStr(); + return 0; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} +#endif + + +int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} + + +int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int ival = 0; + int result = node->QueryIntValue( &ival ); + *value = (unsigned)ival; + return result; +} + + +int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int result = TIXML_WRONG_TYPE; + if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = true; + result = TIXML_SUCCESS; + } + else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = false; + result = TIXML_SUCCESS; + } + return result; +} + + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} +#endif + + +int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} +#endif + + +void TiXmlElement::SetAttribute( const char * name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} +#endif + + +void TiXmlElement::SetDoubleAttribute( const char * name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} +#endif + + +void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); + if ( attrib ) { + attrib->SetValue( cvalue ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); + if ( attrib ) { + attrib->SetValue( _value ); + } +} +#endif + + +void TiXmlElement::Print( FILE* cfile, int depth ) const +{ + int i; + assert( cfile ); + for ( i=0; iNext() ) + { + fprintf( cfile, " " ); + attrib->Print( cfile, depth ); + } + + // There are 3 different formatting approaches: + // 1) An element without children is printed as a node + // 2) An element with only a text child is printed as text + // 3) An element with children is printed on multiple lines. + TiXmlNode* node; + if ( !firstChild ) + { + fprintf( cfile, " />" ); + } + else if ( firstChild == lastChild && firstChild->ToText() ) + { + fprintf( cfile, ">" ); + firstChild->Print( cfile, depth + 1 ); + fprintf( cfile, "", value.c_str() ); + } + else + { + fprintf( cfile, ">" ); + + for ( node = firstChild; node; node=node->NextSibling() ) + { + if ( !node->ToText() ) + { + fprintf( cfile, "\n" ); + } + node->Print( cfile, depth+1 ); + } + fprintf( cfile, "\n" ); + for( i=0; i", value.c_str() ); + } +} + + +void TiXmlElement::CopyTo( TiXmlElement* target ) const +{ + // superclass: + TiXmlNode::CopyTo( target ); + + // Element class: + // Clone the attributes, then clone the children. + const TiXmlAttribute* attribute = 0; + for( attribute = attributeSet.First(); + attribute; + attribute = attribute->Next() ) + { + target->SetAttribute( attribute->Name(), attribute->Value() ); + } + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + +bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this, attributeSet.First() ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +TiXmlNode* TiXmlElement::Clone() const +{ + TiXmlElement* clone = new TiXmlElement( Value() ); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +const char* TiXmlElement::GetText() const +{ + const TiXmlNode* child = this->FirstChild(); + if ( child ) { + const TiXmlText* childText = child->ToText(); + if ( childText ) { + return childText->Value(); + } + } + return 0; +} + + +TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + ClearError(); +} + +TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} + + +#ifdef TIXML_USE_STL +TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} +#endif + + +TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) +{ + return LoadFile( Value(), encoding ); +} + + +bool TiXmlDocument::SaveFile() const +{ + return SaveFile( Value() ); +} + +bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) +{ + TIXML_STRING filename( _filename ); + value = filename; + + // reading in binary mode so that tinyxml can normalize the EOL + FILE* file = TiXmlFOpen( value.c_str (), "rb" ); + + if ( file ) + { + bool result = LoadFile( file, encoding ); + fclose( file ); + return result; + } + else + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } +} + +bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) +{ + if ( !file ) + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Delete the existing data: + Clear(); + location.Clear(); + + // Get the file size, so we can pre-allocate the string. HUGE speed impact. + long length = 0; + fseek( file, 0, SEEK_END ); + length = ftell( file ); + fseek( file, 0, SEEK_SET ); + + // Strange case, but good to handle up front. + if ( length <= 0 ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Subtle bug here. TinyXml did use fgets. But from the XML spec: + // 2.11 End-of-Line Handling + // + // + // ...the XML processor MUST behave as if it normalized all line breaks in external + // parsed entities (including the document entity) on input, before parsing, by translating + // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to + // a single #xA character. + // + // + // It is not clear fgets does that, and certainly isn't clear it works cross platform. + // Generally, you expect fgets to translate from the convention of the OS to the c/unix + // convention, and not work generally. + + /* + while( fgets( buf, sizeof(buf), file ) ) + { + data += buf; + } + */ + + char* buf = new char[ length+1 ]; + buf[0] = 0; + + if ( fread( buf, length, 1, file ) != 1 ) { + delete [] buf; + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Process the buffer in place to normalize new lines. (See comment above.) + // Copies from the 'p' to 'q' pointer, where p can advance faster if + // a newline-carriage return is hit. + // + // Wikipedia: + // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or + // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... + // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others + // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS + // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 + + const char* p = buf; // the read head + char* q = buf; // the write head + const char CR = 0x0d; + const char LF = 0x0a; + + buf[length] = 0; + while( *p ) { + assert( p < (buf+length) ); + assert( q <= (buf+length) ); + assert( q <= p ); + + if ( *p == CR ) { + *q++ = LF; + p++; + if ( *p == LF ) { // check for CR+LF (and skip LF) + p++; + } + } + else { + *q++ = *p++; + } + } + assert( q <= (buf+length) ); + *q = 0; + + Parse( buf, 0, encoding ); + + delete [] buf; + return !Error(); +} + + +bool TiXmlDocument::SaveFile( const char * filename ) const +{ + // The old c stuff lives on... + FILE* fp = TiXmlFOpen( filename, "w" ); + if ( fp ) + { + bool result = SaveFile( fp ); + fclose( fp ); + return result; + } + return false; +} + + +bool TiXmlDocument::SaveFile( FILE* fp ) const +{ + if ( useMicrosoftBOM ) + { + const unsigned char TIXML_UTF_LEAD_0 = 0xefU; + const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; + const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + + fputc( TIXML_UTF_LEAD_0, fp ); + fputc( TIXML_UTF_LEAD_1, fp ); + fputc( TIXML_UTF_LEAD_2, fp ); + } + Print( fp, 0 ); + return (ferror(fp) == 0); +} + + +void TiXmlDocument::CopyTo( TiXmlDocument* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->error = error; + target->errorId = errorId; + target->errorDesc = errorDesc; + target->tabsize = tabsize; + target->errorLocation = errorLocation; + target->useMicrosoftBOM = useMicrosoftBOM; + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + + +TiXmlNode* TiXmlDocument::Clone() const +{ + TiXmlDocument* clone = new TiXmlDocument(); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlDocument::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + node->Print( cfile, depth ); + fprintf( cfile, "\n" ); + } +} + + +bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +const TiXmlAttribute* TiXmlAttribute::Next() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} + +/* +TiXmlAttribute* TiXmlAttribute::Next() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} +*/ + +const TiXmlAttribute* TiXmlAttribute::Previous() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} + +/* +TiXmlAttribute* TiXmlAttribute::Previous() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} +*/ + +void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + TIXML_STRING n, v; + + EncodeString( name, &n ); + EncodeString( value, &v ); + + if (value.find ('\"') == TIXML_STRING::npos) { + if ( cfile ) { + fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; + } + } + else { + if ( cfile ) { + fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; + } + } +} + + +int TiXmlAttribute::QueryIntValue( int* ival ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +int TiXmlAttribute::QueryDoubleValue( double* dval ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +void TiXmlAttribute::SetIntValue( int _value ) +{ + char buf [64]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); + #else + sprintf (buf, "%d", _value); + #endif + SetValue (buf); +} + +void TiXmlAttribute::SetDoubleValue( double _value ) +{ + char buf [256]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); + #else + sprintf (buf, "%g", _value); + #endif + SetValue (buf); +} + +int TiXmlAttribute::IntValue() const +{ + return atoi (value.c_str ()); +} + +double TiXmlAttribute::DoubleValue() const +{ + return atof (value.c_str ()); +} + + +TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) +{ + Clear(); + base.CopyTo( this ); + return *this; +} + + +void TiXmlComment::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlComment::CopyTo( TiXmlComment* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlComment::Clone() const +{ + TiXmlComment* clone = new TiXmlComment(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlText::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + if ( cdata ) + { + int i; + fprintf( cfile, "\n" ); + for ( i=0; i\n", value.c_str() ); // unformatted output + } + else + { + TIXML_STRING buffer; + EncodeString( value, &buffer ); + fprintf( cfile, "%s", buffer.c_str() ); + } +} + + +void TiXmlText::CopyTo( TiXmlText* target ) const +{ + TiXmlNode::CopyTo( target ); + target->cdata = cdata; +} + + +bool TiXmlText::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlText::Clone() const +{ + TiXmlText* clone = 0; + clone = new TiXmlText( "" ); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlDeclaration::TiXmlDeclaration( const char * _version, + const char * _encoding, + const char * _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} + + +#ifdef TIXML_USE_STL +TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} +#endif + + +TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + copy.CopyTo( this ); +} + + +TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + if ( cfile ) fprintf( cfile, "" ); + if ( str ) (*str) += "?>"; +} + + +void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->version = version; + target->encoding = encoding; + target->standalone = standalone; +} + + +bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlDeclaration::Clone() const +{ + TiXmlDeclaration* clone = new TiXmlDeclaration(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlUnknown::Print( FILE* cfile, int depth ) const +{ + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlUnknown::Clone() const +{ + TiXmlUnknown* clone = new TiXmlUnknown(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlAttributeSet::TiXmlAttributeSet() +{ + sentinel.next = &sentinel; + sentinel.prev = &sentinel; +} + + +TiXmlAttributeSet::~TiXmlAttributeSet() +{ + assert( sentinel.next == &sentinel ); + assert( sentinel.prev == &sentinel ); +} + + +void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) +{ + #ifdef TIXML_USE_STL + assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. + #else + assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. + #endif + + addMe->next = &sentinel; + addMe->prev = sentinel.prev; + + sentinel.prev->next = addMe; + sentinel.prev = addMe; +} + +void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) +{ + TiXmlAttribute* node; + + for( node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node == removeMe ) + { + node->prev->next = node->next; + node->next->prev = node->prev; + node->next = 0; + node->prev = 0; + return; + } + } + assert( 0 ); // we tried to remove a non-linked attribute. +} + + +#ifdef TIXML_USE_STL +TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node->name == name ) + return node; + } + return 0; +} + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} +#endif + + +TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( strcmp( node->name.c_str(), name ) == 0 ) + return node; + } + return 0; +} + + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} + + +#ifdef TIXML_USE_STL +std::istream& operator>> (std::istream & in, TiXmlNode & base) +{ + TIXML_STRING tag; + tag.reserve( 8 * 1000 ); + base.StreamIn( &in, &tag ); + + base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); + return in; +} +#endif + + +#ifdef TIXML_USE_STL +std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out << printer.Str(); + + return out; +} + + +std::string& operator<< (std::string& out, const TiXmlNode& base ) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out.append( printer.Str() ); + + return out; +} +#endif + + +TiXmlHandle TiXmlHandle::FirstChild() const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement() const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild(); + for ( i=0; + child && iNextSibling(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild( value ); + for ( i=0; + child && iNextSibling( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement(); + for ( i=0; + child && iNextSiblingElement(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement( value ); + for ( i=0; + child && iNextSiblingElement( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) +{ + DoIndent(); + buffer += "<"; + buffer += element.Value(); + + for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) + { + buffer += " "; + attrib->Print( 0, 0, &buffer ); + } + + if ( !element.FirstChild() ) + { + buffer += " />"; + DoLineBreak(); + } + else + { + buffer += ">"; + if ( element.FirstChild()->ToText() + && element.LastChild() == element.FirstChild() + && element.FirstChild()->ToText()->CDATA() == false ) + { + simpleTextPrint = true; + // no DoLineBreak()! + } + else + { + DoLineBreak(); + } + } + ++depth; + return true; +} + + +bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) +{ + --depth; + if ( !element.FirstChild() ) + { + // nothing. + } + else + { + if ( simpleTextPrint ) + { + simpleTextPrint = false; + } + else + { + DoIndent(); + } + buffer += ""; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlText& text ) +{ + if ( text.CDATA() ) + { + DoIndent(); + buffer += ""; + DoLineBreak(); + } + else if ( simpleTextPrint ) + { + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + } + else + { + DoIndent(); + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) +{ + DoIndent(); + declaration.Print( 0, 0, &buffer ); + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlComment& comment ) +{ + DoIndent(); + buffer += ""; + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) +{ + DoIndent(); + buffer += "<"; + buffer += unknown.Value(); + buffer += ">"; + DoLineBreak(); + return true; +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h new file mode 100644 index 0000000..a3589e5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/tinyxml.h @@ -0,0 +1,1805 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TINYXML_INCLUDED +#define TINYXML_INCLUDED + +#ifdef _MSC_VER +#pragma warning( push ) +#pragma warning( disable : 4530 ) +#pragma warning( disable : 4786 ) +#endif + +#include +#include +#include +#include +#include + +// Help out windows: +#if defined( _DEBUG ) && !defined( DEBUG ) +#define DEBUG +#endif + +#ifdef TIXML_USE_STL + #include + #include + #include + #define TIXML_STRING std::string +#else + #include "tinystr.h" + #define TIXML_STRING TiXmlString +#endif + +// Deprecated library function hell. Compilers want to use the +// new safe versions. This probably doesn't fully address the problem, +// but it gets closer. There are too many compilers for me to fully +// test. If you get compilation troubles, undefine TIXML_SAFE +#define TIXML_SAFE + +#ifdef TIXML_SAFE + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + // Microsoft visual studio, version 2005 and higher. + #define TIXML_SNPRINTF _snprintf_s + #define TIXML_SSCANF sscanf_s + #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + //#pragma message( "Using _sn* functions." ) + #define TIXML_SNPRINTF _snprintf + #define TIXML_SSCANF sscanf + #elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + //#warning( "Using sn* functions." ) + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #else + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #endif +#endif + +class TiXmlDocument; +class TiXmlElement; +class TiXmlComment; +class TiXmlUnknown; +class TiXmlAttribute; +class TiXmlText; +class TiXmlDeclaration; +class TiXmlParsingData; + +const int TIXML_MAJOR_VERSION = 2; +const int TIXML_MINOR_VERSION = 6; +const int TIXML_PATCH_VERSION = 2; + +/* Internal structure for tracking location of items + in the XML file. +*/ +struct TiXmlCursor +{ + TiXmlCursor() { Clear(); } + void Clear() { row = col = -1; } + + int row; // 0 based. + int col; // 0 based. +}; + + +/** + Implements the interface to the "Visitor pattern" (see the Accept() method.) + If you call the Accept() method, it requires being passed a TiXmlVisitor + class to handle callbacks. For nodes that contain other nodes (Document, Element) + you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves + are simply called with Visit(). + + If you return 'true' from a Visit method, recursive parsing will continue. If you return + false, no children of this node or its sibilings will be Visited. + + All flavors of Visit methods have a default implementation that returns 'true' (continue + visiting). You need to only override methods that are interesting to you. + + Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. + + You should never change the document from a callback. + + @sa TiXmlNode::Accept() +*/ +class TiXmlVisitor +{ +public: + virtual ~TiXmlVisitor() {} + + /// Visit a document. + virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } + /// Visit a document. + virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } + + /// Visit an element. + virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } + /// Visit an element. + virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } + + /// Visit a declaration + virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } + /// Visit a text node + virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } + /// Visit a comment node + virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } + /// Visit an unknown node + virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } +}; + +// Only used by Attribute::Query functions +enum +{ + TIXML_SUCCESS, + TIXML_NO_ATTRIBUTE, + TIXML_WRONG_TYPE +}; + + +// Used by the parsing routines. +enum TiXmlEncoding +{ + TIXML_ENCODING_UNKNOWN, + TIXML_ENCODING_UTF8, + TIXML_ENCODING_LEGACY +}; + +const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; + +/** TiXmlBase is a base class for every class in TinyXml. + It does little except to establish that TinyXml classes + can be printed and provide some utility functions. + + In XML, the document and elements can contain + other elements and other types of nodes. + + @verbatim + A Document can contain: Element (container or leaf) + Comment (leaf) + Unknown (leaf) + Declaration( leaf ) + + An Element can contain: Element (container or leaf) + Text (leaf) + Attributes (not on tree) + Comment (leaf) + Unknown (leaf) + + A Decleration contains: Attributes (not on tree) + @endverbatim +*/ +class TiXmlBase +{ + friend class TiXmlNode; + friend class TiXmlElement; + friend class TiXmlDocument; + +public: + TiXmlBase() : userData(0) {} + virtual ~TiXmlBase() {} + + /** All TinyXml classes can print themselves to a filestream + or the string class (TiXmlString in non-STL mode, std::string + in STL mode.) Either or both cfile and str can be null. + + This is a formatted print, and will insert + tabs and newlines. + + (For an unformatted stream, use the << operator.) + */ + virtual void Print( FILE* cfile, int depth ) const = 0; + + /** The world does not agree on whether white space should be kept or + not. In order to make everyone happy, these global, static functions + are provided to set whether or not TinyXml will condense all white space + into a single space or not. The default is to condense. Note changing this + value is not thread safe. + */ + static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } + + /// Return the current white space setting. + static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } + + /** Return the position, in the original source file, of this node or attribute. + The row and column are 1-based. (That is the first row and first column is + 1,1). If the returns values are 0 or less, then the parser does not have + a row and column value. + + Generally, the row and column value will be set when the TiXmlDocument::Load(), + TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set + when the DOM was created from operator>>. + + The values reflect the initial load. Once the DOM is modified programmatically + (by adding or changing nodes and attributes) the new values will NOT update to + reflect changes in the document. + + There is a minor performance cost to computing the row and column. Computation + can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. + + @sa TiXmlDocument::SetTabSize() + */ + int Row() const { return location.row + 1; } + int Column() const { return location.col + 1; } ///< See Row() + + void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. + void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. + const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. + + // Table that returs, for a given lead byte, the total number of bytes + // in the UTF-8 sequence. + static const int utf8ByteTable[256]; + + virtual const char* Parse( const char* p, + TiXmlParsingData* data, + TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; + + /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, + or they will be transformed into entities! + */ + static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); + + enum + { + TIXML_NO_ERROR = 0, + TIXML_ERROR, + TIXML_ERROR_OPENING_FILE, + TIXML_ERROR_PARSING_ELEMENT, + TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, + TIXML_ERROR_READING_ELEMENT_VALUE, + TIXML_ERROR_READING_ATTRIBUTES, + TIXML_ERROR_PARSING_EMPTY, + TIXML_ERROR_READING_END_TAG, + TIXML_ERROR_PARSING_UNKNOWN, + TIXML_ERROR_PARSING_COMMENT, + TIXML_ERROR_PARSING_DECLARATION, + TIXML_ERROR_DOCUMENT_EMPTY, + TIXML_ERROR_EMBEDDED_NULL, + TIXML_ERROR_PARSING_CDATA, + TIXML_ERROR_DOCUMENT_TOP_ONLY, + + TIXML_ERROR_STRING_COUNT + }; + +protected: + + static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); + + inline static bool IsWhiteSpace( char c ) + { + return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); + } + inline static bool IsWhiteSpace( int c ) + { + if ( c < 256 ) + return IsWhiteSpace( (char) c ); + return false; // Again, only truly correct for English/Latin...but usually works. + } + + #ifdef TIXML_USE_STL + static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); + static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); + #endif + + /* Reads an XML name into the string provided. Returns + a pointer just past the last character of the name, + or 0 if the function has an error. + */ + static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); + + /* Reads text. Returns a pointer past the given end tag. + Wickedly complex options, but it keeps the (sensitive) code in one place. + */ + static const char* ReadText( const char* in, // where to start + TIXML_STRING* text, // the string read + bool ignoreWhiteSpace, // whether to keep the white space + const char* endTag, // what ends this text + bool ignoreCase, // whether to ignore case in the end tag + TiXmlEncoding encoding ); // the current encoding + + // If an entity has been found, transform it into a character. + static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); + + // Get a character, while interpreting entities. + // The length can be from 0 to 4 bytes. + inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) + { + assert( p ); + if ( encoding == TIXML_ENCODING_UTF8 ) + { + *length = utf8ByteTable[ *((const unsigned char*)p) ]; + assert( *length >= 0 && *length < 5 ); + } + else + { + *length = 1; + } + + if ( *length == 1 ) + { + if ( *p == '&' ) + return GetEntity( p, _value, length, encoding ); + *_value = *p; + return p+1; + } + else if ( *length ) + { + //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), + // and the null terminator isn't needed + for( int i=0; p[i] && i<*length; ++i ) { + _value[i] = p[i]; + } + return p + (*length); + } + else + { + // Not valid text. + return 0; + } + } + + // Return true if the next characters in the stream are any of the endTag sequences. + // Ignore case only works for english, and should only be relied on when comparing + // to English words: StringEqual( p, "version", true ) is fine. + static bool StringEqual( const char* p, + const char* endTag, + bool ignoreCase, + TiXmlEncoding encoding ); + + static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; + + TiXmlCursor location; + + /// Field containing a generic user pointer + void* userData; + + // None of these methods are reliable for any language except English. + // Good for approximation, not great for accuracy. + static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); + static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); + inline static int ToLower( int v, TiXmlEncoding encoding ) + { + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( v < 128 ) return tolower( v ); + return v; + } + else + { + return tolower( v ); + } + } + static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); + +private: + TiXmlBase( const TiXmlBase& ); // not implemented. + void operator=( const TiXmlBase& base ); // not allowed. + + struct Entity + { + const char* str; + unsigned int strLength; + char chr; + }; + enum + { + NUM_ENTITY = 5, + MAX_ENTITY_LENGTH = 6 + + }; + static Entity entity[ NUM_ENTITY ]; + static bool condenseWhiteSpace; +}; + + +/** The parent class for everything in the Document Object Model. + (Except for attributes). + Nodes have siblings, a parent, and children. A node can be + in a document, or stand on its own. The type of a TiXmlNode + can be queried, and it can be cast to its more defined type. +*/ +class TiXmlNode : public TiXmlBase +{ + friend class TiXmlDocument; + friend class TiXmlElement; + +public: + #ifdef TIXML_USE_STL + + /** An input stream operator, for every class. Tolerant of newlines and + formatting, but doesn't expect them. + */ + friend std::istream& operator >> (std::istream& in, TiXmlNode& base); + + /** An output stream operator, for every class. Note that this outputs + without any newlines or formatting, as opposed to Print(), which + includes tabs and new lines. + + The operator<< and operator>> are not completely symmetric. Writing + a node to a stream is very well defined. You'll get a nice stream + of output, without any extra whitespace or newlines. + + But reading is not as well defined. (As it always is.) If you create + a TiXmlElement (for example) and read that from an input stream, + the text needs to define an element or junk will result. This is + true of all input streams, but it's worth keeping in mind. + + A TiXmlDocument will read nodes until it reads a root element, and + all the children of that root element. + */ + friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); + + /// Appends the XML node or attribute to a std::string. + friend std::string& operator<< (std::string& out, const TiXmlNode& base ); + + #endif + + /** The types of XML nodes supported by TinyXml. (All the + unsupported types are picked up by UNKNOWN.) + */ + enum NodeType + { + TINYXML_DOCUMENT, + TINYXML_ELEMENT, + TINYXML_COMMENT, + TINYXML_UNKNOWN, + TINYXML_TEXT, + TINYXML_DECLARATION, + TINYXML_TYPECOUNT + }; + + virtual ~TiXmlNode(); + + /** The meaning of 'value' changes for the specific type of + TiXmlNode. + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + + The subclasses will wrap this function. + */ + const char *Value() const { return value.c_str (); } + + #ifdef TIXML_USE_STL + /** Return Value() as a std::string. If you only use STL, + this is more efficient than calling Value(). + Only available in STL mode. + */ + const std::string& ValueStr() const { return value; } + #endif + + const TIXML_STRING& ValueTStr() const { return value; } + + /** Changes the value of the node. Defined as: + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + */ + void SetValue(const char * _value) { value = _value;} + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Delete all the children of this node. Does not affect 'this'. + void Clear(); + + /// One step up the DOM. + TiXmlNode* Parent() { return parent; } + const TiXmlNode* Parent() const { return parent; } + + const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. + TiXmlNode* FirstChild() { return firstChild; } + const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. + /// The first child of this node with the matching 'value'. Will be null if none found. + TiXmlNode* FirstChild( const char * _value ) { + // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) + // call the method, cast the return back to non-const. + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); + } + const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. + TiXmlNode* LastChild() { return lastChild; } + + const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. + TiXmlNode* LastChild( const char * _value ) { + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. + #endif + + /** An alternate way to walk the children of a node. + One way to iterate over nodes is: + @verbatim + for( child = parent->FirstChild(); child; child = child->NextSibling() ) + @endverbatim + + IterateChildren does the same thing with the syntax: + @verbatim + child = 0; + while( child = parent->IterateChildren( child ) ) + @endverbatim + + IterateChildren takes the previous child as input and finds + the next one. If the previous child is null, it returns the + first. IterateChildren will return null when done. + */ + const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); + } + + /// This flavor of IterateChildren searches for children with a particular 'value' + const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + #endif + + /** Add a new node related to this. Adds a child past the LastChild. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); + + + /** Add a new node related to this. Adds a child past the LastChild. + + NOTE: the node to be added is passed by pointer, and will be + henceforth owned (and deleted) by tinyXml. This method is efficient + and avoids an extra copy, but should be used with care as it + uses a different memory model than the other insert functions. + + @sa InsertEndChild + */ + TiXmlNode* LinkEndChild( TiXmlNode* addThis ); + + /** Add a new node related to this. Adds a child before the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); + + /** Add a new node related to this. Adds a child after the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); + + /** Replace a child of this node. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); + + /// Delete a child of this node. + bool RemoveChild( TiXmlNode* removeThis ); + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling() const { return prev; } + TiXmlNode* PreviousSibling() { return prev; } + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling( const char * ) const; + TiXmlNode* PreviousSibling( const char *_prev ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Navigate to a sibling node. + const TiXmlNode* NextSibling() const { return next; } + TiXmlNode* NextSibling() { return next; } + + /// Navigate to a sibling node with the given 'value'. + const TiXmlNode* NextSibling( const char * ) const; + TiXmlNode* NextSibling( const char* _next ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement() const; + TiXmlElement* NextSiblingElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement( const char * ) const; + TiXmlElement* NextSiblingElement( const char *_next ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement() const; + TiXmlElement* FirstChildElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); + } + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement( const char * _value ) const; + TiXmlElement* FirstChildElement( const char * _value ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /** Query the type (as an enumerated value, above) of this node. + The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, + TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. + */ + int Type() const { return type; } + + /** Return a pointer to the Document this node lives in. + Returns null if not in a document. + */ + const TiXmlDocument* GetDocument() const; + TiXmlDocument* GetDocument() { + return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); + } + + /// Returns true if this node has no children. + bool NoChildren() const { return !firstChild; } + + virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + /** Create an exact duplicate of this node and return it. The memory must be deleted + by the caller. + */ + virtual TiXmlNode* Clone() const = 0; + + /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the + XML tree will be conditionally visited and the host will be called back + via the TiXmlVisitor interface. + + This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse + the XML for the callbacks, so the performance of TinyXML is unchanged by using this + interface versus any other.) + + The interface has been based on ideas from: + + - http://www.saxproject.org/ + - http://c2.com/cgi/wiki?HierarchicalVisitorPattern + + Which are both good references for "visiting". + + An example of using Accept(): + @verbatim + TiXmlPrinter printer; + tinyxmlDoc.Accept( &printer ); + const char* xmlcstr = printer.CStr(); + @endverbatim + */ + virtual bool Accept( TiXmlVisitor* visitor ) const = 0; + +protected: + TiXmlNode( NodeType _type ); + + // Copy to the allocated object. Shared functionality between Clone, Copy constructor, + // and the assignment operator. + void CopyTo( TiXmlNode* target ) const; + + #ifdef TIXML_USE_STL + // The real work of the input operator. + virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; + #endif + + // Figure out what is at *p, and parse it. Returns null if it is not an xml node. + TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); + + TiXmlNode* parent; + NodeType type; + + TiXmlNode* firstChild; + TiXmlNode* lastChild; + + TIXML_STRING value; + + TiXmlNode* prev; + TiXmlNode* next; + +private: + TiXmlNode( const TiXmlNode& ); // not implemented. + void operator=( const TiXmlNode& base ); // not allowed. +}; + + +/** An attribute is a name-value pair. Elements have an arbitrary + number of attributes, each with a unique name. + + @note The attributes are not TiXmlNodes, since they are not + part of the tinyXML document object model. There are other + suggested ways to look at this problem. +*/ +class TiXmlAttribute : public TiXmlBase +{ + friend class TiXmlAttributeSet; + +public: + /// Construct an empty attribute. + TiXmlAttribute() : TiXmlBase() + { + document = 0; + prev = next = 0; + } + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlAttribute( const std::string& _name, const std::string& _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + #endif + + /// Construct an attribute with a name and value. + TiXmlAttribute( const char * _name, const char * _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + + const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. + const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. + #ifdef TIXML_USE_STL + const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. + #endif + int IntValue() const; ///< Return the value of this attribute, converted to an integer. + double DoubleValue() const; ///< Return the value of this attribute, converted to a double. + + // Get the tinyxml string representation + const TIXML_STRING& NameTStr() const { return name; } + + /** QueryIntValue examines the value string. It is an alternative to the + IntValue() method with richer error checking. + If the value is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. + + A specialized but useful call. Note that for success it returns 0, + which is the opposite of almost all other TinyXml calls. + */ + int QueryIntValue( int* _value ) const; + /// QueryDoubleValue examines the value string. See QueryIntValue(). + int QueryDoubleValue( double* _value ) const; + + void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. + void SetValue( const char* _value ) { value = _value; } ///< Set the value. + + void SetIntValue( int _value ); ///< Set the value from an integer. + void SetDoubleValue( double _value ); ///< Set the value from a double. + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetName( const std::string& _name ) { name = _name; } + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Get the next sibling attribute in the DOM. Returns null at end. + const TiXmlAttribute* Next() const; + TiXmlAttribute* Next() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); + } + + /// Get the previous sibling attribute in the DOM. Returns null at beginning. + const TiXmlAttribute* Previous() const; + TiXmlAttribute* Previous() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); + } + + bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } + bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } + bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } + + /* Attribute parsing starts: first letter of the name + returns: the next char after the value end quote + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + // Prints this Attribute to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + + // [internal use] + // Set the document pointer so the attribute can report errors. + void SetDocument( TiXmlDocument* doc ) { document = doc; } + +private: + TiXmlAttribute( const TiXmlAttribute& ); // not implemented. + void operator=( const TiXmlAttribute& base ); // not allowed. + + TiXmlDocument* document; // A pointer back to a document, for error reporting. + TIXML_STRING name; + TIXML_STRING value; + TiXmlAttribute* prev; + TiXmlAttribute* next; +}; + + +/* A class used to manage a group of attributes. + It is only used internally, both by the ELEMENT and the DECLARATION. + + The set can be changed transparent to the Element and Declaration + classes that use it, but NOT transparent to the Attribute + which has to implement a next() and previous() method. Which makes + it a bit problematic and prevents the use of STL. + + This version is implemented with circular lists because: + - I like circular lists + - it demonstrates some independence from the (typical) doubly linked list. +*/ +class TiXmlAttributeSet +{ +public: + TiXmlAttributeSet(); + ~TiXmlAttributeSet(); + + void Add( TiXmlAttribute* attribute ); + void Remove( TiXmlAttribute* attribute ); + + const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + + TiXmlAttribute* Find( const char* _name ) const; + TiXmlAttribute* FindOrCreate( const char* _name ); + +# ifdef TIXML_USE_STL + TiXmlAttribute* Find( const std::string& _name ) const; + TiXmlAttribute* FindOrCreate( const std::string& _name ); +# endif + + +private: + //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), + //*ME: this class must be also use a hidden/disabled copy-constructor !!! + TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed + void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) + + TiXmlAttribute sentinel; +}; + + +/** The element is a container class. It has a value, the element name, + and can contain other elements, text, comments, and unknowns. + Elements also contain an arbitrary number of attributes. +*/ +class TiXmlElement : public TiXmlNode +{ +public: + /// Construct an element. + TiXmlElement (const char * in_value); + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlElement( const std::string& _value ); + #endif + + TiXmlElement( const TiXmlElement& ); + + TiXmlElement& operator=( const TiXmlElement& base ); + + virtual ~TiXmlElement(); + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + */ + const char* Attribute( const char* name ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an integer, + the integer value will be put in the return 'i', if 'i' + is non-null. + */ + const char* Attribute( const char* name, int* i ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an double, + the double value will be put in the return 'd', if 'd' + is non-null. + */ + const char* Attribute( const char* name, double* d ) const; + + /** QueryIntAttribute examines the attribute - it is an alternative to the + Attribute() method with richer error checking. + If the attribute is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. If the attribute + does not exist, then TIXML_NO_ATTRIBUTE is returned. + */ + int QueryIntAttribute( const char* name, int* _value ) const; + /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). + int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; + /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). + Note that '1', 'true', or 'yes' are considered true, while '0', 'false' + and 'no' are considered false. + */ + int QueryBoolAttribute( const char* name, bool* _value ) const; + /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). + int QueryDoubleAttribute( const char* name, double* _value ) const; + /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). + int QueryFloatAttribute( const char* name, float* _value ) const { + double d; + int result = QueryDoubleAttribute( name, &d ); + if ( result == TIXML_SUCCESS ) { + *_value = (float)d; + } + return result; + } + + #ifdef TIXML_USE_STL + /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). + int QueryStringAttribute( const char* name, std::string* _value ) const { + const char* cstr = Attribute( name ); + if ( cstr ) { + *_value = std::string( cstr ); + return TIXML_SUCCESS; + } + return TIXML_NO_ATTRIBUTE; + } + + /** Template form of the attribute query which will try to read the + attribute into the specified type. Very easy, very powerful, but + be careful to make sure to call this with the correct type. + + NOTE: This method doesn't work correctly for 'string' types that contain spaces. + + @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE + */ + template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + std::stringstream sstream( node->ValueStr() ); + sstream >> *outValue; + if ( !sstream.fail() ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; + } + + int QueryValueAttribute( const std::string& name, std::string* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + *outValue = node->ValueStr(); + return TIXML_SUCCESS; + } + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char* name, const char * _value ); + + #ifdef TIXML_USE_STL + const std::string* Attribute( const std::string& name ) const; + const std::string* Attribute( const std::string& name, int* i ) const; + const std::string* Attribute( const std::string& name, double* d ) const; + int QueryIntAttribute( const std::string& name, int* _value ) const; + int QueryDoubleAttribute( const std::string& name, double* _value ) const; + + /// STL std::string form. + void SetAttribute( const std::string& name, const std::string& _value ); + ///< STL std::string form. + void SetAttribute( const std::string& name, int _value ); + ///< STL std::string form. + void SetDoubleAttribute( const std::string& name, double value ); + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char * name, int value ); + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetDoubleAttribute( const char * name, double value ); + + /** Deletes an attribute with the given name. + */ + void RemoveAttribute( const char * name ); + #ifdef TIXML_USE_STL + void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. + #endif + + const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. + TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } + const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. + TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, GetText() is limited compared to getting the TiXmlText child + and accessing it directly. + + If the first child of 'this' is a TiXmlText, the GetText() + returns the character string of the Text node, else null is returned. + + This is a convenient method for getting the text of simple contained text: + @verbatim + This is text + const char* str = fooElement->GetText(); + @endverbatim + + 'str' will be a pointer to "This is text". + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + This is text + @endverbatim + + then the value of str would be null. The first child node isn't a text node, it is + another element. From this XML: + @verbatim + This is text + @endverbatim + GetText() will return "This is ". + + WARNING: GetText() accesses a child node - don't become confused with the + similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are + safe type casts on the referenced node. + */ + const char* GetText() const; + + /// Creates a new Element and returns it - the returned element is a copy. + virtual TiXmlNode* Clone() const; + // Print the Element to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: next char past '<' + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + + void CopyTo( TiXmlElement* target ) const; + void ClearThis(); // like clear, but initializes 'this' object as well + + // Used to be public [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + /* [internal use] + Reads the "value" of the element -- another element, or text. + This should terminate with the current end tag. + */ + const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + +private: + TiXmlAttributeSet attributeSet; +}; + + +/** An XML comment. +*/ +class TiXmlComment : public TiXmlNode +{ +public: + /// Constructs an empty comment. + TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} + /// Construct a comment from text. + TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { + SetValue( _value ); + } + TiXmlComment( const TiXmlComment& ); + TiXmlComment& operator=( const TiXmlComment& base ); + + virtual ~TiXmlComment() {} + + /// Returns a copy of this Comment. + virtual TiXmlNode* Clone() const; + // Write this Comment to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: at the ! of the !-- + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlComment* target ) const; + + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif +// virtual void StreamOut( TIXML_OSTREAM * out ) const; + +private: + +}; + + +/** XML text. A text node can have 2 ways to output the next. "normal" output + and CDATA. It will default to the mode it was parsed from the XML file and + you generally want to leave it alone, but you can change the output mode with + SetCDATA() and query it with CDATA(). +*/ +class TiXmlText : public TiXmlNode +{ + friend class TiXmlElement; +public: + /** Constructor for text element. By default, it is treated as + normal, encoded text. If you want it be output as a CDATA text + element, set the parameter _cdata to 'true' + */ + TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + virtual ~TiXmlText() {} + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + #endif + + TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } + TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } + + // Write this text object to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /// Queries whether this represents text using a CDATA section. + bool CDATA() const { return cdata; } + /// Turns on or off a CDATA representation of text. + void SetCDATA( bool _cdata ) { cdata = _cdata; } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + /// [internal use] Creates a new Element and returns it. + virtual TiXmlNode* Clone() const; + void CopyTo( TiXmlText* target ) const; + + bool Blank() const; // returns true if all white space and new lines + // [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + bool cdata; // true if this should be input and output as a CDATA style text element +}; + + +/** In correct XML the declaration is the first entry in the file. + @verbatim + + @endverbatim + + TinyXml will happily read or write files without a declaration, + however. There are 3 possible attributes to the declaration: + version, encoding, and standalone. + + Note: In this version of the code, the attributes are + handled as special cases, not generic attributes, simply + because there can only be at most 3 and they are always the same. +*/ +class TiXmlDeclaration : public TiXmlNode +{ +public: + /// Construct an empty declaration. + TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} + +#ifdef TIXML_USE_STL + /// Constructor. + TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ); +#endif + + /// Construct. + TiXmlDeclaration( const char* _version, + const char* _encoding, + const char* _standalone ); + + TiXmlDeclaration( const TiXmlDeclaration& copy ); + TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); + + virtual ~TiXmlDeclaration() {} + + /// Version. Will return an empty string if none was found. + const char *Version() const { return version.c_str (); } + /// Encoding. Will return an empty string if none was found. + const char *Encoding() const { return encoding.c_str (); } + /// Is this a standalone document? + const char *Standalone() const { return standalone.c_str (); } + + /// Creates a copy of this Declaration and returns it. + virtual TiXmlNode* Clone() const; + // Print this declaration to a FILE stream. + virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlDeclaration* target ) const; + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + + TIXML_STRING version; + TIXML_STRING encoding; + TIXML_STRING standalone; +}; + + +/** Any tag that tinyXml doesn't recognize is saved as an + unknown. It is a tag of text, but should not be modified. + It will be written back to the XML, unchanged, when the file + is saved. + + DTD tags get thrown into TiXmlUnknowns. +*/ +class TiXmlUnknown : public TiXmlNode +{ +public: + TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} + virtual ~TiXmlUnknown() {} + + TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } + TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } + + /// Creates a copy of this Unknown and returns it. + virtual TiXmlNode* Clone() const; + // Print this Unknown to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected: + void CopyTo( TiXmlUnknown* target ) const; + + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + +}; + + +/** Always the top level node. A document binds together all the + XML pieces. It can be saved, loaded, and printed to the screen. + The 'value' of a document node is the xml file name. +*/ +class TiXmlDocument : public TiXmlNode +{ +public: + /// Create an empty document, that has no name. + TiXmlDocument(); + /// Create a document with a name. The name of the document is also the filename of the xml. + TiXmlDocument( const char * documentName ); + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlDocument( const std::string& documentName ); + #endif + + TiXmlDocument( const TiXmlDocument& copy ); + TiXmlDocument& operator=( const TiXmlDocument& copy ); + + virtual ~TiXmlDocument() {} + + /** Load a file using the current document value. + Returns true if successful. Will delete any existing + document data before loading. + */ + bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the current document value. Returns true if successful. + bool SaveFile() const; + /// Load a file using the given filename. Returns true if successful. + bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given filename. Returns true if successful. + bool SaveFile( const char * filename ) const; + /** Load a file using the given FILE*. Returns true if successful. Note that this method + doesn't stream - the entire object pointed at by the FILE* + will be interpreted as an XML file. TinyXML doesn't stream in XML from the current + file location. Streaming may be added in the future. + */ + bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given FILE*. Returns true if successful. + bool SaveFile( FILE* ) const; + + #ifdef TIXML_USE_STL + bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. + { + return LoadFile( filename.c_str(), encoding ); + } + bool SaveFile( const std::string& filename ) const ///< STL std::string version. + { + return SaveFile( filename.c_str() ); + } + #endif + + /** Parse the given null terminated block of xml data. Passing in an encoding to this + method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml + to use that encoding, regardless of what TinyXml might otherwise try to detect. + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + + /** Get the root element -- the only top level element -- of the document. + In well formed XML, there should only be one. TinyXml is tolerant of + multiple elements at the document level. + */ + const TiXmlElement* RootElement() const { return FirstChildElement(); } + TiXmlElement* RootElement() { return FirstChildElement(); } + + /** If an error occurs, Error will be set to true. Also, + - The ErrorId() will contain the integer identifier of the error (not generally useful) + - The ErrorDesc() method will return the name of the error. (very useful) + - The ErrorRow() and ErrorCol() will return the location of the error (if known) + */ + bool Error() const { return error; } + + /// Contains a textual (english) description of the error if one occurs. + const char * ErrorDesc() const { return errorDesc.c_str (); } + + /** Generally, you probably want the error string ( ErrorDesc() ). But if you + prefer the ErrorId, this function will fetch it. + */ + int ErrorId() const { return errorId; } + + /** Returns the location (if known) of the error. The first column is column 1, + and the first row is row 1. A value of 0 means the row and column wasn't applicable + (memory errors, for example, have no row/column) or the parser lost the error. (An + error in the error reporting, in that case.) + + @sa SetTabSize, Row, Column + */ + int ErrorRow() const { return errorLocation.row+1; } + int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() + + /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) + to report the correct values for row and column. It does not change the output + or input in any way. + + By calling this method, with a tab size + greater than 0, the row and column of each node and attribute is stored + when the file is loaded. Very useful for tracking the DOM back in to + the source file. + + The tab size is required for calculating the location of nodes. If not + set, the default of 4 is used. The tabsize is set per document. Setting + the tabsize to 0 disables row/column tracking. + + Note that row and column tracking is not supported when using operator>>. + + The tab size needs to be enabled before the parse or load. Correct usage: + @verbatim + TiXmlDocument doc; + doc.SetTabSize( 8 ); + doc.Load( "myfile.xml" ); + @endverbatim + + @sa Row, Column + */ + void SetTabSize( int _tabsize ) { tabsize = _tabsize; } + + int TabSize() const { return tabsize; } + + /** If you have handled the error, it can be reset with this call. The error + state is automatically cleared if you Parse a new XML block. + */ + void ClearError() { error = false; + errorId = 0; + errorDesc = ""; + errorLocation.row = errorLocation.col = 0; + //errorLocation.last = 0; + } + + /** Write the document to standard out using formatted printing ("pretty print"). */ + void Print() const { Print( stdout, 0 ); } + + /* Write the document to a string using formatted printing ("pretty print"). This + will allocate a character array (new char[]) and return it as a pointer. The + calling code pust call delete[] on the return char* to avoid a memory leak. + */ + //char* PrintToMemory() const; + + /// Print this Document to a FILE stream. + virtual void Print( FILE* cfile, int depth = 0 ) const; + // [internal use] + void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + + virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + // [internal use] + virtual TiXmlNode* Clone() const; + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + void CopyTo( TiXmlDocument* target ) const; + + bool error; + int errorId; + TIXML_STRING errorDesc; + int tabsize; + TiXmlCursor errorLocation; + bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. +}; + + +/** + A TiXmlHandle is a class that wraps a node pointer with null checks; this is + an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml + DOM structure. It is a separate utility class. + + Take an example: + @verbatim + + + + + + + @endverbatim + + Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very + easy to write a *lot* of code that looks like: + + @verbatim + TiXmlElement* root = document.FirstChildElement( "Document" ); + if ( root ) + { + TiXmlElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + TiXmlElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + TiXmlElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. + @endverbatim + + And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity + of such code. A TiXmlHandle checks for null pointers so it is perfectly safe + and correct to use: + + @verbatim + TiXmlHandle docHandle( &document ); + TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); + if ( child2 ) + { + // do something useful + @endverbatim + + Which is MUCH more concise and useful. + + It is also safe to copy handles - internally they are nothing more than node pointers. + @verbatim + TiXmlHandle handleCopy = handle; + @endverbatim + + What they should not be used for is iteration: + + @verbatim + int i=0; + while ( true ) + { + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); + if ( !child ) + break; + // do something + ++i; + } + @endverbatim + + It seems reasonable, but it is in fact two embedded while loops. The Child method is + a linear walk to find the element, so this code would iterate much more than it needs + to. Instead, prefer: + + @verbatim + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); + + for( child; child; child=child->NextSiblingElement() ) + { + // do something + } + @endverbatim +*/ +class TiXmlHandle +{ +public: + /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. + TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } + /// Copy constructor + TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } + TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } + + /// Return a handle to the first child node. + TiXmlHandle FirstChild() const; + /// Return a handle to the first child node with the given name. + TiXmlHandle FirstChild( const char * value ) const; + /// Return a handle to the first child element. + TiXmlHandle FirstChildElement() const; + /// Return a handle to the first child element with the given name. + TiXmlHandle FirstChildElement( const char * value ) const; + + /** Return a handle to the "index" child with the given name. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( const char* value, int index ) const; + /** Return a handle to the "index" child. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( int index ) const; + /** Return a handle to the "index" child element with the given name. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( const char* value, int index ) const; + /** Return a handle to the "index" child element. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( int index ) const; + + #ifdef TIXML_USE_STL + TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } + TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } + + TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } + TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } + #endif + + /** Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* ToNode() const { return node; } + /** Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } + /** Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } + /** Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } + + /** @deprecated use ToNode. + Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* Node() const { return ToNode(); } + /** @deprecated use ToElement. + Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* Element() const { return ToElement(); } + /** @deprecated use ToText() + Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* Text() const { return ToText(); } + /** @deprecated use ToUnknown() + Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* Unknown() const { return ToUnknown(); } + +private: + TiXmlNode* node; +}; + + +/** Print to memory functionality. The TiXmlPrinter is useful when you need to: + + -# Print to memory (especially in non-STL mode) + -# Control formatting (line endings, etc.) + + When constructed, the TiXmlPrinter is in its default "pretty printing" mode. + Before calling Accept() you can call methods to control the printing + of the XML document. After TiXmlNode::Accept() is called, the printed document can + be accessed via the CStr(), Str(), and Size() methods. + + TiXmlPrinter uses the Visitor API. + @verbatim + TiXmlPrinter printer; + printer.SetIndent( "\t" ); + + doc.Accept( &printer ); + fprintf( stdout, "%s", printer.CStr() ); + @endverbatim +*/ +class TiXmlPrinter : public TiXmlVisitor +{ +public: + TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), + buffer(), indent( " " ), lineBreak( "\n" ) {} + + virtual bool VisitEnter( const TiXmlDocument& doc ); + virtual bool VisitExit( const TiXmlDocument& doc ); + + virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); + virtual bool VisitExit( const TiXmlElement& element ); + + virtual bool Visit( const TiXmlDeclaration& declaration ); + virtual bool Visit( const TiXmlText& text ); + virtual bool Visit( const TiXmlComment& comment ); + virtual bool Visit( const TiXmlUnknown& unknown ); + + /** Set the indent characters for printing. By default 4 spaces + but tab (\t) is also useful, or null/empty string for no indentation. + */ + void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } + /// Query the indention string. + const char* Indent() { return indent.c_str(); } + /** Set the line breaking string. By default set to newline (\n). + Some operating systems prefer other characters, or can be + set to the null/empty string for no indenation. + */ + void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } + /// Query the current line breaking string. + const char* LineBreak() { return lineBreak.c_str(); } + + /** Switch over to "stream printing" which is the most dense formatting without + linebreaks. Common when the XML is needed for network transmission. + */ + void SetStreamPrinting() { indent = ""; + lineBreak = ""; + } + /// Return the result. + const char* CStr() { return buffer.c_str(); } + /// Return the length of the result string. + size_t Size() { return buffer.size(); } + + #ifdef TIXML_USE_STL + /// Return the result. + const std::string& Str() { return buffer; } + #endif + +private: + void DoIndent() { + for( int i=0; i +#include + +#include "tinyxml.h" + +//#define DEBUG_PARSER +#if defined( DEBUG_PARSER ) +# if defined( DEBUG ) && defined( _MSC_VER ) +# include +# define TIXML_LOG OutputDebugString +# else +# define TIXML_LOG printf +# endif +#endif + +// Note tha "PutString" hardcodes the same list. This +// is less flexible than it appears. Changing the entries +// or order will break putstring. +TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = +{ + { "&", 5, '&' }, + { "<", 4, '<' }, + { ">", 4, '>' }, + { """, 6, '\"' }, + { "'", 6, '\'' } +}; + +// Bunch of unicode info at: +// http://www.unicode.org/faq/utf_bom.html +// Including the basic of this table, which determines the #bytes in the +// sequence from the lead byte. 1 placed for invalid sequences -- +// although the result will be junk, pass it through as much as possible. +// Beware of the non-characters in UTF-8: +// ef bb bf (Microsoft "lead bytes") +// ef bf be +// ef bf bf + +const unsigned char TIXML_UTF_LEAD_0 = 0xefU; +const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; +const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + +const int TiXmlBase::utf8ByteTable[256] = +{ + // 0 1 2 3 4 5 6 7 8 9 a b c d e f + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 + 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 + 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte + 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid +}; + + +void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) +{ + const unsigned long BYTE_MASK = 0xBF; + const unsigned long BYTE_MARK = 0x80; + const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; + + if (input < 0x80) + *length = 1; + else if ( input < 0x800 ) + *length = 2; + else if ( input < 0x10000 ) + *length = 3; + else if ( input < 0x200000 ) + *length = 4; + else + { *length = 0; return; } // This code won't covert this correctly anyway. + + output += *length; + + // Scary scary fall throughs. + switch (*length) + { + case 4: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 3: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 2: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 1: + --output; + *output = (char)(input | FIRST_BYTE_MARK[*length]); + } +} + + +/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalpha( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalpha( anyByte ); +// } +} + + +/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalnum( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalnum( anyByte ); +// } +} + + +class TiXmlParsingData +{ + friend class TiXmlDocument; + public: + void Stamp( const char* now, TiXmlEncoding encoding ); + + const TiXmlCursor& Cursor() const { return cursor; } + + private: + // Only used by the document! + TiXmlParsingData( const char* start, int _tabsize, int row, int col ) + { + assert( start ); + stamp = start; + tabsize = _tabsize; + cursor.row = row; + cursor.col = col; + } + + TiXmlCursor cursor; + const char* stamp; + int tabsize; +}; + + +void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) +{ + assert( now ); + + // Do nothing if the tabsize is 0. + if ( tabsize < 1 ) + { + return; + } + + // Get the current row, column. + int row = cursor.row; + int col = cursor.col; + const char* p = stamp; + assert( p ); + + while ( p < now ) + { + // Treat p as unsigned, so we have a happy compiler. + const unsigned char* pU = (const unsigned char*)p; + + // Code contributed by Fletcher Dunn: (modified by lee) + switch (*pU) { + case 0: + // We *should* never get here, but in case we do, don't + // advance past the terminating null character, ever + return; + + case '\r': + // bump down to the next line + ++row; + col = 0; + // Eat the character + ++p; + + // Check for \r\n sequence, and treat this as a single character + if (*p == '\n') { + ++p; + } + break; + + case '\n': + // bump down to the next line + ++row; + col = 0; + + // Eat the character + ++p; + + // Check for \n\r sequence, and treat this as a single + // character. (Yes, this bizarre thing does occur still + // on some arcane platforms...) + if (*p == '\r') { + ++p; + } + break; + + case '\t': + // Eat the character + ++p; + + // Skip to next tab stop + col = (col / tabsize + 1) * tabsize; + break; + + case TIXML_UTF_LEAD_0: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( *(p+1) && *(p+2) ) + { + // In these cases, don't advance the column. These are + // 0-width spaces. + if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) + p += 3; + else + { p +=3; ++col; } // A normal character. + } + } + else + { + ++p; + ++col; + } + break; + + default: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // Eat the 1 to 4 byte utf8 character. + int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; + if ( step == 0 ) + step = 1; // Error case from bad encoding, but handle gracefully. + p += step; + + // Just advance one column, of course. + ++col; + } + else + { + ++p; + ++col; + } + break; + } + } + cursor.row = row; + cursor.col = col; + assert( cursor.row >= -1 ); + assert( cursor.col >= -1 ); + stamp = p; + assert( stamp ); +} + + +const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) +{ + if ( !p || !*p ) + { + return 0; + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + while ( *p ) + { + const unsigned char* pU = (const unsigned char*)p; + + // Skip the stupid Microsoft UTF-8 Byte order marks + if ( *(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==TIXML_UTF_LEAD_1 + && *(pU+2)==TIXML_UTF_LEAD_2 ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbeU ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbfU ) + { + p += 3; + continue; + } + + if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. + ++p; + else + break; + } + } + else + { + while ( *p && IsWhiteSpace( *p ) ) + ++p; + } + + return p; +} + +#ifdef TIXML_USE_STL +/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) +{ + for( ;; ) + { + if ( !in->good() ) return false; + + int c = in->peek(); + // At this scope, we can't get to a document. So fail silently. + if ( !IsWhiteSpace( c ) || c <= 0 ) + return true; + + *tag += (char) in->get(); + } +} + +/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) +{ + //assert( character > 0 && character < 128 ); // else it won't work in utf-8 + while ( in->good() ) + { + int c = in->peek(); + if ( c == character ) + return true; + if ( c <= 0 ) // Silent failure: can't get document at this scope + return false; + + in->get(); + *tag += (char) c; + } + return false; +} +#endif + +// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The +// "assign" optimization removes over 10% of the execution time. +// +const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) +{ + // Oddly, not supported on some comilers, + //name->clear(); + // So use this: + *name = ""; + assert( p ); + + // Names start with letters or underscores. + // Of course, in unicode, tinyxml has no idea what a letter *is*. The + // algorithm is generous. + // + // After that, they can be letters, underscores, numbers, + // hyphens, or colons. (Colons are valid ony for namespaces, + // but tinyxml can't tell namespaces from names.) + if ( p && *p + && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) + { + const char* start = p; + while( p && *p + && ( IsAlphaNum( (unsigned char ) *p, encoding ) + || *p == '_' + || *p == '-' + || *p == '.' + || *p == ':' ) ) + { + //(*name) += *p; // expensive + ++p; + } + if ( p-start > 0 ) { + name->assign( start, p-start ); + } + return p; + } + return 0; +} + +const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) +{ + // Presume an entity, and pull it out. + TIXML_STRING ent; + int i; + *length = 0; + + if ( *(p+1) && *(p+1) == '#' && *(p+2) ) + { + unsigned long ucs = 0; + ptrdiff_t delta = 0; + unsigned mult = 1; + + if ( *(p+2) == 'x' ) + { + // Hexadecimal. + if ( !*(p+3) ) return 0; + + const char* q = p+3; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != 'x' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else if ( *q >= 'a' && *q <= 'f' ) + ucs += mult * (*q - 'a' + 10); + else if ( *q >= 'A' && *q <= 'F' ) + ucs += mult * (*q - 'A' + 10 ); + else + return 0; + mult *= 16; + --q; + } + } + else + { + // Decimal. + if ( !*(p+2) ) return 0; + + const char* q = p+2; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != '#' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else + return 0; + mult *= 10; + --q; + } + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // convert the UCS to UTF-8 + ConvertUTF32ToUTF8( ucs, value, length ); + } + else + { + *value = (char)ucs; + *length = 1; + } + return p + delta + 1; + } + + // Now try to match it. + for( i=0; iappend( cArr, len ); + } + } + else + { + bool whitespace = false; + + // Remove leading white space: + p = SkipWhiteSpace( p, encoding ); + while ( p && *p + && !StringEqual( p, endTag, caseInsensitive, encoding ) ) + { + if ( *p == '\r' || *p == '\n' ) + { + whitespace = true; + ++p; + } + else if ( IsWhiteSpace( *p ) ) + { + whitespace = true; + ++p; + } + else + { + // If we've found whitespace, add it before the + // new character. Any whitespace just becomes a space. + if ( whitespace ) + { + (*text) += ' '; + whitespace = false; + } + int len; + char cArr[4] = { 0, 0, 0, 0 }; + p = GetChar( p, cArr, &len, encoding ); + if ( len == 1 ) + (*text) += cArr[0]; // more efficient + else + text->append( cArr, len ); + } + } + } + if ( p && *p ) + p += strlen( endTag ); + return ( p && *p ) ? p : 0; +} + +#ifdef TIXML_USE_STL + +void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + // The basic issue with a document is that we don't know what we're + // streaming. Read something presumed to be a tag (and hope), then + // identify it, and call the appropriate stream method on the tag. + // + // This "pre-streaming" will never read the closing ">" so the + // sub-tag can orient itself. + + if ( !StreamTo( in, '<', tag ) ) + { + SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + while ( in->good() ) + { + int tagIndex = (int) tag->length(); + while ( in->good() && in->peek() != '>' ) + { + int c = in->get(); + if ( c <= 0 ) + { + SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + break; + } + (*tag) += (char) c; + } + + if ( in->good() ) + { + // We now have something we presume to be a node of + // some sort. Identify it, and call the node to + // continue streaming. + TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); + + if ( node ) + { + node->StreamIn( in, tag ); + bool isElement = node->ToElement() != 0; + delete node; + node = 0; + + // If this is the root element, we're done. Parsing will be + // done by the >> operator. + if ( isElement ) + { + return; + } + } + else + { + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + } + } + // We should have returned sooner. + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); +} + +#endif + +const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) +{ + ClearError(); + + // Parse away, at the document level. Since a document + // contains nothing but other tags, most of what happens + // here is skipping white space. + if ( !p || !*p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + // Note that, for a document, this needs to come + // before the while space skip, so that parsing + // starts from the pointer we are given. + location.Clear(); + if ( prevData ) + { + location.row = prevData->cursor.row; + location.col = prevData->cursor.col; + } + else + { + location.row = 0; + location.col = 0; + } + TiXmlParsingData data( p, TabSize(), location.row, location.col ); + location = data.Cursor(); + + if ( encoding == TIXML_ENCODING_UNKNOWN ) + { + // Check for the Microsoft UTF-8 lead bytes. + const unsigned char* pU = (const unsigned char*)p; + if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 + && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 + && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) + { + encoding = TIXML_ENCODING_UTF8; + useMicrosoftBOM = true; + } + } + + p = SkipWhiteSpace( p, encoding ); + if ( !p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + while ( p && *p ) + { + TiXmlNode* node = Identify( p, encoding ); + if ( node ) + { + p = node->Parse( p, &data, encoding ); + LinkEndChild( node ); + } + else + { + break; + } + + // Did we get encoding info? + if ( encoding == TIXML_ENCODING_UNKNOWN + && node->ToDeclaration() ) + { + TiXmlDeclaration* dec = node->ToDeclaration(); + const char* enc = dec->Encoding(); + assert( enc ); + + if ( *enc == 0 ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice + else + encoding = TIXML_ENCODING_LEGACY; + } + + p = SkipWhiteSpace( p, encoding ); + } + + // Was this empty? + if ( !firstChild ) { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); + return 0; + } + + // All is well. + return p; +} + +void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + // The first error in a chain is more accurate - don't set again! + if ( error ) + return; + + assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); + error = true; + errorId = err; + errorDesc = errorString[ errorId ]; + + errorLocation.Clear(); + if ( pError && data ) + { + data->Stamp( pError, encoding ); + errorLocation = data->Cursor(); + } +} + + +TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) +{ + TiXmlNode* returnNode = 0; + + p = SkipWhiteSpace( p, encoding ); + if( !p || !*p || *p != '<' ) + { + return 0; + } + + p = SkipWhiteSpace( p, encoding ); + + if ( !p || !*p ) + { + return 0; + } + + // What is this thing? + // - Elements start with a letter or underscore, but xml is reserved. + // - Comments: "; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // [ 1475201 ] TinyXML parses entities in comments + // Oops - ReadText doesn't work, because we don't want to parse the entities. + // p = ReadText( p, &value, false, endTag, false, encoding ); + // + // from the XML spec: + /* + [Definition: Comments may appear anywhere in a document outside other markup; in addition, + they may appear within the document type declaration at places allowed by the grammar. + They are not part of the document's character data; an XML processor MAY, but need not, + make it possible for an application to retrieve the text of comments. For compatibility, + the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity + references MUST NOT be recognized within comments. + + An example of a comment: + + + */ + + value = ""; + // Keep all the white space. + while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) + { + value.append( p, 1 ); + ++p; + } + if ( p && *p ) + p += strlen( endTag ); + + return p; +} + + +const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) return 0; + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + // Read the name, the '=' and the value. + const char* pErr = p; + p = ReadName( p, &name, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); + return 0; + } + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p || *p != '=' ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + ++p; // skip '=' + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + const char* end; + const char SINGLE_QUOTE = '\''; + const char DOUBLE_QUOTE = '\"'; + + if ( *p == SINGLE_QUOTE ) + { + ++p; + end = "\'"; // single quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else if ( *p == DOUBLE_QUOTE ) + { + ++p; + end = "\""; // double quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else + { + // All attribute values should be in single or double quotes. + // But this is such a common error that the parser will try + // its best, even without them. + value = ""; + while ( p && *p // existence + && !IsWhiteSpace( *p ) // whitespace + && *p != '/' && *p != '>' ) // tag end + { + if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { + // [ 1451649 ] Attribute values with trailing quotes not handled correctly + // We did not have an opening quote but seem to have a + // closing one. Give up and throw an error. + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + value += *p; + ++p; + } + } + return p; +} + +#ifdef TIXML_USE_STL +void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->peek(); + if ( !cdata && (c == '<' ) ) + { + return; + } + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + (*tag) += (char) c; + in->get(); // "commits" the peek made above + + if ( cdata && c == '>' && tag->size() >= 3 ) { + size_t len = tag->size(); + if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { + // terminator of cdata. + return; + } + } + } +} +#endif + +const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + value = ""; + TiXmlDocument* document = GetDocument(); + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + + const char* const startTag = ""; + + if ( cdata || StringEqual( p, startTag, false, encoding ) ) + { + cdata = true; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // Keep all the white space, ignore the encoding, etc. + while ( p && *p + && !StringEqual( p, endTag, false, encoding ) + ) + { + value += *p; + ++p; + } + + TIXML_STRING dummy; + p = ReadText( p, &dummy, false, endTag, false, encoding ); + return p; + } + else + { + bool ignoreWhite = true; + + const char* end = "<"; + p = ReadText( p, &value, ignoreWhite, end, false, encoding ); + if ( p && *p ) + return p-1; // don't truncate the '<' + return 0; + } +} + +#ifdef TIXML_USE_STL +void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->get(); + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + (*tag) += (char) c; + + if ( c == '>' ) + { + // All is well. + return; + } + } +} +#endif + +const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) +{ + p = SkipWhiteSpace( p, _encoding ); + // Find the beginning, find the end, and look for + // the stuff in-between. + TiXmlDocument* document = GetDocument(); + if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); + return 0; + } + if ( data ) + { + data->Stamp( p, _encoding ); + location = data->Cursor(); + } + p += 5; + + version = ""; + encoding = ""; + standalone = ""; + + while ( p && *p ) + { + if ( *p == '>' ) + { + ++p; + return p; + } + + p = SkipWhiteSpace( p, _encoding ); + if ( StringEqual( p, "version", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + version = attrib.Value(); + } + else if ( StringEqual( p, "encoding", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + encoding = attrib.Value(); + } + else if ( StringEqual( p, "standalone", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + standalone = attrib.Value(); + } + else + { + // Read over whatever it is. + while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) + ++p; + } + } + return 0; +} + +bool TiXmlText::Blank() const +{ + for ( unsigned i=0; i + + The world has many languages + Мир имеет много языков + el mundo tiene muchos idiomas + 世界有ĺľĺ¤ščŻ­č¨€ + <Đ ŃŃŃкий название="name" ценноŃŃ‚ŃŚ="value"><имеет> + <汉语 ĺŤĺ­—="name" 价值="value">世界有ĺľĺ¤ščŻ­č¨€ + "Mëtæl!" + <ä>Umlaut Element + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml new file mode 100644 index 0000000..91a319d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/tinyxml/utf8testverify.xml @@ -0,0 +1,11 @@ + + + The world has many languages + Мир имеет много языков + el mundo tiene muchos idiomas + 世界有ĺľĺ¤ščŻ­č¨€ + <Đ ŃŃŃкий название="name" ценноŃŃ‚ŃŚ="value"><имеет> + <汉语 ĺŤĺ­—="name" 价值="value">世界有ĺľĺ¤ščŻ­č¨€ + "MĂ«tæl!" + <ä>Umlaut Element + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h new file mode 100644 index 0000000..4086469 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/lexical_cast.h @@ -0,0 +1,27 @@ +#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H +#define BOOST_REPLACEMENT_LEXICAL_CAST_H + +#include + +namespace boost +{ + + template T lexical_cast(const char* txt) + { + double result = atof(txt); + return result; + }; + + struct bad_lexical_cast + { + const char* what() + { + return ("bad lexical cast\n"); + } + + }; + +} //namespace boost + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp new file mode 100644 index 0000000..ed38915 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.cpp @@ -0,0 +1,28 @@ +#include "printf_console.h" +#include + +#include + + +void logError(const char* msg, const char* arg0, const char* arg1, const char* arg2) +{ + LOG << msg << " " << arg0 << " " << arg1 << " " << arg2 << std::endl; +} + +void logDebug(const char* msg, float v0, float v1) +{ + LOG << msg << " " << v0 << " " << v1 << std::endl; +}; +void logDebug(const char* msg, const char* msg1, const char* arg1) +{ + LOG << msg << " " << msg1 << " " << arg1 << std::endl; +} + +void logInform(const char* msg, const char* arg0) +{ + LOG << msg << " " << arg0 << std::endl; +} +void logWarn(const char* msg,int id, const char* arg0) +{ + LOG << msg << " " << id << " " << arg0 << std::endl; +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h new file mode 100644 index 0000000..247aab2 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/printf_console.h @@ -0,0 +1,13 @@ +#ifndef PRINTF_CONSOLE_H +#define PRINTF_CONSOLE_H + + +void logError(const char* msg="", const char* arg0="", const char* arg1="", const char* arg2=""); +void logDebug(const char* msg, float v0, float v1); +void logDebug(const char* msg, const char* msg1="", const char* arg1=""); +void logInform(const char* msg, const char* arg0=""); +void logWarn(const char* msg,int id, const char* arg0=""); + +#endif + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h new file mode 100644 index 0000000..5d29732 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/shared_ptr.h @@ -0,0 +1,210 @@ +/* +Bullet Continuous Collision Detection and Physics Library Maya Plugin +Copyright (c) 2008 Walt Disney Studios + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising +from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must +not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +Written by: Nicola Candussi + +Modified by Francisco Gochez +Dec 2011 - Added deferencing operator +*/ + +//my_shared_ptr + +#ifndef DYN_SHARED_PTR_H +#define DYN_SHARED_PTR_H + +#define DYN_SHARED_PTR_THREAD_SAFE + + +#include + +#ifdef _WIN32 +#include + +class my_shared_count { +public: + my_shared_count(): m_count(1) { } + ~my_shared_count() { } + + long increment() + { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + return InterlockedIncrement(&m_count); +#else + return ++m_count; +#endif + } + + long decrement() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + return InterlockedDecrement(&m_count); +#else + return ++m_count; +#endif + } + + long use_count() { return m_count; } + +private: + long m_count; +}; +#else //ifdef WIN32 + + +#include + +class my_shared_count { +public: + my_shared_count(): m_count(1) { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_init(&m_mutex, 0); +#endif + } + ~my_shared_count() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_destroy(&m_mutex); +#endif + } + + long increment() + { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_lock(&m_mutex); +#endif + long c = ++m_count; +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_unlock(&m_mutex); +#endif + return c; + } + + long decrement() { +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_lock(&m_mutex); +#endif + long c = --m_count; +#ifdef DYN_SHARED_PTR_THREAD_SAFE + pthread_mutex_unlock(&m_mutex); +#endif + return c; + } + + long use_count() { return m_count; } + +private: + long m_count; + mutable pthread_mutex_t m_mutex; +}; + +#endif + + +template +class my_shared_ptr +{ +public: + my_shared_ptr(): m_ptr(NULL), m_count(NULL) { } + my_shared_ptr(my_shared_ptr const& other): + m_ptr(other.m_ptr), + m_count(other.m_count) + { + if(other.m_count != NULL) other.m_count->increment(); + } + + template + my_shared_ptr(my_shared_ptr const& other): + m_ptr(other.m_ptr), + m_count(other.m_count) + { + if(other.m_count != NULL) other.m_count->increment(); + } + + my_shared_ptr(T const* other): m_ptr(const_cast(other)), m_count(NULL) + { + if(other != NULL) m_count = new my_shared_count; + } + + ~my_shared_ptr() + { + giveup_ownership(); + } + + void reset(T const* other) + { + if(m_ptr == other) return; + giveup_ownership(); + m_ptr = const_cast(other); + if(other != NULL) m_count = new my_shared_count; + else m_count = NULL; + } + + T* get() { return m_ptr; } + T const* get() const { return m_ptr; } + T* operator->() { return m_ptr; } + T const* operator->() const { return m_ptr; } + operator bool() const { return m_ptr != NULL; } + T& operator*() const + { + assert(m_ptr != 0); + return *m_ptr; + } + + bool operator<(my_shared_ptr const& rhs) const { return m_ptr < rhs.m_ptr; } + + my_shared_ptr& operator=(my_shared_ptr const& other) { + if(m_ptr == other.m_ptr) return *this; + giveup_ownership(); + m_ptr = other.m_ptr; + m_count = other.m_count; + if(other.m_count != NULL) m_count->increment(); + return *this; + } + + template + my_shared_ptr& operator=(my_shared_ptr& other) { + if(m_ptr == other.m_ptr) return *this; + giveup_ownership(); + m_ptr = other.m_ptr; + m_count = other.m_count; + if(other.m_count != NULL) m_count->increment(); + return *this; + } + +protected: + + template friend class my_shared_ptr; + void giveup_ownership() + { + if(m_count != NULL) { + if( m_count->decrement() == 0) { + delete m_ptr; + m_ptr = NULL; + delete m_count; + m_count = NULL; + } + } + } + +protected: + T *m_ptr; + my_shared_count *m_count; + +}; + + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp new file mode 100644 index 0000000..6c8a294 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/boost_replacement/string_split.cpp @@ -0,0 +1,253 @@ + + +#include +//#include +#include +#include +#include + +#include "string_split.h" + +namespace boost +{ + void split( std::vector&pieces, const std::string& vector_str, std::vector separators) + { + assert(separators.size()==1); + if (separators.size()==1) + { + char** strArray = str_split(vector_str.c_str(),separators[0].c_str()); + int numSubStr = str_array_len(strArray); + for (int i=0;i is_any_of(const char* seps) + { + std::vector strArray; + + int numSeps = strlen(seps); + for (int i=0;i +#include +#include + +namespace boost +{ + void split( std::vector&pieces, const std::string& vector_str, std::vector separators); + std::vector is_any_of(const char* seps); +}; + +///The string split C code is by Lars Wirzenius +///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char + + +/* Split a string into substrings. Return dynamic array of dynamically + allocated substrings, or NULL if there was an error. Caller is + expected to free the memory, for example with str_array_free. */ +char** str_split(const char* input, const char* sep); + +/* Free a dynamic array of dynamic strings. */ +void str_array_free(char** array); + +/* Return length of a NULL-delimited array of strings. */ +size_t str_array_len(char** array); + +#endif //STRING_SPLIT_H + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE new file mode 100644 index 0000000..e80920e --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/LICENSE @@ -0,0 +1,15 @@ +Software License Agreement (Apache License) + +Copyright 2011 John Hsu + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt new file mode 100644 index 0000000..4e3bff6 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/README.txt @@ -0,0 +1,7 @@ +The URDF (U-Robot Description Format) library + provides core data structures and a simple XML parsers + for populating the class data structures from an URDF file. + +For now, the details of the URDF specifications reside on + http://ros.org/wiki/urdf + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h new file mode 100644 index 0000000..336af0f --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h @@ -0,0 +1,63 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_PARSER_URDF_PARSER_H +#define URDF_PARSER_URDF_PARSER_H + +#include + +#include +#include "tinyxml/tinyxml.h" + +//#include + +#ifndef M_PI +#define M_PI 3.1415925438 +#endif //M_PI + + +#include + + + + +namespace urdf{ + + my_shared_ptr parseURDF(const std::string &xml_string); + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp new file mode 100644 index 0000000..67e9eb1 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/check_urdf.cpp @@ -0,0 +1,137 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" +#include +#include + +using namespace urdf; + +void printTree(my_shared_ptr link,int level = 0) +{ + level+=2; + int count = 0; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + for(int j=0;jname << std::endl; + // first grandchild + printTree(*child,level); + } + else + { + for(int j=0;jname << " has a null child!" << *child << std::endl; + } + } + +} + + +#define MSTRINGIFY(A) #A + + +const char* urdf_char = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + +); + + +int main(int argc, char** argv) +{ + + std::string xml_string; + + if (argc < 2){ + std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; + + xml_string = std::string(urdf_char); + + } else + { + + + std::fstream xml_file(argv[1], std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + } + + my_shared_ptr robot = parseURDF(xml_string); + if (!robot){ + std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + return -1; + } + std::cout << "robot name is: " << robot->getName() << std::endl; + + // get info from parser + std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; + // get root link + my_shared_ptr root_link=robot->getRoot(); + if (!root_link) return -1; + + std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; + + + // print entire tree + printTree(root_link); + return 0; +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp new file mode 100644 index 0000000..bde0c5f --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/joint.cpp @@ -0,0 +1,579 @@ +/********************************************************************* +* Software Ligcense Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#include +#include +#ifdef URDF_USE_BOOST +#include +#else +#include +#endif +#include + +#ifdef URDF_USE_CONSOLE_BRIDGE + #include +#else + #include "urdf/boost_replacement/printf_console.h" +#endif + +#include +#include + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) +{ + jd.clear(); + + // Get joint damping + const char* damping_str = config->Attribute("damping"); + if (damping_str == NULL){ + logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); + jd.damping = 0; + } + else + { + try + { + jd.damping = boost::lexical_cast(damping_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("damping value (%s) is not a float: %s",damping_str, e.what()); + return false; + } + } + + // Get joint friction + const char* friction_str = config->Attribute("friction"); + if (friction_str == NULL){ + logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); + jd.friction = 0; + } + else + { + try + { + jd.friction = boost::lexical_cast(friction_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("friction value (%s) is not a float: %s",friction_str, e.what()); + return false; + } + } + + if (damping_str == NULL && friction_str == NULL) + { + logError("joint dynamics element specified with no damping and no friction"); + return false; + } + else{ + logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); + return true; + } +} + +bool parseJointLimits(JointLimits &jl, TiXmlElement* config) +{ + jl.clear(); + + // Get lower joint limit + const char* lower_str = config->Attribute("lower"); + if (lower_str == NULL){ + logDebug("urdfdom.joint_limit: no lower, defaults to 0"); + jl.lower = 0; + } + else + { + try + { + jl.lower = boost::lexical_cast(lower_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("lower value (%s) is not a float: %s", lower_str, e.what()); + return false; + } + } + + // Get upper joint limit + const char* upper_str = config->Attribute("upper"); + if (upper_str == NULL){ + logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); + jl.upper = 0; + } + else + { + try + { + jl.upper = boost::lexical_cast(upper_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("upper value (%s) is not a float: %s",upper_str, e.what()); + return false; + } + } + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str == NULL){ + logError("joint limit: no effort"); + return false; + } + else + { + try + { + jl.effort = boost::lexical_cast(effort_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("effort value (%s) is not a float: %s",effort_str, e.what()); + return false; + } + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str == NULL){ + logError("joint limit: no velocity"); + return false; + } + else + { + try + { + jl.velocity = boost::lexical_cast(velocity_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointSafety(JointSafety &js, TiXmlElement* config) +{ + js.clear(); + + // Get soft_lower_limit joint limit + const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); + if (soft_lower_limit_str == NULL) + { + logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); + js.soft_lower_limit = 0; + } + else + { + try + { + js.soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); + return false; + } + } + + // Get soft_upper_limit joint limit + const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); + if (soft_upper_limit_str == NULL) + { + logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); + js.soft_upper_limit = 0; + } + else + { + try + { + js.soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); + return false; + } + } + + // Get k_position_ safety "position" gain - not exactly position gain + const char* k_position_str = config->Attribute("k_position"); + if (k_position_str == NULL) + { + logDebug("urdfdom.joint_safety: no k_position, using default value"); + js.k_position = 0; + } + else + { + try + { + js.k_position = boost::lexical_cast(k_position_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); + return false; + } + } + // Get k_velocity_ safety velocity gain + const char* k_velocity_str = config->Attribute("k_velocity"); + if (k_velocity_str == NULL) + { + logError("joint safety: no k_velocity"); + return false; + } + else + { + try + { + js.k_velocity = boost::lexical_cast(k_velocity_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) +{ + jc.clear(); + + // Get rising edge position + const char* rising_position_str = config->Attribute("rising"); + if (rising_position_str == NULL) + { + logDebug("urdfdom.joint_calibration: no rising, using default value"); + jc.rising.reset(0); + } + else + { + try + { + jc.rising.reset(new double(boost::lexical_cast(rising_position_str))); + } + catch (boost::bad_lexical_cast &e) + { + logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); + return false; + } + } + + // Get falling edge position + const char* falling_position_str = config->Attribute("falling"); + if (falling_position_str == NULL) + { + logDebug("urdfdom.joint_calibration: no falling, using default value"); + jc.falling.reset(0); + } + else + { + try + { + jc.falling.reset(new double(boost::lexical_cast(falling_position_str))); + } + catch (boost::bad_lexical_cast &e) + { + logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJointMimic(JointMimic &jm, TiXmlElement* config) +{ + jm.clear(); + + // Get name of joint to mimic + const char* joint_name_str = config->Attribute("joint"); + + if (joint_name_str == NULL) + { + logError("joint mimic: no mimic joint specified"); + return false; + } + else + jm.joint_name = joint_name_str; + + // Get mimic multiplier + const char* multiplier_str = config->Attribute("multiplier"); + + if (multiplier_str == NULL) + { + logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); + jm.multiplier = 1; + } + else + { + try + { + jm.multiplier = boost::lexical_cast(multiplier_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); + return false; + } + } + + + // Get mimic offset + const char* offset_str = config->Attribute("offset"); + if (offset_str == NULL) + { + logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); + jm.offset = 0; + } + else + { + try + { + jm.offset = boost::lexical_cast(offset_str); + } + catch (boost::bad_lexical_cast &e) + { + logError("offset value (%s) is not a float: %s",offset_str, e.what()); + return false; + } + } + + return true; +} + +bool parseJoint(Joint &joint, TiXmlElement* config) +{ + joint.clear(); + + // Get Joint Name + const char *name = config->Attribute("name"); + if (!name) + { + logError("unnamed joint found"); + return false; + } + joint.name = name; + + // Get transform from Parent Link to Joint Frame + TiXmlElement *origin_xml = config->FirstChildElement("origin"); + if (!origin_xml) + { + logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); + joint.parent_to_joint_origin_transform.clear(); + } + else + { + if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) + { + joint.parent_to_joint_origin_transform.clear(); + logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); + return false; + } + } + + // Get Parent Link + TiXmlElement *parent_xml = config->FirstChildElement("parent"); + if (parent_xml) + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); + } + else + { + joint.parent_link_name = std::string(pname); + } + } + + // Get Child Link + TiXmlElement *child_xml = config->FirstChildElement("child"); + if (child_xml) + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); + } + else + { + joint.child_link_name = std::string(pname); + } + } + + // Get Joint type + const char* type_char = config->Attribute("type"); + if (!type_char) + { + logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); + return false; + } + + std::string type_str = type_char; + if (type_str == "planar") + joint.type = Joint::PLANAR; + else if (type_str == "floating") + joint.type = Joint::FLOATING; + else if (type_str == "revolute") + joint.type = Joint::REVOLUTE; + else if (type_str == "continuous") + joint.type = Joint::CONTINUOUS; + else if (type_str == "prismatic") + joint.type = Joint::PRISMATIC; + else if (type_str == "fixed") + joint.type = Joint::FIXED; + else + { + logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); + return false; + } + + // Get Joint Axis + if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); + joint.axis = Vector3(1.0, 0.0, 0.0); + } + else{ + if (axis_xml->Attribute("xyz")){ + try { + joint.axis.init(axis_xml->Attribute("xyz")); + } + catch (ParseError &e) { + joint.axis.clear(); + logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); + return false; + } + } + } + } + + // Get limit + TiXmlElement *limit_xml = config->FirstChildElement("limit"); + if (limit_xml) + { + joint.limits.reset(new JointLimits()); + if (!parseJointLimits(*joint.limits, limit_xml)) + { + logError("Could not parse limit element for joint [%s]", joint.name.c_str()); + joint.limits.reset(0); + return false; + } + } + else if (joint.type == Joint::REVOLUTE) + { + logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); + return false; + } + else if (joint.type == Joint::PRISMATIC) + { + logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); + return false; + } + + // Get safety + TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); + if (safety_xml) + { + joint.safety.reset(new JointSafety()); + if (!parseJointSafety(*joint.safety, safety_xml)) + { + logError("Could not parse safety element for joint [%s]", joint.name.c_str()); + joint.safety.reset(0); + return false; + } + } + + // Get calibration + TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); + if (calibration_xml) + { + joint.calibration.reset(new JointCalibration()); + if (!parseJointCalibration(*joint.calibration, calibration_xml)) + { + logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); + joint.calibration.reset(0); + return false; + } + } + + // Get Joint Mimic + TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); + if (mimic_xml) + { + joint.mimic.reset(new JointMimic()); + if (!parseJointMimic(*joint.mimic, mimic_xml)) + { + logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); + joint.mimic.reset(0); + return false; + } + } + + // Get Dynamics + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + joint.dynamics.reset(new JointDynamics()); + if (!parseJointDynamics(*joint.dynamics, prop_xml)) + { + logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); + joint.dynamics.reset(0); + return false; + } + } + + return true; +} + + + + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp new file mode 100644 index 0000000..a224146 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/link.cpp @@ -0,0 +1,505 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + + +#include +#include +//#include +//#include +#ifdef URDF_USE_BOOST +#include +#else +#include +#endif + +#include +#include +#ifdef URDF_USE_CONSOLE_BRIDGE +#include +#else +#include "urdf/boost_replacement/printf_console.h" +#endif + + + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) +{ + bool has_rgb = false; + bool has_filename = false; + + material.clear(); + + if (!config->Attribute("name")) + { + logError("Material must contain a name attribute"); + return false; + } + + material.name = config->Attribute("name"); + + // texture + TiXmlElement *t = config->FirstChildElement("texture"); + if (t) + { + if (t->Attribute("filename")) + { + material.texture_filename = t->Attribute("filename"); + has_filename = true; + } + } + + // color + TiXmlElement *c = config->FirstChildElement("color"); + if (c) + { + if (c->Attribute("rgba")) { + + try { + material.color.init(c->Attribute("rgba")); + has_rgb = true; + } + catch (ParseError &e) { + material.color.clear(); + logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); + } + } + } + + if (!has_rgb && !has_filename) { + if (!only_name_is_ok) // no need for an error if only name is ok + { + if (!has_rgb) logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); + if (!has_filename) logError(std::string("Material ["+material.name+"] not defined in file").c_str()); + } + return false; + } + return true; +} + + +bool parseSphere(Sphere &s, TiXmlElement *c) +{ + s.clear(); + + s.type = Geometry::SPHERE; + if (!c->Attribute("radius")) + { + logError("Sphere shape must have a radius attribute"); + return false; + } + + try + { + s.radius = boost::lexical_cast(c->Attribute("radius")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what(); + // logError(stm.str().c_str()); + logError("radius issue"); + return false; + } + + return true; +} + +bool parseBox(Box &b, TiXmlElement *c) +{ + b.clear(); + + b.type = Geometry::BOX; + if (!c->Attribute("size")) + { + logError("Box shape has no size attribute"); + return false; + } + try + { + b.dim.init(c->Attribute("size")); + } + catch (ParseError &e) + { + b.dim.clear(); + logError(e.what()); + return false; + } + return true; +} + +bool parseCylinder(Cylinder &y, TiXmlElement *c) +{ + y.clear(); + + y.type = Geometry::CYLINDER; + if (!c->Attribute("length") || + !c->Attribute("radius")) + { + logError("Cylinder shape must have both length and radius attributes"); + return false; + } + + try + { + y.length = boost::lexical_cast(c->Attribute("length")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "length [" << c->Attribute("length") << "] is not a valid float"; + //logError(stm.str().c_str()); + logError("length"); + return false; + } + + try + { + y.radius = boost::lexical_cast(c->Attribute("radius")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + //logError(stm.str().c_str()); + logError("radius"); + return false; + } + return true; +} + + +bool parseMesh(Mesh &m, TiXmlElement *c) +{ + m.clear(); + + m.type = Geometry::MESH; + if (!c->Attribute("filename")) { + logError("Mesh must contain a filename attribute"); + return false; + } + + m.filename = c->Attribute("filename"); + + if (c->Attribute("scale")) { + try { + m.scale.init(c->Attribute("scale")); + } + catch (ParseError &e) { + m.scale.clear(); + logError("Mesh scale was specified, but could not be parsed: %s", e.what()); + return false; + } + } + else + { + m.scale.x = m.scale.y = m.scale.z = 1; + } + return true; +} + +my_shared_ptr parseGeometry(TiXmlElement *g) +{ + my_shared_ptr geom; + if (!g) return geom; + + TiXmlElement *shape = g->FirstChildElement(); + if (!shape) + { + logError("Geometry tag contains no child element."); + return geom; + } + + const std::string type_name = shape->ValueTStr().c_str(); + if (type_name == "sphere") + { + Sphere *s = new Sphere(); + geom.reset(s); + if (parseSphere(*s, shape)) + return geom; + } + else if (type_name == "box") + { + Box *b = new Box(); + geom.reset(b); + if (parseBox(*b, shape)) + return geom; + } + else if (type_name == "cylinder") + { + Cylinder *c = new Cylinder(); + geom.reset(c); + if (parseCylinder(*c, shape)) + return geom; + } + else if (type_name == "mesh") + { + Mesh *m = new Mesh(); + geom.reset(m); + if (parseMesh(*m, shape)) + return geom; + } + else + { + logError("Unknown geometry type '%s'", type_name.c_str()); + return geom; + } + + return my_shared_ptr(); +} + +bool parseInertial(Inertial &i, TiXmlElement *config) +{ + i.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(i.origin, o)) + return false; + } + + TiXmlElement *mass_xml = config->FirstChildElement("mass"); + if (!mass_xml) + { + logError("Inertial element must have a mass element"); + return false; + } + if (!mass_xml->Attribute("value")) + { + logError("Inertial: mass element must have value attribute"); + return false; + } + + try + { + i.mass = boost::lexical_cast(mass_xml->Attribute("value")); + } + catch (boost::bad_lexical_cast &e) + { + // std::stringstream stm; + // stm << "Inertial: mass [" << mass_xml->Attribute("value") + // << "] is not a float"; + //logError(stm.str().c_str()); + logError("Inertial mass issue"); + return false; + } + + TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); + if (!inertia_xml) + { + logError("Inertial element must have inertia element"); + return false; + } + if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && + inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && + inertia_xml->Attribute("izz"))) + { + logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); + return false; + } + try + { + i.ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); + i.ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); + i.ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); + i.iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); + i.iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); + i.izz = boost::lexical_cast(inertia_xml->Attribute("izz")); + } + catch (boost::bad_lexical_cast &e) + { + /* std::stringstream stm; + stm << "Inertial: one of the inertia elements is not a valid double:" + << " ixx [" << inertia_xml->Attribute("ixx") << "]" + << " ixy [" << inertia_xml->Attribute("ixy") << "]" + << " ixz [" << inertia_xml->Attribute("ixz") << "]" + << " iyy [" << inertia_xml->Attribute("iyy") << "]" + << " iyz [" << inertia_xml->Attribute("iyz") << "]" + << " izz [" << inertia_xml->Attribute("izz") << "]"; + logError(stm.str().c_str()); + */ + logError("Inertia error"); + + return false; + } + return true; +} + +bool parseVisual(Visual &vis, TiXmlElement *config) +{ + vis.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(vis.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + vis.geometry = parseGeometry(geom); + if (!vis.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + vis.name = name_char; + + // Material + TiXmlElement *mat = config->FirstChildElement("material"); + if (mat) { + // get material name + if (!mat->Attribute("name")) { + logError("Visual material must contain a name attribute"); + return false; + } + vis.material_name = mat->Attribute("name"); + + // try to parse material element in place + vis.material.reset(new Material()); + if (!parseMaterial(*vis.material, mat, true)) + { + logDebug("urdfdom: material has only name, actual material definition may be in the model"); + } + } + + return true; +} + +bool parseCollision(Collision &col, TiXmlElement* config) +{ + col.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(col.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + col.geometry = parseGeometry(geom); + if (!col.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + col.name = name_char; + + return true; +} + +bool parseLink(Link &link, TiXmlElement* config) +{ + + link.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the link."); + return false; + } + link.name = std::string(name_char); + + // Inertial (optional) + TiXmlElement *i = config->FirstChildElement("inertial"); + if (i) + { + link.inertial.reset(new Inertial()); + if (!parseInertial(*link.inertial, i)) + { + logError("Could not parse inertial element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Multiple Visuals (optional) + for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + { + + my_shared_ptr vis; + vis.reset(new Visual()); + if (parseVisual(*vis, vis_xml)) + { + link.visual_array.push_back(vis); + } + else + { + vis.reset(0); + logError("Could not parse visual element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Visual (optional) + // Assign the first visual to the .visual ptr, if it exists + if (!link.visual_array.empty()) + link.visual = link.visual_array[0]; + + // Multiple Collisions (optional) + for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + { + my_shared_ptr col; + col.reset(new Collision()); + if (parseCollision(*col, col_xml)) + { + link.collision_array.push_back(col); + } + else + { + col.reset(0); + logError("Could not parse collision element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Collision (optional) + // Assign the first collision to the .collision ptr, if it exists + if (!link.collision_array.empty()) + link.collision = link.collision_array[0]; + return true; + +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp new file mode 100644 index 0000000..e8562d0 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/model.cpp @@ -0,0 +1,240 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ +//#include +#include +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" +#ifdef URDF_USE_CONSOLE_BRIDGE + #include +#else + #include "urdf/boost_replacement/printf_console.h" +#endif + +namespace urdf{ + +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); +bool parseLink(Link &link, TiXmlElement *config); +bool parseJoint(Joint &joint, TiXmlElement *config); + + +my_shared_ptr parseURDF(const std::string &xml_string) +{ + my_shared_ptr model(new ModelInterface); + model->clear(); + + TiXmlDocument xml_doc; + xml_doc.Parse(xml_string.c_str()); + if (xml_doc.Error()) + { + logError(xml_doc.ErrorDesc()); + xml_doc.ClearError(); + model.reset(0); + return model; + } + + TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); + if (!robot_xml) + { + logError("Could not find the 'robot' element in the xml file"); + model.reset(0); + return model; + } + + // Get robot name + const char *name = robot_xml->Attribute("name"); + if (!name) + { + logError("No name given for the robot."); + model.reset(0); + return model; + } + model->name_ = std::string(name); + + // Get all Material elements + for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + { + my_shared_ptr material; + material.reset(new Material); + + try { + parseMaterial(*material, material_xml, false); // material needs to be fully defined here + if (model->getMaterial(material->name)) + { + logError("material '%s' is not unique.", material->name.c_str()); + material.reset(0); + model.reset(0); + return model; + } + else + { + model->materials_.insert(make_pair(material->name,material)); + logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); + } + } + catch (ParseError &e) { + logError("material xml is not initialized correctly"); + material.reset(0); + model.reset(0); + return model; + } + } + + // Get all Link elements + for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + { + my_shared_ptr link; + link.reset(new Link); + model->m_numLinks++; + + try { + parseLink(*link, link_xml); + if (model->getLink(link->name)) + { + logError("link '%s' is not unique.", link->name.c_str()); + model.reset(0); + return model; + } + else + { + // set link visual material + logDebug("urdfdom: setting link '%s' material", link->name.c_str()); + if (link->visual) + { + if (!link->visual->material_name.empty()) + { + if (model->getMaterial(link->visual->material_name)) + { + logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); + link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); + } + else + { + if (link->visual->material) + { + logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); + model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); + } + else + { + logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); + model.reset(0); + return model; + } + } + } + } + + model->links_.insert(make_pair(link->name,link)); + logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); + } + } + catch (ParseError &e) { + logError("link xml is not initialized correctly"); + model.reset(0); + return model; + } + } + if (model->links_.empty()){ + logError("No link elements found in urdf file"); + model.reset(0); + return model; + } + + // Get all Joint elements + for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + my_shared_ptr joint; + joint.reset(new Joint); + model->m_numJoints++; + + if (parseJoint(*joint, joint_xml)) + { + if (model->getJoint(joint->name)) + { + logError("joint '%s' is not unique.", joint->name.c_str()); + model.reset(0); + return model; + } + else + { + model->joints_.insert(make_pair(joint->name,joint)); + logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); + } + } + else + { + logError("joint xml is not initialized correctly"); + model.reset(0); + return model; + } + } + + + // every link has children links and joints, but no parents, so we create a + // local convenience data structure for keeping child->parent relations + std::map parent_link_tree; + parent_link_tree.clear(); + + // building tree: name mapping + try + { + model->initTree(parent_link_tree); + } + catch(ParseError &e) + { + logError("Failed to build tree: %s", e.what()); + model.reset(0); + return model; + } + + // find the root link + try + { + model->initRoot(parent_link_tree); + } + catch(ParseError &e) + { + logError("Failed to find root link: %s", e.what()); + model.reset(0); + return model; + } + + return model; +} + + + +} + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp new file mode 100644 index 0000000..e90247c --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/pose.cpp @@ -0,0 +1,91 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen, John Hsu */ + + +#include +#include +#include +//#include +#include + +#ifdef URDF_USE_CONSOLE_BRIDGE +#include +#else +#include "urdf/boost_replacement/printf_console.h" +#endif + +#include +#include + + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml) +{ + pose.clear(); + if (xml) + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str != NULL) + { + try { + pose.position.init(xyz_str); + } + catch (ParseError &e) { + logError(e.what()); + return false; + } + } + + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str != NULL) + { + try { + pose.rotation.init(rpy_str); + } + catch (ParseError &e) { + logError(e.what()); + return false; + } + } + } + return true; +} + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp new file mode 100644 index 0000000..4980e17 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp @@ -0,0 +1,85 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseTwist(Twist &twist, TiXmlElement* xml) +{ + twist.clear(); + if (xml) + { + const char* linear_char = xml->Attribute("linear"); + if (linear_char != NULL) + { + try { + twist.linear.init(linear_char); + } + catch (ParseError &e) { + twist.linear.clear(); + logError("Malformed linear string [%s]: %s", linear_char, e.what()); + return false; + } + } + + const char* angular_char = xml->Attribute("angular"); + if (angular_char != NULL) + { + try { + twist.angular.init(angular_char); + } + catch (ParseError &e) { + twist.angular.clear(); + logError("Malformed angular [%s]: %s", angular_char, e.what()); + return false; + } + } + } + return true; +} + +} + + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp new file mode 100644 index 0000000..f30b845 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp @@ -0,0 +1,154 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseModelState(ModelState &ms, TiXmlElement* config) +{ + ms.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the model_state."); + return false; + } + ms.name = std::string(name_char); + + const char *time_stamp_char = config->Attribute("time_stamp"); + if (time_stamp_char) + { + try { + double sec = boost::lexical_cast(time_stamp_char); + ms.time_stamp.set(sec); + } + catch (boost::bad_lexical_cast &e) { + logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); + return false; + } + } + + TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); + if (joint_state_elem) + { + boost::shared_ptr joint_state; + joint_state.reset(new JointState()); + + const char *joint_char = joint_state_elem->Attribute("joint"); + if (joint_char) + joint_state->joint = std::string(joint_char); + else + { + logError("No joint name given for the model_state."); + return false; + } + + // parse position + const char *position_char = joint_state_elem->Attribute("position"); + if (position_char) + { + + std::vector pieces; + boost::split( pieces, position_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->position.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("position element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // parse velocity + const char *velocity_char = joint_state_elem->Attribute("velocity"); + if (velocity_char) + { + + std::vector pieces; + boost::split( pieces, velocity_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->velocity.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // parse effort + const char *effort_char = joint_state_elem->Attribute("effort"); + if (effort_char) + { + + std::vector pieces; + boost::split( pieces, effort_char, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->effort.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) { + throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // add to vector + ms.joint_states.push_back(joint_state); + } +}; + + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp new file mode 100644 index 0000000..85a886d --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp @@ -0,0 +1,364 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseCamera(Camera &camera, TiXmlElement* config) +{ + camera.clear(); + camera.type = VisualSensor::CAMERA; + + TiXmlElement *image = config->FirstChildElement("image"); + if (image) + { + const char* width_char = image->Attribute("width"); + if (width_char) + { + try + { + camera.width = boost::lexical_cast(width_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image width attribute"); + return false; + } + + const char* height_char = image->Attribute("height"); + if (height_char) + { + try + { + camera.height = boost::lexical_cast(height_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image height attribute"); + return false; + } + + const char* format_char = image->Attribute("format"); + if (format_char) + camera.format = std::string(format_char); + else + { + logError("Camera sensor needs an image format attribute"); + return false; + } + + const char* hfov_char = image->Attribute("hfov"); + if (hfov_char) + { + try + { + camera.hfov = boost::lexical_cast(hfov_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image hfov attribute"); + return false; + } + + const char* near_char = image->Attribute("near"); + if (near_char) + { + try + { + camera.near = boost::lexical_cast(near_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image near attribute"); + return false; + } + + const char* far_char = image->Attribute("far"); + if (far_char) + { + try + { + camera.far = boost::lexical_cast(far_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); + return false; + } + } + else + { + logError("Camera sensor needs an image far attribute"); + return false; + } + + } + else + { + logError("Camera sensor has no element"); + return false; + } + return true; +} + +bool parseRay(Ray &ray, TiXmlElement* config) +{ + ray.clear(); + ray.type = VisualSensor::RAY; + + TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + if (horizontal) + { + const char* samples_char = horizontal->Attribute("samples"); + if (samples_char) + { + try + { + ray.horizontal_samples = boost::lexical_cast(samples_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = horizontal->Attribute("resolution"); + if (resolution_char) + { + try + { + ray.horizontal_resolution = boost::lexical_cast(resolution_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); + return false; + } + } + + const char* min_angle_char = horizontal->Attribute("min_angle"); + if (min_angle_char) + { + try + { + ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); + return false; + } + } + + const char* max_angle_char = horizontal->Attribute("max_angle"); + if (max_angle_char) + { + try + { + ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); + return false; + } + } + } + + TiXmlElement *vertical = config->FirstChildElement("vertical"); + if (vertical) + { + const char* samples_char = vertical->Attribute("samples"); + if (samples_char) + { + try + { + ray.vertical_samples = boost::lexical_cast(samples_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = vertical->Attribute("resolution"); + if (resolution_char) + { + try + { + ray.vertical_resolution = boost::lexical_cast(resolution_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); + return false; + } + } + + const char* min_angle_char = vertical->Attribute("min_angle"); + if (min_angle_char) + { + try + { + ray.vertical_min_angle = boost::lexical_cast(min_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); + return false; + } + } + + const char* max_angle_char = vertical->Attribute("max_angle"); + if (max_angle_char) + { + try + { + ray.vertical_max_angle = boost::lexical_cast(max_angle_char); + } + catch (boost::bad_lexical_cast &e) + { + logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); + return false; + } + } + } +} + +boost::shared_ptr parseVisualSensor(TiXmlElement *g) +{ + boost::shared_ptr visual_sensor; + + // get sensor type + TiXmlElement *sensor_xml; + if (g->FirstChildElement("camera")) + { + Camera *camera = new Camera(); + visual_sensor.reset(camera); + sensor_xml = g->FirstChildElement("camera"); + if (!parseCamera(*camera, sensor_xml)) + visual_sensor.reset(); + } + else if (g->FirstChildElement("ray")) + { + Ray *ray = new Ray(); + visual_sensor.reset(ray); + sensor_xml = g->FirstChildElement("ray"); + if (!parseRay(*ray, sensor_xml)) + visual_sensor.reset(); + } + else + { + logError("No know sensor types [camera|ray] defined in block"); + } + return visual_sensor; +} + + +bool parseSensor(Sensor &sensor, TiXmlElement* config) +{ + sensor.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + logError("No name given for the sensor."); + return false; + } + sensor.name = std::string(name_char); + + // parse parent_link_name + const char *parent_link_name_char = config->Attribute("parent_link_name"); + if (!parent_link_name_char) + { + logError("No parent_link_name given for the sensor."); + return false; + } + sensor.parent_link_name = std::string(parent_link_name_char); + + // parse origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(sensor.origin, o)) + return false; + } + + // parse sensor + sensor.sensor = parseVisualSensor(config); + return true; +} + + +} + + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp new file mode 100644 index 0000000..ddc27c5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp @@ -0,0 +1,71 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace urdf{ + +bool parseWorld(World &world, TiXmlElement* config) +{ + + // to be implemented + + return true; +} + +bool exportWorld(World &world, TiXmlElement* xml) +{ + TiXmlElement * world_xml = new TiXmlElement("world"); + world_xml->SetAttribute("name", world.name); + + // to be implemented + // exportModels(*world.models, world_xml); + + xml->LinkEndChild(world_xml); + + return true; +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp new file mode 100644 index 0000000..d835eb3 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp @@ -0,0 +1,20 @@ +#include "urdf_parser/urdf_parser.h" +#include +#include + +int main(int argc, char** argv){ + while (true){ + std::string xml_string; + std::fstream xml_file(argv[1], std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + + + urdf::parseURDF(xml_string); + } +} diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE new file mode 100644 index 0000000..e80920e --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE @@ -0,0 +1,15 @@ +Software License Agreement (Apache License) + +Copyright 2011 John Hsu + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt new file mode 100644 index 0000000..6a841d5 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt @@ -0,0 +1,6 @@ +The URDF (U-Robot Description Format) headers + provides core data structure headers for URDF. + +For now, the details of the URDF specifications reside on + http://ros.org/wiki/urdf + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h new file mode 100644 index 0000000..24222f1 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h @@ -0,0 +1,53 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +// URDF exceptions +#ifndef URDF_INTERFACE_EXCEPTION_H_ +#define URDF_INTERFACE_EXCEPTION_H_ + +#include +#include + +namespace urdf +{ + +class ParseError: public std::runtime_error +{ +public: + ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h new file mode 100644 index 0000000..9c15dd7 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h @@ -0,0 +1,101 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Josh Faust */ + +#ifndef URDF_INTERFACE_COLOR_H +#define URDF_INTERFACE_COLOR_H + +#include +#include +#include +//#include +//#include + +namespace urdf +{ + +class Color +{ +public: + Color() {this->clear();}; + float r; + float g; + float b; + float a; + + void clear() + { + r = g = b = 0.0f; + a = 1.0f; + } + bool init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector rgba; + + boost::split( pieces, vector_str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + try + { + rgba.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + return false; + } + } + } + + if (rgba.size() != 4) + { + return false; + } + this->r = rgba[0]; + this->g = rgba[1]; + this->b = rgba[2]; + this->a = rgba[3]; + + return true; + }; +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h new file mode 100644 index 0000000..cd889dc --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h @@ -0,0 +1,234 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_JOINT_H +#define URDF_INTERFACE_JOINT_H + +#include +#include +#ifdef URDF_USE_BOOST +#include +#define my_shared_ptr my_shared_ptr +#else +#include +#endif + +#include + + +namespace urdf{ + +class Link; + +class JointDynamics +{ +public: + JointDynamics() { this->clear(); }; + double damping; + double friction; + + void clear() + { + damping = 0; + friction = 0; + }; +}; + +class JointLimits +{ +public: + JointLimits() { this->clear(); }; + double lower; + double upper; + double effort; + double velocity; + + void clear() + { + lower = 0; + upper = 0; + effort = 0; + velocity = 0; + }; +}; + +/// \brief Parameters for Joint Safety Controllers +class JointSafety +{ +public: + /// clear variables on construction + JointSafety() { this->clear(); }; + + /// + /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. + /// + /// Basic safety controller operation is as follows + /// + /// current safety controllers will take effect on joints outside the position range below: + /// + /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, + /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] + /// + /// if (joint_position is outside of the position range above) + /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) + /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) + /// else + /// velocity_limit_min = -JointLimits::velocity + /// velocity_limit_max = JointLimits::velocity + /// + /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, + /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] + /// + /// if (joint_velocity is outside of the velocity range above) + /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) + /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) + /// else + /// effort_limit_min = -JointLimits::effort + /// effort_limit_max = JointLimits::effort + /// + /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] + /// + /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits + /// + double soft_upper_limit; + double soft_lower_limit; + double k_position; + double k_velocity; + + void clear() + { + soft_upper_limit = 0; + soft_lower_limit = 0; + k_position = 0; + k_velocity = 0; + }; +}; + + +class JointCalibration +{ +public: + JointCalibration() { this->clear(); }; + double reference_position; + my_shared_ptr rising, falling; + + void clear() + { + reference_position = 0; + }; +}; + +class JointMimic +{ +public: + JointMimic() { this->clear(); }; + double offset; + double multiplier; + std::string joint_name; + + void clear() + { + offset = 0.0; + multiplier = 0.0; + joint_name.clear(); + }; +}; + + +class Joint +{ +public: + + Joint() { this->clear(); }; + + std::string name; + enum + { + UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED + } type; + + /// \brief type_ meaning of axis_ + /// ------------------------------------------------------ + /// UNKNOWN unknown type + /// REVOLUTE rotation axis + /// PRISMATIC translation axis + /// FLOATING N/A + /// PLANAR plane normal axis + /// FIXED N/A + Vector3 axis; + + /// child Link element + /// child link frame is the same as the Joint frame + std::string child_link_name; + + /// parent Link element + /// origin specifies the transform from Parent Link to Joint Frame + std::string parent_link_name; + /// transform from Parent Link frame to Joint frame + Pose parent_to_joint_origin_transform; + + /// Joint Dynamics + my_shared_ptr dynamics; + + /// Joint Limits + my_shared_ptr limits; + + /// Unsupported Hidden Feature + my_shared_ptr safety; + + /// Unsupported Hidden Feature + my_shared_ptr calibration; + + /// Option to Mimic another Joint + my_shared_ptr mimic; + + void clear() + { + this->axis.clear(); + this->child_link_name.clear(); + this->parent_link_name.clear(); + this->parent_to_joint_origin_transform.clear(); + this->dynamics.reset(0); + this->limits.reset(0); + this->safety.reset(0); + this->calibration.reset(0); + this->type = UNKNOWN; + }; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h new file mode 100644 index 0000000..22e64cf --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h @@ -0,0 +1,262 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_LINK_H +#define URDF_INTERFACE_LINK_H + +#include +#include +#include + + +#ifdef URDF_USE_BOOST + #include + #include +#else + #include +#endif + +#include "joint.h" +#include "color.h" +#include "pose.h" + +namespace urdf{ + +class Geometry +{ +public: + enum {SPHERE, BOX, CYLINDER, MESH} type; + + virtual ~Geometry(void) + { + } +}; + +class Sphere : public Geometry +{ +public: + Sphere() { this->clear(); type = SPHERE; }; + double radius; + + void clear() + { + radius = 0; + }; +}; + +class Box : public Geometry +{ +public: + Box() { this->clear(); type = BOX; } + Vector3 dim; + + void clear() + { + this->dim.clear(); + }; +}; + +class Cylinder : public Geometry +{ +public: + Cylinder() { this->clear(); type = CYLINDER; }; + double length; + double radius; + + void clear() + { + length = 0; + radius = 0; + }; +}; + +class Mesh : public Geometry +{ +public: + Mesh() { this->clear(); type = MESH; }; + std::string filename; + Vector3 scale; + + void clear() + { + filename.clear(); + // default scale + scale.x = 1; + scale.y = 1; + scale.z = 1; + }; +}; + +class Material +{ +public: + Material() { this->clear(); }; + std::string name; + std::string texture_filename; + Color color; + + void clear() + { + color.clear(); + texture_filename.clear(); + name.clear(); + }; +}; + +class Inertial +{ +public: + Inertial() { this->clear(); }; + Pose origin; + double mass; + double ixx,ixy,ixz,iyy,iyz,izz; + + void clear() + { + origin.clear(); + mass = 0; + ixx = ixy = ixz = iyy = iyz = izz = 0; + }; +}; + +class Visual +{ +public: + Visual() { this->clear(); }; + Pose origin; + my_shared_ptr geometry; + + std::string material_name; + my_shared_ptr material; + + void clear() + { + origin.clear(); + material_name.clear(); + material.reset(0); + geometry.reset(0); + name.clear(); + }; + + std::string name; +}; + +class Collision +{ +public: + Collision() { this->clear(); }; + Pose origin; + my_shared_ptr geometry; + + void clear() + { + origin.clear(); + geometry.reset(0); + name.clear(); + }; + + std::string name; + +}; + + +class Link +{ +public: + Link() { this->clear(); }; + + std::string name; + + /// inertial element + my_shared_ptr inertial; + + /// visual element + my_shared_ptr visual; + + /// collision element + my_shared_ptr collision; + + /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) + std::vector > collision_array; + + /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array) + std::vector > visual_array; + + /// Parent Joint element + /// explicitly stating "parent" because we want directional-ness for tree structure + /// every link can have one parent + my_shared_ptr parent_joint; + + std::vector > child_joints; + std::vector > child_links; + + mutable int m_link_index; + + const Link* getParent() const + {return parent_link_;} + + void setParent(const my_shared_ptr &parent) + { + parent_link_ = parent.get(); + } + + void clear() + { + this->name.clear(); + this->inertial.reset(0); + this->visual.reset(0); + this->collision.reset(0); + this->parent_joint.reset(0); + this->child_joints.clear(); + this->child_links.clear(); + this->collision_array.clear(); + this->visual_array.clear(); + this->m_link_index=-1; + this->parent_link_ = NULL; + }; + +private: +// boost::weak_ptr parent_link_; + const Link* parent_link_; + +}; + + + + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h new file mode 100644 index 0000000..8e93d94 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h @@ -0,0 +1,220 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_MODEL_H +#define URDF_INTERFACE_MODEL_H + +#include +#include +//#include +#include +#include //printf +#include + +namespace urdf { + +class ModelInterface +{ +public: + my_shared_ptr getRoot(void) const{return this->root_link_;}; + my_shared_ptr getLink(const std::string& name) const + { + my_shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(0); + else + ptr = this->links_.find(name)->second; + return ptr; + }; + + my_shared_ptr getJoint(const std::string& name) const + { + my_shared_ptr ptr; + if (this->joints_.find(name) == this->joints_.end()) + ptr.reset(0); + else + ptr = this->joints_.find(name)->second; + return ptr; + }; + + + const std::string& getName() const {return name_;}; + void getLinks(std::vector >& links) const + { + for (std::map >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) + { + links.push_back(link->second); + } + }; + + void clear() + { + m_numLinks=0; + m_numJoints = 0; + name_.clear(); + this->links_.clear(); + this->joints_.clear(); + this->materials_.clear(); + this->root_link_.reset(0); + }; + + /// non-const getLink() + void getLink(const std::string& name,my_shared_ptr &link) const + { + my_shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(0); + else + ptr = this->links_.find(name)->second; + link = ptr; + }; + + /// non-const getMaterial() + my_shared_ptr getMaterial(const std::string& name) const + { + my_shared_ptr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(0); + else + ptr = this->materials_.find(name)->second; + return ptr; + }; + + void initTree(std::map &parent_link_tree) + { + // loop through all joints, for every link, assign children links and children joints + for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) + { + std::string parent_link_name = joint->second->parent_link_name; + std::string child_link_name = joint->second->child_link_name; + + if (parent_link_name.empty() || child_link_name.empty()) + { + assert(0); + + // throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); + } + else + { + // find child and parent links + my_shared_ptr child_link, parent_link; + this->getLink(child_link_name, child_link); + if (!child_link) + { + printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() ); + assert(0); +// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); + } + this->getLink(parent_link_name, parent_link); + if (!parent_link) + { + assert(0); + +/* throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" to your urdf file."); + + */} + + //set parent link for child link + child_link->setParent(parent_link); + + //set parent joint for child link + child_link->parent_joint = joint->second; + + //set child joint for parent link + parent_link->child_joints.push_back(joint->second); + + //set child link for parent link + parent_link->child_links.push_back(child_link); + + // fill in child/parent string map + parent_link_tree[child_link->name] = parent_link_name; + } + } + } + + void initRoot(const std::map &parent_link_tree) + { + this->root_link_.reset(0); + + // find the links that have no parent in the tree + for (std::map >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::const_iterator parent = parent_link_tree.find(l->first); + if (parent == parent_link_tree.end()) + { + // store root link + if (!this->root_link_) + { + getLink(l->first, this->root_link_); + } + // we already found a root link + else + { + assert(0); + // throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); + } + } + } + if (!this->root_link_) + { + assert(0); + //throw ParseError("No root link found. The robot xml is not a valid tree."); + } + } + + + /// \brief complete list of Links + std::map > links_; + /// \brief complete list of Joints + std::map > joints_; + /// \brief complete list of Materials + std::map > materials_; + + /// \brief The name of the robot model + std::string name_; + + /// \brief The root is always a link (the parent of the tree describing the robot) + my_shared_ptr root_link_; + + int m_numLinks;//includes parent + int m_numJoints; + + +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h new file mode 100644 index 0000000..93183c8 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h @@ -0,0 +1,265 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF_INTERFACE_POSE_H +#define URDF_INTERFACE_POSE_H + +#include +//#include +#include +#include +#ifndef M_PI +#define M_PI 3.141592538 +#endif //M_PI + +#ifdef URDF_USE_BOOST + #include + #include +#else + #include + #include +#endif //URDF_USE_BOOST + +#include +#include + +namespace urdf{ + +class Vector3 +{ +public: + Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; + Vector3() {this->clear();}; + double x; + double y; + double z; + + void clear() {this->x=this->y=this->z=0.0;}; + void init(const std::string &vector_str) + { + this->clear(); + std::vector pieces; + std::vector xyz; + boost::split( pieces, vector_str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + xyz.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + assert(0); + // throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); + } + } + } + + + + if (xyz.size() != 3) + { + assert(0); + // throw ParseError("Parser found " + boost::lexical_cast(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); + } + this->x = xyz[0]; + this->y = xyz[1]; + this->z = xyz[2]; + } + + Vector3 operator+(Vector3 vec) + { + return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); + }; +}; + +class Rotation +{ +public: + Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; + Rotation() {this->clear();}; + void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const + { + quat_x = this->x; + quat_y = this->y; + quat_z = this->z; + quat_w = this->w; + }; + void getRPY(double &roll,double &pitch,double &yaw) const + { + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = this->x * this->x; + sqy = this->y * this->y; + sqz = this->z * this->z; + sqw = this->w * this->w; + + roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); + double sarg = -2 * (this->x*this->z - this->w*this->y); + pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); + yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); + + }; + void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) + { + this->x = quat_x; + this->y = quat_y; + this->z = quat_z; + this->w = quat_w; + this->normalize(); + }; + void setFromRPY(double roll, double pitch, double yaw) + { + double phi, the, psi; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); + this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); + this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); + this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); + + this->normalize(); + }; + + double x,y,z,w; + + void init(const std::string &rotation_str) + { + this->clear(); + Vector3 rpy; + rpy.init(rotation_str); + setFromRPY(rpy.x, rpy.y, rpy.z); + } + + void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } + + void normalize() + { + double s = sqrt(this->x * this->x + + this->y * this->y + + this->z * this->z + + this->w * this->w); + if (s == 0.0) + { + this->x = 0.0; + this->y = 0.0; + this->z = 0.0; + this->w = 1.0; + } + else + { + this->x /= s; + this->y /= s; + this->z /= s; + this->w /= s; + } + }; + + // Multiplication operator (copied from gazebo) + Rotation operator*( const Rotation &qt ) const + { + Rotation c; + + c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; + c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; + c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; + c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; + + return c; + }; + /// Rotate a vector using the quaternion + Vector3 operator*(Vector3 vec) const + { + Rotation tmp; + Vector3 result; + + tmp.w = 0.0; + tmp.x = vec.x; + tmp.y = vec.y; + tmp.z = vec.z; + + tmp = (*this) * (tmp * this->GetInverse()); + + result.x = tmp.x; + result.y = tmp.y; + result.z = tmp.z; + + return result; + }; + // Get the inverse of this quaternion + Rotation GetInverse() const + { + Rotation q; + + double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; + + if (norm > 0.0) + { + q.w = this->w / norm; + q.x = -this->x / norm; + q.y = -this->y / norm; + q.z = -this->z / norm; + } + + return q; + }; + + +}; + +class Pose +{ +public: + Pose() { this->clear(); }; + + Vector3 position; + Rotation rotation; + + void clear() + { + this->position.clear(); + this->rotation.clear(); + }; +}; + +} + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h new file mode 100644 index 0000000..5560de3 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h @@ -0,0 +1,68 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#ifndef URDF_TWIST_H +#define URDF_TWIST_H + +#include +#include +#include +#include +#include + +namespace urdf{ + + +class Twist +{ +public: + Twist() { this->clear(); }; + + Vector3 linear; + // Angular velocity represented by Euler angles + Vector3 angular; + + void clear() + { + this->linear.clear(); + this->angular.clear(); + }; +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h new file mode 100644 index 0000000..b132719 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h @@ -0,0 +1,141 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#ifndef URDF_MODEL_STATE_H +#define URDF_MODEL_STATE_H + +#include +#include +#include +#include +#include + +#include "urdf_model/pose.h" +#include + + +namespace urdf{ + +class Time +{ +public: + Time() { this->clear(); }; + + void set(double _seconds) + { + this->sec = (int32_t)(floor(_seconds)); + this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); + this->Correct(); + }; + + operator double () + { + return (static_cast(this->sec) + + static_cast(this->nsec)*1e-9); + }; + + int32_t sec; + int32_t nsec; + + void clear() + { + this->sec = 0; + this->nsec = 0; + }; +private: + void Correct() + { + // Make any corrections + if (this->nsec >= 1e9) + { + this->sec++; + this->nsec = (int32_t)(this->nsec - 1e9); + } + else if (this->nsec < 0) + { + this->sec--; + this->nsec = (int32_t)(this->nsec + 1e9); + } + }; +}; + + +class JointState +{ +public: + JointState() { this->clear(); }; + + /// joint name + std::string joint; + + std::vector position; + std::vector velocity; + std::vector effort; + + void clear() + { + this->joint.clear(); + this->position.clear(); + this->velocity.clear(); + this->effort.clear(); + } +}; + +class ModelState +{ +public: + ModelState() { this->clear(); }; + + /// state name must be unique + std::string name; + + Time time_stamp; + + void clear() + { + this->name.clear(); + this->time_stamp.set(0); + this->joint_states.clear(); + }; + + std::vector > joint_states; + +}; + +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h new file mode 100644 index 0000000..05f1917 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h @@ -0,0 +1,42 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef URDF_MODEL_STATE_TWIST_ +#define URDF_MODEL_STATE_TWIST_ + +#warning "Please Use #include " + +#include + +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h new file mode 100644 index 0000000..3b99695 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h @@ -0,0 +1,176 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +/* example + + + + + 1.5708 + + + + + + + + + + + + + + +*/ + + + +#ifndef URDF_SENSOR_H +#define URDF_SENSOR_H + +#include +#include +#include +#include +#include +#include "urdf_model/pose.h" +#include "urdf_model/joint.h" +#include "urdf_model/link.h" + +namespace urdf{ + +class VisualSensor +{ +public: + enum {CAMERA, RAY} type; + virtual ~VisualSensor(void) + { + } +}; + +class Camera : public VisualSensor +{ +public: + Camera() { this->clear(); }; + unsigned int width, height; + /// format is optional: defaults to R8G8B8), but can be + /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + std::string format; + double hfov; + double near; + double far; + + void clear() + { + hfov = 0; + width = 0; + height = 0; + format.clear(); + near = 0; + far = 0; + }; +}; + +class Ray : public VisualSensor +{ +public: + Ray() { this->clear(); }; + unsigned int horizontal_samples; + double horizontal_resolution; + double horizontal_min_angle; + double horizontal_max_angle; + unsigned int vertical_samples; + double vertical_resolution; + double vertical_min_angle; + double vertical_max_angle; + + void clear() + { + // set defaults + horizontal_samples = 1; + horizontal_resolution = 1; + horizontal_min_angle = 0; + horizontal_max_angle = 0; + vertical_samples = 1; + vertical_resolution = 1; + vertical_min_angle = 0; + vertical_max_angle = 0; + }; +}; + + +class Sensor +{ +public: + Sensor() { this->clear(); }; + + /// sensor name must be unique + std::string name; + + /// update rate in Hz + double update_rate; + + /// transform from parent frame to optical center + /// with z-forward and x-right, y-down + Pose origin; + + /// sensor + boost::shared_ptr sensor; + + + /// Parent link element name. A pointer is stored in parent_link_. + std::string parent_link_name; + + boost::shared_ptr getParent() const + {return parent_link_.lock();}; + + void setParent(boost::shared_ptr parent) + { this->parent_link_ = parent; } + + void clear() + { + this->name.clear(); + this->sensor.reset(); + this->parent_link_name.clear(); + this->parent_link_.reset(); + }; + +private: + boost::weak_ptr parent_link_; + +}; +} +#endif diff --git a/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h new file mode 100644 index 0000000..eb13fc4 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h @@ -0,0 +1,114 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +/* encapsulates components in a world + see http://ros.org/wiki/usdf/XML/urdf_world and + for details +*/ +/* example world XML + + + + + ... + + + + + + + + + + + + + + + + + + + +*/ + +#ifndef USDF_STATE_H +#define USDF_STATE_H + +#include +#include +#include +#include +#include +#include + +#include "urdf_model/model.h" +#include "urdf_model/pose.h" +#include "urdf_model/twist.h" + +namespace urdf{ + +class Entity +{ +public: + boost::shared_ptr model; + Pose origin; + Twist twist; +}; + +class World +{ +public: + World() { this->clear(); }; + + /// world name must be unique + std::string name; + + std::vector models; + + void initXml(TiXmlElement* config); + + void clear() + { + this->name.clear(); + }; +}; +} + +#endif + diff --git a/3rdparty/rbdl/addons/urdfreader/urdfreader.cc b/3rdparty/rbdl/addons/urdfreader/urdfreader.cc new file mode 100644 index 0000000..8bb4305 --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/urdfreader.cc @@ -0,0 +1,326 @@ +#include + +#include "urdfreader.h" + +#include +#include +#include +#include +#include + +#ifdef RBDL_USE_ROS_URDF_LIBRARY +#include +#include + +typedef boost::shared_ptr LinkPtr; +typedef const boost::shared_ptr ConstLinkPtr; +typedef boost::shared_ptr JointPtr; +typedef boost::shared_ptr ModelPtr; + +#else +#include +#include + +typedef my_shared_ptr LinkPtr; +typedef const my_shared_ptr ConstLinkPtr; +typedef my_shared_ptr JointPtr; +typedef my_shared_ptr ModelPtr; + +#endif + +using namespace std; + +namespace RigidBodyDynamics { + +namespace Addons { + +using namespace Math; + +typedef vector URDFLinkVector; +typedef vector URDFJointVector; +typedef map URDFLinkMap; +typedef map URDFJointMap; + +bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose) { + LinkPtr urdf_root_link; + + URDFLinkMap link_map; + link_map = urdf_model->links_; + + URDFJointMap joint_map; + joint_map = urdf_model->joints_; + + vector joint_names; + + // Holds the links that we are processing in our depth first traversal with the top element being the current link. + stack link_stack; + // Holds the child joint index of the current link + stack joint_index_stack; + + // add the bodies in a depth-first order of the model tree + link_stack.push (link_map[(urdf_model->getRoot()->name)]); + + // add the root body + ConstLinkPtr& root = urdf_model->getRoot (); + Vector3d root_inertial_rpy; + Vector3d root_inertial_position; + Matrix3d root_inertial_inertia; + double root_inertial_mass; + + if (root->inertial) { + root_inertial_mass = root->inertial->mass; + + root_inertial_position.set ( + root->inertial->origin.position.x, + root->inertial->origin.position.y, + root->inertial->origin.position.z); + + root_inertial_inertia(0,0) = root->inertial->ixx; + root_inertial_inertia(0,1) = root->inertial->ixy; + root_inertial_inertia(0,2) = root->inertial->ixz; + + root_inertial_inertia(1,0) = root->inertial->ixy; + root_inertial_inertia(1,1) = root->inertial->iyy; + root_inertial_inertia(1,2) = root->inertial->iyz; + + root_inertial_inertia(2,0) = root->inertial->ixz; + root_inertial_inertia(2,1) = root->inertial->iyz; + root_inertial_inertia(2,2) = root->inertial->izz; + + root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]); + + Body root_link = Body (root_inertial_mass, + root_inertial_position, + root_inertial_inertia); + + Joint root_joint (JointTypeFixed); + if (floating_base) { + root_joint = JointTypeFloatingBase; + } + + SpatialTransform root_joint_frame = SpatialTransform (); + + if (verbose) { + cout << "+ Adding Root Body " << endl; + cout << " joint frame: " << root_joint_frame << endl; + if (floating_base) { + cout << " joint type : floating" << endl; + } else { + cout << " joint type : fixed" << endl; + } + cout << " body inertia: " << endl << root_link.mInertia << endl; + cout << " body mass : " << root_link.mMass << endl; + cout << " body name : " << root->name << endl; + } + + rbdl_model->AppendBody(root_joint_frame, + root_joint, + root_link, + root->name); + } + + // depth first traversal: push the first child onto our joint_index_stack + joint_index_stack.push(0); + + while (link_stack.size() > 0) { + LinkPtr cur_link = link_stack.top(); + + unsigned int joint_idx = joint_index_stack.top(); + + // Add any child bodies and increment current joint index if we still have child joints to process. + if (joint_idx < cur_link->child_joints.size()) { + JointPtr cur_joint = cur_link->child_joints[joint_idx]; + + // increment joint index + joint_index_stack.pop(); + joint_index_stack.push (joint_idx + 1); + + link_stack.push (link_map[cur_joint->child_link_name]); + joint_index_stack.push(0); + + if (verbose) { + for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) { + cout << " "; + } + cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl; + } + + joint_names.push_back(cur_joint->name); + } else { + link_stack.pop(); + joint_index_stack.pop(); + } + } + + unsigned int j; + for (j = 0; j < joint_names.size(); j++) { + JointPtr urdf_joint = joint_map[joint_names[j]]; + LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name]; + LinkPtr urdf_child = link_map[urdf_joint->child_link_name]; + + // determine where to add the current joint and child body + unsigned int rbdl_parent_id = 0; + + rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str()); + + if (rbdl_parent_id == std::numeric_limits::max()) + cerr << "Error while processing joint '" << urdf_joint->name + << "': parent link '" << urdf_parent->name + << "' could not be found." << endl; + + // create the joint + Joint rbdl_joint; + if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) { + Vector3d axis (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); + + if (fabs(axis.dot(Vector3d (1., 0., 0.)) - 1.0) < std::numeric_limits::epsilon()) { + rbdl_joint = Joint(JointTypeRevoluteX); + } else if (fabs(axis.dot(Vector3d (0., 1., 0.)) - 1.0) < std::numeric_limits::epsilon()) { + rbdl_joint = Joint(JointTypeRevoluteY); + } else if (fabs(axis.dot(Vector3d (0., 0., 1.)) - 1.0) < std::numeric_limits::epsilon()) { + rbdl_joint = Joint(JointTypeRevoluteZ); + } else { + rbdl_joint = Joint(JointTypeRevolute, axis); + } + } else if (urdf_joint->type == urdf::Joint::PRISMATIC) { + Vector3d axis (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); + rbdl_joint = Joint (JointTypePrismatic, axis); + } else if (urdf_joint->type == urdf::Joint::FIXED) { + rbdl_joint = Joint (JointTypeFixed); + } else if (urdf_joint->type == urdf::Joint::FLOATING) { + // todo: what order of DoF should be used? + rbdl_joint = Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.)); + } else if (urdf_joint->type == urdf::Joint::PLANAR) { + // todo: which two directions should be used that are perpendicular + // to the specified axis? + cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl; + return false; + } + + // compute the joint transformation + Vector3d joint_rpy; + Vector3d joint_translation; + urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]); + joint_translation.set ( + urdf_joint->parent_to_joint_origin_transform.position.x, + urdf_joint->parent_to_joint_origin_transform.position.y, + urdf_joint->parent_to_joint_origin_transform.position.z + ); + SpatialTransform rbdl_joint_frame = + Xrot (joint_rpy[0], Vector3d (1., 0., 0.)) + * Xrot (joint_rpy[1], Vector3d (0., 1., 0.)) + * Xrot (joint_rpy[2], Vector3d (0., 0., 1.)) + * Xtrans (Vector3d ( + joint_translation + )); + + // assemble the body + Vector3d link_inertial_position; + Vector3d link_inertial_rpy; + Matrix3d link_inertial_inertia = Matrix3d::Zero(); + double link_inertial_mass = 0.; + + // but only if we actually have inertial data + if (urdf_child->inertial) { + link_inertial_mass = urdf_child->inertial->mass; + + link_inertial_position.set ( + urdf_child->inertial->origin.position.x, + urdf_child->inertial->origin.position.y, + urdf_child->inertial->origin.position.z + ); + urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]); + + link_inertial_inertia(0,0) = urdf_child->inertial->ixx; + link_inertial_inertia(0,1) = urdf_child->inertial->ixy; + link_inertial_inertia(0,2) = urdf_child->inertial->ixz; + + link_inertial_inertia(1,0) = urdf_child->inertial->ixy; + link_inertial_inertia(1,1) = urdf_child->inertial->iyy; + link_inertial_inertia(1,2) = urdf_child->inertial->iyz; + + link_inertial_inertia(2,0) = urdf_child->inertial->ixz; + link_inertial_inertia(2,1) = urdf_child->inertial->iyz; + link_inertial_inertia(2,2) = urdf_child->inertial->izz; + + if (link_inertial_rpy != Vector3d (0., 0., 0.)) { + cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl; + return false; + } + } + + Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia); + + if (verbose) { + cout << "+ Adding Body: " << urdf_child->name << endl; + cout << " parent_id : " << rbdl_parent_id << endl; + cout << " joint frame: " << rbdl_joint_frame << endl; + cout << " joint dofs : " << rbdl_joint.mDoFCount << endl; + for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) { + cout << " " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl; + } + cout << " body inertia: " << endl << rbdl_body.mInertia << endl; + cout << " body mass : " << rbdl_body.mMass << endl; + cout << " body name : " << urdf_child->name << endl; + } + + if (urdf_joint->type == urdf::Joint::FLOATING) { + Matrix3d zero_matrix = Matrix3d::Zero(); + Body null_body (0., Vector3d::Zero(3), zero_matrix); + Joint joint_txtytz(JointTypeTranslationXYZ); + string trans_body_name = urdf_child->name + "_Translate"; + rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body, trans_body_name); + + Joint joint_euler_zyx (JointTypeEulerXYZ); + rbdl_model->AppendBody (SpatialTransform(), joint_euler_zyx, rbdl_body, urdf_child->name); + } else { + rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name); + } + } + + return true; +} + +RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose) { + ifstream model_file (filename); + if (!model_file) { + cerr << "Error opening file '" << filename << "'." << endl; + abort(); + } + + // reserve memory for the contents of the file + string model_xml_string; + model_file.seekg(0, std::ios::end); + model_xml_string.reserve(model_file.tellg()); + model_file.seekg(0, std::ios::beg); + model_xml_string.assign((std::istreambuf_iterator(model_file)), std::istreambuf_iterator()); + + model_file.close(); + + return URDFReadFromString (model_xml_string.c_str(), model, floating_base, verbose); +} + +RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose) { + assert (model); + + ModelPtr urdf_model = urdf::parseURDF (model_xml_string); + + if (!construct_model (model, urdf_model, floating_base, verbose)) { + cerr << "Error constructing model from urdf file." << endl; + return false; + } + + model->gravity.set (0., 0., -9.81); + + return true; +} + +} + +} diff --git a/3rdparty/rbdl/addons/urdfreader/urdfreader.h b/3rdparty/rbdl/addons/urdfreader/urdfreader.h new file mode 100644 index 0000000..6e6e9ae --- /dev/null +++ b/3rdparty/rbdl/addons/urdfreader/urdfreader.h @@ -0,0 +1,18 @@ +#ifndef RBDL_URDFREADER_H +#define RBDL_URDFREADER_H + +#include + +namespace RigidBodyDynamics { + +struct Model; + +namespace Addons { + RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose = false); + RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose = false); +} + +} + +/* _RBDL_URDFREADER_H */ +#endif diff --git a/3rdparty/rbdl/doc/Mainpage.h b/3rdparty/rbdl/doc/Mainpage.h new file mode 100644 index 0000000..6fc67cb --- /dev/null +++ b/3rdparty/rbdl/doc/Mainpage.h @@ -0,0 +1,201 @@ +/** \file Mainpage.h + * \mainpage Mainpage + * \image html rbdl_logo.png + * + * This is the documentation of RBDL, the Rigid Body Dynamics Library. The + * library contains highly efficient code for both forward and inverse + * dynamics for kinematic chains and branched models. It includes: + * + * \li Recursive Newton Euler Algorithm (RNEA) + * \li Composite Rigid Body Algorithm (CRBA) + * \li Articulated Body Algorithm (ABA). + + * Furthermore it contains code for forward and inverse \ref + * kinematics_group "kinematics", computations of Jacobians, \ref + * constraints_group "constraints" for contact and collision handling, and + * closed loop models. \link RigidBodyDynamics::Model Models \endlink can + * be loaded from Lua scripts or URDF files. The code and API is heavily + * inspired by the pseudo code of the book "Rigid Body Dynamics Algorithms" + * of Roy + * Featherstone. + + * The code has no external dependencies but for optimal performance it is + * advised to use version 3 of the Eigen math library. More information about it can + * be found here: http://eigen.tuxfamily.org/. The Eigen3 library + * must be obtained and installed separately. + * + * \note This library is free and published under a very liberal \ref + * license "license". If you use it in scientific work we would greatly + * appreciate a citation as described \ref citation "here". + * + * \section download Download + * + * All development takes place on GitHub and you can follow RBDL's + * development and obtain code here:
+ * https://github.com/rbdl/rbdl + * + * \section recent_changes Recent Changes + *
    + *
  • 02 May 2018: New release 2.6.0: + *
      + *
    • Added support for closed-loop models by replacing Contacts API by a new + * Constraints API. Loop constraints can be stabilized using Baumgarte + * stabilization. Special thanks to Davide Corradi for this contribution!
      • + *
    • New constraint type CustomConstraint: a versatile interface to define + * more general types of constraints (e.g. time dependent), contributed by + * Matthew J. Millard.
      • + *
    • New joint type JointTypeHelical that can be used for screwing motions + * (translations and simultaneous rotations), contributed by Stuart Anderson.
      • + *
    • Added support to specify external forces on bodies on constrained forward + * dynamics and NonlinearEffects() (contributed by Matthew J. Millard)
      • + *
    • Changed Quaternion multiplication behaviour for a more standard + * convention: multiplying q1 (1,0,0,0) with q2 (0,1,0,0) results now in + * (0,0,1,0) instead of the previous (0,0,-1,0).
      • + *
    • Removed Model::SetFloatingBaseBody(). Use JointTypeFloatingBase instead.
      • + *
    • LuaModel: extended specification to support ConstraintSets.
      • + *
    + *
    • + *
  • 28 April 2016: New release 2.5.0: + *
      + *
    • Added an experimental Cython based Python wrapper of RBDL. The API is + * very close to the C++ API. For a brief glimpse of the API see + * \ref PythonExample "Python Example"
      • + *
    • Matthew Millard added CustomJoints which allow to create different joint + * types completely by user code. They are implemented as proxy joints for + * which their behaviour is specified using virtual functions.
      • + *
    • Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + * the joint space inertia matrix with a vector in O(n) time.
      • + *
    • Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + * the floating base joint.
      • + *
    • Loading of floating base URDF models must now be specified as a third + * parameter to URDFReadFromFile() and URDFReadFromString()
      • + *
    • Added the URDF code from Bullet3 which gets used when ROS is not found. + * Otherwise use the URDF libraries found via Catkin.
      • + *
    • Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + * that compute both linear and angular quantities
      • + *
    • Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + * JointTypeFloatingBase instead.
      • + *
    • Fixed building issues when building DLL with MSVC++.
      • + *
    + *
    • + *
  • 20 March 2016: New bugfix version 2.4.1: + *
      + *
    • critical: fixed termination criterion for + * InverseKinematics(). The termination criterion would be evaluated too + * early and thus report convergence too early. This was reported + * independently by Kevin Stein, Yun Fei, and Davide Corradi. Thanks + * for the reports!
      • + *
    • critical: fixed CompositeRigidBodyAlgorithm() when using + * spherical joints (thanks to SĂ©bastien BarthĂ©lĂ©my for + * reporting!)
      • + * + *
    + *
    • + *
  • 23 February 2015: New version 2.4.0: + *
      + *
    • Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() and ComputeContactImpulsesRangeSpaceSparse()
      • + *
    • Added null-space method ForwardDynamicsContactsNullSpace() and ComputeContactImpulsesNullSpace()
      • + *
    • Renamed ForwardDynamicsContactsLagrangian() to ForwardDynamicsContactsDirect() and ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect()
      • + *
    • Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis()
      • + *
    • Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The functionality has been integrated into CalcCenterOfMass().
      • + *
    • CalcPointJacobian() does not clear the argument of the result anymore. Caller has to ensure that the matrix was set to zero before using this function.
      • + *
    • Added optional workspace parameters for ForwardDynamicsLagrangian() to optionally reduce memory allocations
      • + *
    • Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ which are equivalent to the emulated multidof joints but faster.
      • + *
    • Added optional parameter to CalcCenterOfMass() to compute angular momentum.
      • + *
    • Added CalcBodySpatialJacobian()
      • + *
    • Added CalcContactJacobian()
      • + *
    • Added NonlinearEffects()
      • + *
    • Added solving of linear systems using standard Householder QR
      • + *
    • LuaModel: Added LuaModelReadFromLuaState()
      • + *
    • URDFReader: Fixed various issues and using faster joints for floating base models
      • + *
    • Various performance improvements
      • + *
    + *
    • + *
+ * + * See \subpage api_version_checking_page for a complete version history. + * + * \section Example Examples + * + * A simple example for creation of a model and computation of the forward + * dynamics using the C++ API can be found \ref SimpleExample "here". + * + * Another example that uses the \ref addon_luamodel_page "LuaModel Addon" can be found \ref + * LuaModelExample "here". + * + * An example of the Python wrapper can be found at \ref PythonExample + * "Python Example". + * + * \section ModuleOverview API Overview + * + * \li \subpage modeling_page + * \li \subpage joint_description + * \li \ref kinematics_group + * \li \ref dynamics_group + * \li \ref constraints_group + * \li \subpage addon_luamodel_page + * + * The page \subpage api_version_checking_page contains information about + * incompatibilities of the existing versions and how to migrate. + * + * \section citation Citation + * + * An overview of the theoretical and implementation details has been + * published in Felis, + * M.L. Auton Robot (2017) 41: 495. To cite RBDL in your academic + * research you can use the following BibTeX entry: + * + * \code + * @Article{Felis2016, + * author="Felis, Martin L.", + * title="RBDL: an efficient rigid-body dynamics library using recursive algorithms", + * journal="Autonomous Robots", + * year="2016", + * pages="1--17", + * issn="1573-7527", + * doi="10.1007/s10514-016-9574-0", + * url="http://dx.doi.org/10.1007/s10514-016-9574-0" + * } + * \endcode + * + * \section license License + * + * The library is published under the very permissive zlib free software + * license which should allow you to use the software wherever you need. + * Here is the full license text: + * \verbatim +RBDL - Rigid Body Dynamics Library +Copyright (c) 2011-2018 Martin Felis + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. +\endverbatim + + * \section Acknowledgements + * + * Previous work on this library was funded by the Heidelberg + * Graduate School of Mathematical and Computational Methods for the + * Sciences (HGS) and the European FP7 projects ECHORD (grant number 231143) and Koroibot (grant number 611909). + * + */ diff --git a/3rdparty/rbdl/doc/api_changes.txt b/3rdparty/rbdl/doc/api_changes.txt new file mode 100644 index 0000000..dfb2f8a --- /dev/null +++ b/3rdparty/rbdl/doc/api_changes.txt @@ -0,0 +1,186 @@ +2.6.0 -> next +- Performance improvements of single DOF revolute joints around coordinate + axes. +- Removed Model::X_J. All joints now have to compute X_lambda directly, + especially CustomJoints + +2.5.0 -> 2.6.0 +- Added support for closed-loop models by replacing Contacts API by a new + Constraints API. Loop constraints can be stabilized using Baumgarte + stabilization. Special thanks to Davide Corradi for this contribution! +- New constraint type CustomConstraint: a versatile interface to define + more general types of constraints (e.g. time dependent), contributed by + Matthew J. Millard. +- New joint type JointTypeHelical that can be used for screwing motions + (translations and simultaneous rotations), contributed by Stuart Anderson. +- Added support to specify external forces on bodies on constrained forward + dynamics and NonlinearEffects() (contributed by Matthew J. Millard) +- Changed Quaternion multiplication behaviour for a more standard + convention: multiplying q1 (1,0,0,0) with q2 (0,1,0,0) results now in + (0,0,1,0) instead of the previous (0,0,-1,0). +- Removed Model::SetFloatingBaseBody(). Use JointTypeFloatingBase instead. +- LuaModel: extended specification to support ConstraintSets. + +2.4.1 -> 2.5.0 +- Added an experimental Cython based Python wrapper of RBDL. The API is + very close to the C++ API. For a brief glimpse of the API see the file + python/test_wrapper.py. +- Matthew Millard added CustomJoints which allow to create different joint + types completely by user code. They are implemented as proxy joints for + which their behaviour is specified using virtual functions. +- Added CalcMInvTimesTau() that evaluates multiplication of the inverse of + the joint space inertia matrix with a vector in O(n) time. +- Added JointTypeFloatingBase which uses TX,TY,TZ and a spherical joint for + the floating base joint. +- Loading of floating base URDF models must now be specified as a third + parameter to URDFReadFromFile() and URDFReadFromString() +- Added the URDF code from Bullet3 which gets used when ROS is not found. + Otherwise use the URDF libraries found via Catkin. +- Added CalcPointVelocity6D, CalcPointAcceleration6D, and CalcPointJacobian6D + that compute both linear and angular quantities +- Removed Model::SetFloatingBase (body). Use a 6-DoF joint or + JointTypeFloatingBase instead. +- Fixed building issues when building DLL with MSVC++. + +2.4.0 -> 2.4.1 (20 April 2016) +This is a bugfix release that maintains binary compatibility and only fixes +erroneous behaviour. +- critical: fixed termination criterion for InverseKinematics. The termination + criterion would be evaluated too early and thus report convergence too + early. This was reported independently by Kevin Stein, Yun Fei, and Davide + Corradi. Thanks for the reports! +- critical: fixed CompositeRigidBodyAlgorithm when using spherical joints + (thanks to Sébastien Barthélémy for reporting!) + +2.3.3 -> 2.4.0 (23 February 2015) +- Added sparse range-space method ForwardDynamicsContactsRangeSpaceSparse() + and ComputeContactImpulsesRangeSpaceSparse() +- Added null-space method ForwardDynamicsContactsNullSpace() + and ComputeContactImpulsesNullSpace() +- Renamed ForwardDynamicsContactsLagrangian() to + ForwardDynamicsContactsDirect() and + ComputeContactImpulsesLagrangian() to ComputeContactImpulsesDirect() +- Renamed ForwardDynamicsContacts() to ForwardDynamicsContactsKokkevis() +- Removed/Fixed CalcAngularMomentum(). The function produced wrong values. The + functionality has been integrated into CalcCenterOfMass(). +- CalcPointJacobian() does not clear the argument of the result anymore. + Caller has to ensure that the matrix was set to zero before using this + function. +- Added optional workspace parameters for ForwardDynamicsLagrangian() to + optionally reduce memory allocations +- Added JointTypeTranslationXYZ, JointTypeEulerXYZ, and JointTypeEulerYXZ + which are equivalent to the emulated multidof joints but faster. +- Added optional parameter to CalcCenterOfMass to compute angular momentum. +- Added CalcBodySpatialJacobian() +- Added CalcContactJacobian() +- Added NonlinearEffects() +- Added solving of linear systems using standard Householder QR +- LuaModel: Added LuaModelReadFromLuaState() +- URDFReader: Fixed various issues and using faster joints for floating + base models +- Various performance improvements + +2.3.2 -> 2.3.3 (21 October 2014) +- critical: fixed ForwardDynamicsContacts with constraints on a body + hat is attached with a fixed joint. Previous versions simply crashed. + Thanks to Yue Hu for reporting! +- rbdl_print_version() now properly prints whether URDFReader was enabled + at build time +- build system: fixed roblems especially building of the URDFreader +- build system: all CMake variables for RBDL are now prefixed with RBDL_ +- FindRBDL.cmake now can use components to search for the LuaModel or + URDFReader addon + +2.3.1 -> 2.3.2 (29 August 2014) +- critical: fixed ForwardDynamicsLagrangian which used uninitialized values for the joint space inertia matrix +- critical: fixed ForwardDynamicsContacts when using 3-dof joints +- critical: fixed CalcBodyWorldOrientation for fixed joints (thanks to Hilaro Tome!) +- critical: fixed CompositeRigidBodyDynamics when using 3-dof joints (thanks to Henning Koch!) + +2.3.0 -> 2.3.1 (13 July 2014) +- critical: fixed angular momentum computation. Version 2.3.0 produced wrong + results. Thanks to Hilario Tome and Benjamin Michaud for reporting! +- critical: fixed JointTypeEulerZYX. Previous versions produce wrong + results! +- fixed library version number for the LuaModel addon. It now uses version + 2.3 instead of the wrong 2.2. + +2.2.2 -> 2.3.0 (14 March 2014) +- disabled clearing of joint space inertia matrix in CRBA. + It is expected that the matrix is cleared by the user when neccessary. +- Added experimental joint type JointTypeEulerZYX. It does not emulate + multiple degrees of freedom using virtual bodies instead it uses a 3 DoF + motion subspace. Performance is better for the + CompositeRigidBodyAlgorithm but worse for other algorithms. +- Using Eigen3's default column-major ordering for matrices when using + Eigen3. This should have no effect for the user unless matrix elements + are accessed using the .data()[i] operator of Eigen3's matrix class. However + if .data()[i] is used the access indices have to be adjusted. +- added functions to compute kinetic and potential energy and the + computation of the center of mass and its linear velocity: + RigidBodyDynamics::Utils::CalcCenterOfMass + RigidBodyDynamics::Utils::CalcPotentialEnergy + RigidBodyDynamics::Utils::CalcKineticEnergy + RigidBodyDynamics::Utils::CalcAngularMomentum + +2.2.1 -> 2.2.2 (06 November 2013) +- adjusted default constructor for Body. It now has the identity matrix as + inertia, instead of a zero matrix. +- LuaModel: made sure that the Body value is optional and uses the default + Body constructor if not defined. + +2.2.0 -> 2.2.1 04 (November 2013) +- properly exporting LuaTables++ functions when using LuaModel addon + Fixes linking +- fixed exported library version (now at 2.2 as expected) + +2.1.0 -> 2.2.0 (28 October 2013) +- added spherical joints that do not suffer from singularities: + Use joint type JointTypeSpherical +- added Model::q_size, and Model::qdot_size that report the sizes including + all 4 Quaternion parameters when spherical joints are used. + User is advised to use Model::q_size and Model::qdot_size instead of + Model::dof_count. +- removed "constraint_" prefix from constraint impulses, forces and + accelerations from the ConstraintSets: + renaming required if values are queried +- Contact impulses: specification of a contact velocity after a collision: + added ConstraintSet::v_plus which can be set for the desired constraint + velocity after a collision. Previously it used the values stored in + ConstraintSet::constraint_acceleration. + User has to store desired exit velocities manually in CS::v_plus + +2.0.1 -> 2.1.0 (29 September 2013) +- made codebase compatible to Debian + Binary symbol export was changed. No change in user code required, + however everything needs to be recompiled and linked. +- Removed Lua 5.2 source + When building the addon LuaModel, one hast to have it installed on the + system already. +- Removed UnitTest++ sources + When building tests one has to have it installed on the system already. + +2.0.0 -> 2.0.1 (05 September 2013) +- fixed compiler errors on some older compilers + No change required when using RBDL version 2.0.0. +- fixed CMake configurations for examples + No change required when using RBDL version 2.0.0. + +1.X.Y -> 2.0.0 (18 July 2013) +- removed Model::Init(): + All initialization is now done in the default constructor in Model(). To + be compatible with the new API simply remove any calls to Model::Init(). +- removed Eigen3 sources: + Eigen3 is no more included in RBDL instead it uses the Eigen3 library + that is installed on the system. If you want to use RBDL with Eigen3 + you have to install it on your system yourself. +- inverted sign of contact forces/impulses: + ConstraintSet::constraint_force and ConstraintSet::constraint_impulse are + now the forces or impulses that are acting on the constrained body by the + constraint. + +1.0.0 -> 1.1.0 (20 February 2013) +- removed constructor Body (mass, com, length, gyration_radii) + This constructor did some erroneous calculations to compute the real + radii of gyration. It was removed as the two remaining constructors are + properly tested and are more general. diff --git a/3rdparty/rbdl/doc/example.h b/3rdparty/rbdl/doc/example.h new file mode 100644 index 0000000..33c66b5 --- /dev/null +++ b/3rdparty/rbdl/doc/example.h @@ -0,0 +1,25 @@ +/** \file example.h + * \page SimpleExample A simple example + * + * Here is a simple example how one can create a meaningless model and + * compute the forward dynamics for it: + * + * \include example.cc + * + * If the library itself is already created, one can compile this example + * with CMake. In the example folder is a CMakeLists.txt file, that can be + * used to automatically create the makefiles by using CMake. It uses the script + * FindRBDL.cmake which can be used to find the library and include + * directory of the headers. + * + * The FindRBDL.cmake script can use the environment variables RBDL_PATH, + * RBDL_INCLUDE_PATH, and RBDL_LIBRARY_PATH to find the required files. + * + * To build it manually you have to specify the following compiler and + * linking switches: + * \code + * g++ example.cc -I/src -I -lrbdl -L -o example + * \endcode + * + */ diff --git a/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png b/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png new file mode 100644 index 0000000..c626d85 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_GeometryAddon_quinticCornerSections.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png new file mode 100644 index 0000000..9bf7c48 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Anderson2007AllPositiveSigns.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_BlendableTorqueMuscle.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_BlendableTorqueMuscle.png new file mode 100644 index 0000000..05ec91e Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_BlendableTorqueMuscle.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png new file mode 100644 index 0000000..f2f2dd8 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_ElbowForearm.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png new file mode 100644 index 0000000..430e399 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_HipKneeAnkle.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png new file mode 100755 index 0000000..813a008 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Lumbar.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png new file mode 100644 index 0000000..49cfaa6 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Shoulder3Dof.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png new file mode 100644 index 0000000..43710de Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_Gymnast_Wrist3Dof.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png new file mode 100644 index 0000000..45a3847 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_falCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png new file mode 100644 index 0000000..b40f6c0 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcCosPhiCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png new file mode 100644 index 0000000..82ee9c8 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcLengthCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png new file mode 100644 index 0000000..7ef78ab Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fcphiCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png new file mode 100644 index 0000000..fbe8f67 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fpeCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png new file mode 100644 index 0000000..5d123f9 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fseCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png new file mode 100644 index 0000000..7d1475f Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png new file mode 100644 index 0000000..6743091 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_MuscleFunctionFactory_fvInvCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_DampingBlendingCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_DampingBlendingCurve.png new file mode 100755 index 0000000..a8ac71d Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_DampingBlendingCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png new file mode 100644 index 0000000..25ec23f Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png new file mode 100644 index 0000000..c13f8ba Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_GaussianActiveTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png new file mode 100644 index 0000000..f879122 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png new file mode 100644 index 0000000..e7db07a Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_PassiveTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png new file mode 100644 index 0000000..5c39517 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png new file mode 100644 index 0000000..696cd0e Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TendonTorqueAngleCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png new file mode 100755 index 0000000..1580bc8 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurve.png differ diff --git a/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png new file mode 100755 index 0000000..109d5b2 Binary files /dev/null and b/3rdparty/rbdl/doc/images/fig_MuscleAddon_TorqueMuscleFunctionFactory_TorqueVelocityCurveSimple.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo.png b/3rdparty/rbdl/doc/logo/rbdl_logo.png new file mode 100644 index 0000000..5d0a755 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo.svg b/3rdparty/rbdl/doc/logo/rbdl_logo.svg new file mode 100644 index 0000000..68280da --- /dev/null +++ b/3rdparty/rbdl/doc/logo/rbdl_logo.svg @@ -0,0 +1,104 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png b/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png new file mode 100644 index 0000000..18146d7 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_16x16.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png b/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png new file mode 100644 index 0000000..eac696e Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_32x32.png differ diff --git a/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png b/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png new file mode 100644 index 0000000..ea92ae7 Binary files /dev/null and b/3rdparty/rbdl/doc/logo/rbdl_logo_64x64.png differ diff --git a/3rdparty/rbdl/doc/luamodel_example.h b/3rdparty/rbdl/doc/luamodel_example.h new file mode 100644 index 0000000..2b2465f --- /dev/null +++ b/3rdparty/rbdl/doc/luamodel_example.h @@ -0,0 +1,43 @@ +/** \file example.h + * \page LuaModelExample LuaModel example + * + * Here is an example on how to use the \ref luamodel_introduction Addon both + * for loading the models from C++ code and also how the model file looks + * like. + * + * Using Lua as a description format for models instead of C++ code has + * numerous advantages such as simplified modeling, less C++ compilations, + * and easier exchange of models between multiple users. As the file format + * uses Lua code it is easy to create parameterized models. + * + * To be able to use the addon one has to enable the addon in CMake by + * setting BUILD_ADDON_LUAMODEL to true. + * + * Here is an example how \ref luamodel_introduction can be loaded by the RBDL: + * + * \include example_luamodel.cc + * + * If the library itself is already created, one can compile this example + * with CMake. In the example folder is a CMakeLists.txt file, that can be + * used to automatically create the makefiles by using CMake. It uses the script + * FindRBDL.cmake which can be used to find the library and include + * directory of the headers. + * + * The documentation about how to write the lua models can be found + * \ref luamodel_introduction "here". + * + * Here is an example: + * + * \include samplemodel.lua + * + * The FindRBDL.cmake script can use the environment variables RBDL_PATH, + * RBDL_INCLUDE_PATH, and RBDL_LIBRARY_PATH to find the required files. + * + * To build it manually you have to specify the following compiler and + * linking switches: + * \code + * g++ example.cc -I/src -I -lrbdl -L -o example + * \endcode + * + */ diff --git a/3rdparty/rbdl/doc/notes/acceleration_visualization.svg b/3rdparty/rbdl/doc/notes/acceleration_visualization.svg new file mode 100644 index 0000000..45cdde3 --- /dev/null +++ b/3rdparty/rbdl/doc/notes/acceleration_visualization.svg @@ -0,0 +1,2003 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex b/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex new file mode 100644 index 0000000..25aee06 --- /dev/null +++ b/3rdparty/rbdl/doc/notes/point_velocity_acceleration.tex @@ -0,0 +1,157 @@ +% File: computation_notes.tex +% Created: Mon Dec 20 02:00 PM 2010 C +% Last Change: Mon Dec 20 02:00 PM 2010 C +% +\documentclass[a4paper]{article} +\usepackage[utf8]{inputenc} +\usepackage{graphicx} + +\newcommand{\Spa}[1]{\mathbf{\hat{#1}}} +\newcommand{\Nspa}[1]{\mathbf{\underline{#1}}} +\newcommand{\Vec}[1]{\mathbf{#1}} +\newcommand{\figref}[1]{\figurename~\ref{#1}} + +\begin{document} + +\section{Computation of the velocity of a point on a rigid body using spatial +algebra} + +The ABA computes automatically the spatial velocity of body $i$ in the +reference frame of the body such that computation of joint velocities are +easier. Now we want to compute the (linear) velocity in base coordinates of a +point $\Vec{p}$ that is attached to body $i$ with body coordinates +${}^i\Vec{r}_p$, i.e. we want to compute ${}^0 \Vec{\dot{r}}_p = {}^0 +\Vec{r}_p$. + +First of all we compute the spatial velocities ${}^0\Spa{v}$ of all bodies in +base coordinates (which can be done during the first loop of the ABA): + +\begin{equation} + {}^0\Spa{v}_i = {}^0\Spa{v}_{\lambda(i)} + {}^{0}\Spa{X}_{i} \Spa{v}_{Ji} +\end{equation} + +where $\Spa{v}_{Ji} = \Spa{S}_i \dot{q}_i $ which is the velocity that is +propagated by joint $i$ from body $\lambda(i)$ to body $i$ along joint axis +$\Spa{S}_i$ (see also RBDA p. 80). + +This is now the velocity of the body $i$ in base coordinates. What is now left +todo is to transform this velocity into the velocity of the point $\Vec{p}$. +Before we can do that we have to compute the coordinates of $P$ in base +coordinates which can be done by: + +\begin{equation} + {}^0\Vec{r}_p = {}^0 \Vec{r}_i + {}^0\Vec{R}_i {}^i \Vec{r}_p +\end{equation} + +for which ${}^0 \Vec{r}_i$ is the origin of the bodies in base coordinates and +${}^0 \Vec{R}_i$ the orientation of the base relative to the body. + +Now we can compute the velocity of point $\Vec{r}_p$ with the following +formulation: +\begin{equation} + {}^0\Spa{v}_p = \textit{xlt}({}^0\Vec{r}_p) {}^0\Spa{v}_i = + \left[ + \begin{array}{cc} + \Vec{1} & 0 \\ + -{}^0\Vec{r}_p \times & \Vec{1} + \end{array} + \right] + {}^0\Spa{v}_i +\end{equation} + +By doing so, the linear part of the spatial velocity ${}^0\Spa{v}_p$ has the +following entries: +\begin{equation} + {}^0\Spa{v}_p = \left[ + \begin{array}{c} + \Nspa{\omega} \\ + -{}^0\Nspa{r}_p \times \Nspa{\omega} + {}^0 \Nspa{\Vec{v}}_i + \end{array} + \right] + = + \left[ + \begin{array}{c} + \Nspa{\omega} \\ + {}^0 \Nspa{\Vec{v}}_i + \Nspa{\omega} \times {}^0 \Nspa{r}_p + \end{array} + \right] +\end{equation} + +For which the bottom line is the term for linear velocity in the standard 3D +notation. + +\section{Computation of the acceleration of a point on a rigid body using +spatial algebra} + +The acceleration of a point depends on three quantities: the position of the +point, velocity of the body and the acceleration of the body. We therefore +assume that we already computed the velocity as described in the previous +section. + +To compute the acceleration of a point we have to compute the spatial +acceleration of a body in base coordinates. This can be expressed as: +\begin{equation} + \Spa{a}_i = {}^0\Spa{a}_{\lambda(i)} + + {}^{0}\Spa{X}_{i} \Spa{a}_{Ji} +\end{equation} +for which $\Spa{a}_{Ji} = \Spa{S}_i \ddot{q}_i + \dot{\Spa{S}}_i \dot{q}_i$. +The last term can be written as: $\dot{\Spa{S}} = \Spa{v}_i \times \Spa{S}_i$. + +\begin{figure}[h!] + \begin{center} + \includegraphics[width=0.9\textwidth]{acceleration_visualization} + \end{center} + \caption{Visualization of the acceleration of a point} + \label{fig:acceleration_visualization} +\end{figure} + +There are three coordinate systems involved in the computation of the +acceleration of point p which are shown in +\figref{fig:acceleration_visualization}. The \emph{base coordinate system} +which is the global reference frame, \emph{link $i$ coordinate system} +which is the coordinate system of body $i$, and the \emph{point coordinate +system} that is locate at the current position of $p$ and has the same +orientation as the base coordinate system. + +First of all we build the transformation from the body coordinate system to +the point coordinate system: +\begin{equation} + {^p}\Vec{X}_i = + \left[ + \begin{array}{cc} + {^0}\Vec{E}_i & \Vec{0} \\ + \Vec{0} & {^0}\Vec{E}_i + \end{array} + \right] + \left[ + \begin{array}{cc} + \Vec{1} & \Vec{0} \\ + -{^i}\Nspa{r}_p \times & \Vec{1} + \end{array} + \right]. +\end{equation} +This can now be used to express the velocity and acceleration of the body at +the point p: +\begin{equation} + {^p}\Spa{v}_i = {^p}\Vec{X}_i \Spa{v}_i +\end{equation} +\begin{equation} + {^p}\Spa{a}_i = {^p}\Vec{X}_i \Spa{a}_i +\end{equation} + +We now want to retrieve the \emph{classical acceleration}. It is expressed in a +coordinate frame that has a pure linear velocity of ${^0}\Nspa{\dot{p}}$. +\begin{equation} + {^p}\Spa{a}'_i = {^p}\Spa{a}_i + + \left[ + \begin{array}{c} + \Vec{0}\\ + {^0}\Nspa{\omega}_i \times {^0}\Nspa{\dot{p}} + \end{array} + \right] +\end{equation} + +The linear part of this vector is then the acceleration of the point in base +coordinates. + +\end{document} diff --git a/3rdparty/rbdl/doc/python_example.h b/3rdparty/rbdl/doc/python_example.h new file mode 100644 index 0000000..1af93ff --- /dev/null +++ b/3rdparty/rbdl/doc/python_example.h @@ -0,0 +1,32 @@ +/** \file python_example.h + * \page PythonExample Python API example + * + * Here is a simple example of the Python API that showcases a subset of + * the wrapped functions and how to access them from python: + * + * \include example.py + * + * To build the wrapper one needs both Cython and the development libraries for + * NumPy installed on the system. If + * this is the case you can build the wrapper by enabling the CMake option + * RBDL_BUILD_PYTHON_WRAPPER when configuring RBDL. You can find the + * wrapper in the subdirectory python/ of your build directory. + * By running the python interpreter in this directory you can load it + * within the python shell using + * + * \code + * Python 2.7.11+ (default, Apr 17 2016, 14:00:29) + * [GCC 5.3.1 20160413] on linux2 + * Type "help", "copyright", "credits" or "license" for more information. + * >>> import rbdl + * \endcode + * + * To install the wrapper you can use the python/setup.py script: + * \code + * sudo ./setup.py install + * \endcode + * + * This installs the RBDL python module globally and allows you to import + * it from any python script. + */ diff --git a/3rdparty/rbdl/examples/luamodel/CMakeLists.txt b/3rdparty/rbdl/examples/luamodel/CMakeLists.txt new file mode 100644 index 0000000..e24d95a --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/CMakeLists.txt @@ -0,0 +1,25 @@ +PROJECT (RBDLEXAMPLE 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} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL COMPONENTS LuaModel REQUIRED) +FIND_PACKAGE (Eigen3 3.0.0 REQUIRED) +FIND_PACKAGE (Lua51 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${LUA_INCLUDE_DIR}) + +# Create an executable +ADD_EXECUTABLE (example_luamodel example_luamodel.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES ( example_luamodel + ${RBDL_LIBRARY} + ${RBDL_LuaModel_LIBRARY} + ${LUA_LIBRARIES} + ) diff --git a/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake b/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake b/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/luamodel/example_luamodel.cc b/3rdparty/rbdl/examples/luamodel/example_luamodel.cc new file mode 100644 index 0000000..d6f1727 --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/example_luamodel.cc @@ -0,0 +1,60 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include +#include + +#ifndef RBDL_BUILD_ADDON_LUAMODEL +#error "Error: RBDL addon LuaModel not enabled." +#endif + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = NULL; + + model = new Model(); + + if (argc != 2) { + std::cerr << "Error: Invalid number of arguments." << std::endl; + std::cerr << "usage: " << argv[0] << " " << std::endl; + exit(-1); + } + + if (!Addons::LuaModelReadFromFile (argv[1], model, false)) { + std::cerr << "Error loading model " << argv[1] << std::endl; + abort(); + } + + std::cout << "Degree of freedom overview:" << std::endl; + std::cout << Utils::GetModelDOFOverview(*model); + + std::cout << "Model Hierarchy:" << std::endl; + std::cout << Utils::GetModelHierarchy(*model); + + VectorNd Q = VectorNd::Zero (model->q_size); + VectorNd QDot = VectorNd::Zero (model->qdot_size); + VectorNd Tau = VectorNd::Zero (model->qdot_size); + VectorNd QDDot = VectorNd::Zero (model->qdot_size); + + std::cout << "Forward Dynamics with q, qdot, tau set to zero:" << std::endl; + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua b/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua new file mode 100644 index 0000000..d0a5bd3 --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/sampleconstrainedmodel.lua @@ -0,0 +1,247 @@ +--[[ +-- This is an example model for the RBDL addon luamodel. You need to +-- enable RBDL_BUILD_ADDON_LUAMODEL to be able to use this file. +--]] + + +m1 = 2 +l1 = 2 +r1 = 0.2 +Izz1 = m1 * l1 * l1 / 3 + +m2 = 2 +l2 = 2 +r2 = 0.2 +Izz2 = m2 * l2 * l2 / 3 + +bodies = { + + virtual = { + mass = 0, + com = {0, 0, 0}, + inertia = { + {0, 0, 0}, + {0, 0, 0}, + {0, 0, 0}, + }, + }, + + link1 = { + mass = m1, + com = {l1/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz1}, + }, + }, + + link2 = { + mass = m2, + com = {l2/2, 0, 0}, + inertia = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, Izz2}, + }, + }, + +} + +joints = { + + revZ = { + {0, 0, 1, 0, 0, 0}, + }, + + trnXYZ = { + {0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + }, + +} + +model = { + + gravity = {0, 0, 0}, + + configuration = { + axis_front = { 1., 0., 0.}, + axis_right = { 0., -1., 0.}, + axis_up = { 0., 0., 1.}, + }, + + frames = { + + { + name = 'base', + parent = 'ROOT', + body = bodies.virtual, + joint = joints.trnXYZ, + }, + { + name = 'l11', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + }, + { + name = 'l12', + parent = 'l11', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + }, + { + name = 'l21', + parent = 'base', + body = bodies.link1, + joint = joints.revZ, + }, + { + name = 'l22', + parent = 'l21', + body = bodies.link2, + joint = joints.revZ, + joint_frame = { + r = {l1, 0, 0}, + }, + }, + + }, + + constraint_sets = { + loop_constraints = { + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_parameter = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_parameter = 0.1, + name = 'linkTY', + }, + }, + + all_constraints = { + { + constraint_type = 'contact', + body = 'base', + point = {0, 0, 0}, + normal = {1, 0, 0}, + name = 'baseTX', + normal_acceleration = 0, + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 1, 0}, + name = 'baseTY', + }, + + { + constraint_type = 'contact', + body = 'base', + normal = {0, 0, 1}, + name = 'baseTZ', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 1, 0, 0}, + stabilization_parameter = 0.1, + name = 'linkTX', + }, + + { + constraint_type = 'loop', + predecessor_body = 'l12', + successor_body = 'l22', + predecessor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {l2, 0, 0}, + }, + successor_transform = { + E = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }, + r = {0, 0, 0}, + }, + axis = {0, 0, 0, 0, 1, 0}, + stabilization_parameter = 0.1, + name = 'linkTY', + }, + }, + }, + +} + +return model diff --git a/3rdparty/rbdl/examples/luamodel/samplemodel.lua b/3rdparty/rbdl/examples/luamodel/samplemodel.lua new file mode 100644 index 0000000..756ed3d --- /dev/null +++ b/3rdparty/rbdl/examples/luamodel/samplemodel.lua @@ -0,0 +1,122 @@ +--[[ +-- This is an example model for the RBDL addon luamodel. You need to +-- enable RBDL_BUILD_ADDON_LUAMODEL to be able to use this file. +--]] + +print ("This is some output from the model file") + +inertia = { + {1.1, 0.1, 0.2}, + {0.3, 1.2, 0.4}, + {0.5, 0.6, 1.3} +} + +pelvis = { mass = 9.3, com = { 1.1, 1.2, 1.3}, inertia = inertia } +thigh = { mass = 4.2, com = { 1.1, 1.2, 1.3}, inertia = inertia } +shank = { mass = 4.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } +foot = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia } + +bodies = { + pelvis = pelvis, + thigh_right = thigh, + shank_right = shank, + thigh_left = thigh, + shank_left = shank +} + +joints = { + freeflyer = { + { 0., 0., 0., 1., 0., 0.}, + { 0., 0., 0., 0., 1., 0.}, + { 0., 0., 0., 0., 0., 1.}, + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + spherical_zyx = { + { 0., 0., 1., 0., 0., 0.}, + { 0., 1., 0., 0., 0., 0.}, + { 1., 0., 0., 0., 0., 0.} + }, + rotational_y = { + { 0., 1., 0., 0., 0., 0.} + }, + fixed = {} +} + +model = { + gravity = {0., 0., -9.81}, + + frames = { + { + name = "pelvis", + parent = "ROOT", + body = bodies.pelvis, + joint = joints.freeflyer, + }, + { + name = "thigh_right", + parent = "pelvis", + body = bodies.thigh_right, + joint = joints.spherical_zyx, + joint_frame = { + r = {0.0, -0.15, 0.}, + E = { + {1., 0., 0.}, + {0., 1., 0.}, + {0., 0., 0.} + } + } + }, + { + name = "shank_right", + parent = "thigh_right", + body = bodies.thigh_right, + joint = joints.rotational_y, + joint_frame = { + r = {0.0, 0., -0.42}, + }, + }, + { + name = "foot_right", + parent = "shank_right", + body = bodies.thigh_right, + joint_frame = { + r = {0.0, 0., -0.41} + }, + }, + { + name = "thigh_left", + parent = "pelvis", + body = bodies.thigh_left, + joint = joints.spherical_zyx, + joint_frame = { + r = {0.0, 0.15, 0.}, + E = { + {1., 0., 0.}, + {0., 1., 0.}, + {0., 0., 0.} + } + } + }, + { + name = "shank_left", + parent = "thigh_left", + body = bodies.thigh_left, + joint = joints.rotational_y, + joint_frame = { + r = {0.0, 0., -0.42}, + }, + }, + { + name = "foot_left", + parent = "shank_left", + body = bodies.thigh_left, + joint_frame = { + r = {0.0, 0., -0.41}, + }, + }, + } +} + +return model diff --git a/3rdparty/rbdl/examples/python/example.py b/3rdparty/rbdl/examples/python/example.py new file mode 100644 index 0000000..fce073f --- /dev/null +++ b/3rdparty/rbdl/examples/python/example.py @@ -0,0 +1,55 @@ +import numpy as np +import rbdl + +# Create a new model +model = rbdl.Model() + +# Create a joint from joint type +joint_rot_y = rbdl.Joint.fromJointType ("JointTypeRevoluteY") + +# Create a body for given mass, center of mass, and inertia at +# the CoM +body = rbdl.Body.fromMassComInertia ( + 1., + np.array([0., 0.5, 0.]), + np.eye(3) * 0.05) +xtrans= rbdl.SpatialTransform() +xtrans.r = np.array([0., 1., 0.]) + +# You can print all types +print (joint_rot_y) +print (model) +print (body) +print (body.mInertia) +print (xtrans) + +# Construct the model +body_1 = model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, body) +body_2 = model.AppendBody (xtrans, joint_rot_y, body) +body_3 = model.AppendBody (xtrans, joint_rot_y, body) + +# Create numpy arrays for the state +q = np.zeros (model.q_size) +qdot = np.zeros (model.qdot_size) +qddot = np.zeros (model.qdot_size) +tau = np.zeros (model.qdot_size) + +# Modify the state +q[0] = 1.3 +q[1] = -0.5 +q[2] = 3.2 + +# Transform coordinates from local to global coordinates +point_local = np.array([1., 2., 3.]) +point_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_3, point_local) +point_local_2 = rbdl.CalcBaseToBodyCoordinates (model, q, body_3, point_base) + +# Perform forward dynamics and print the result +rbdl.ForwardDynamics (model, q, qdot, tau, qddot) +print ("qddot = " + str(qddot.transpose())) + +# Compute and print the jacobian (note: the output parameter +# of the Jacobian G must have proper size!) +G = np.zeros([3,model.qdot_size]) +rbdl.CalcPointJacobian (model, q, body_3, point_local, G) +print ("G = \n" + str(G)) diff --git a/3rdparty/rbdl/examples/simple/CMakeLists.txt b/3rdparty/rbdl/examples/simple/CMakeLists.txt new file mode 100644 index 0000000..2b3a7e4 --- /dev/null +++ b/3rdparty/rbdl/examples/simple/CMakeLists.txt @@ -0,0 +1,22 @@ +PROJECT (RBDLEXAMPLE 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} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL REQUIRED) +FIND_PACKAGE (Eigen3 3.0.0 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) + +# Create an executable +ADD_EXECUTABLE (example example.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES (example + ${RBDL_LIBRARY} + ) diff --git a/3rdparty/rbdl/examples/simple/FindEigen3.cmake b/3rdparty/rbdl/examples/simple/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/simple/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/3rdparty/rbdl/examples/simple/FindRBDL.cmake b/3rdparty/rbdl/examples/simple/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/simple/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/simple/example.cc b/3rdparty/rbdl/examples/simple/example.cc new file mode 100644 index 0000000..0e18f94 --- /dev/null +++ b/3rdparty/rbdl/examples/simple/example.cc @@ -0,0 +1,65 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = NULL; + + unsigned int body_a_id, body_b_id, body_c_id; + Body body_a, body_b, body_c; + Joint joint_a, joint_b, joint_c; + + model = new Model(); + + model->gravity = Vector3d (0., -9.81, 0.); + + body_a = Body (1., Vector3d (0.5, 0., 0.0), Vector3d (1., 1., 1.)); + joint_a = Joint( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 0.5, 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.)); + joint_c = Joint ( + JointTypeRevolute, + Vector3d (0., 0., 1.) + ); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + VectorNd Q = VectorNd::Zero (model->q_size); + VectorNd QDot = VectorNd::Zero (model->qdot_size); + VectorNd Tau = VectorNd::Zero (model->qdot_size); + VectorNd QDDot = VectorNd::Zero (model->qdot_size); + + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt b/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt new file mode 100644 index 0000000..40d8bca --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/CMakeLists.txt @@ -0,0 +1,23 @@ +PROJECT (RBDLEXAMPLE 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} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL COMPONENTS URDFReader REQUIRED) +FIND_PACKAGE (Eigen3 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) + +# Create an executable +ADD_EXECUTABLE (example_urdfreader example_urdfreader.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES ( example_urdfreader + ${RBDL_LIBRARY} + ${RBDL_URDFReader_LIBRARY} + ) diff --git a/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake b/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake new file mode 100644 index 0000000..66ffe8e --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake b/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake new file mode 100644 index 0000000..6804b11 --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc b/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc new file mode 100644 index 0000000..9611e03 --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/example_urdfreader.cc @@ -0,0 +1,58 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include +#include + +#ifndef RBDL_BUILD_ADDON_URDFREADER +#error "Error: RBDL addon URDFReader not enabled." +#endif + +#include + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = new Model(); + + if (argc != 2) { + std::cerr << "Error: not right number of arguments." << std::endl; + std::cerr << "usage: " << argv[0] << " " << std::endl; + exit(-1); + } + + if (!Addons::URDFReadFromFile (argv[1], model, false)) { + std::cerr << "Error loading model " << argv[1] << std::endl; + abort(); + } + + std::cout << "Degree of freedom overview:" << std::endl; + std::cout << Utils::GetModelDOFOverview(*model); + + std::cout << "Model Hierarchy:" << std::endl; + std::cout << Utils::GetModelHierarchy(*model); + + VectorNd Q = VectorNd::Zero (model->q_size); + VectorNd QDot = VectorNd::Zero (model->qdot_size); + VectorNd Tau = VectorNd::Zero (model->qdot_size); + VectorNd QDDot = VectorNd::Zero (model->qdot_size); + + std::cout << "Forward Dynamics with q, qdot, tau set to zero:" << std::endl; + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +} + diff --git a/3rdparty/rbdl/examples/urdfreader/nao.urdf b/3rdparty/rbdl/examples/urdfreader/nao.urdf new file mode 100644 index 0000000..02efa2b --- /dev/null +++ b/3rdparty/rbdl/examples/urdfreader/nao.urdf @@ -0,0 +1,891 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + false + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + -1000.0 + 5 + + + + diff --git a/3rdparty/rbdl/include/rbdl/Body.h b/3rdparty/rbdl/include/rbdl/Body.h new file mode 100644 index 0000000..76e8519 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Body.h @@ -0,0 +1,215 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_BODY_H +#define RBDL_BODY_H + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +/** \brief Describes all properties of a single body + * + * A Body contains information about mass, the location of its center of + * mass, and the ineria tensor in the center of mass. This class is + * designed to use the given information and transform it such that it can + * directly be used by the spatial algebra. + */ +struct RBDL_DLLAPI Body { + Body() : + mMass (0.), + mCenterOfMass (0., 0., 0.), + mInertia (Math::Matrix3d::Zero(3,3)), + mIsVirtual (false) + { }; + Body(const Body &body) : + mMass (body.mMass), + mCenterOfMass (body.mCenterOfMass), + mInertia (body.mInertia), + mIsVirtual (body.mIsVirtual) + {}; + Body& operator= (const Body &body) { + if (this != &body) { + mMass = body.mMass; + mInertia = body.mInertia; + mCenterOfMass = body.mCenterOfMass; + mIsVirtual = body.mIsVirtual; + } + + return *this; + } + + /** \brief Constructs a body from mass, center of mass and radii of gyration + * + * This constructor eases the construction of a new body as all the + * required parameters can be specified as parameters to the + * constructor. These are then used to generate the spatial inertia + * matrix which is expressed at the origin. + * + * \param mass the mass of the body + * \param com the position of the center of mass in the bodies coordinates + * \param gyration_radii the radii of gyration at the center of mass of the body + */ + Body(const double &mass, + const Math::Vector3d &com, + const Math::Vector3d &gyration_radii) : + mMass (mass), + mCenterOfMass(com), + mIsVirtual (false) { + mInertia = Math::Matrix3d ( + gyration_radii[0], 0., 0., + 0., gyration_radii[1], 0., + 0., 0., gyration_radii[2] + ); + } + + /** \brief Constructs a body from mass, center of mass, and a 3x3 inertia matrix + * + * This constructor eases the construction of a new body as all the + * required parameters can simply be specified as parameters to the + * constructor. These are then used to generate the spatial inertia + * matrix which is expressed at the origin. + * + * \param mass the mass of the body + * \param com the position of the center of mass in the bodies coordinates + * \param inertia_C the inertia at the center of mass + */ + Body(const double &mass, + const Math::Vector3d &com, + const Math::Matrix3d &inertia_C) : + mMass (mass), + mCenterOfMass(com), + mInertia (inertia_C), + mIsVirtual (false) { } + + /** \brief Joins inertial parameters of two bodies to create a composite + * body. + * + * This function can be used to joint inertial parameters of two bodies + * to create a composite body that has the inertial properties as if the + * two bodies were joined by a fixed joint. + * + * \param transform The frame transformation from the current body to the + * other body. + * \param other_body The other body that will be merged with *this. + */ + void Join (const Math::SpatialTransform &transform, const Body &other_body) { + // nothing to do if we join a massles body to the current. + if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) { + return; + } + + double other_mass = other_body.mMass; + double new_mass = mMass + other_mass; + + if (new_mass == 0.) { + std::cerr << "Error: cannot join bodies as both have zero mass!" << std::endl; + assert (false); + abort(); + } + + Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r; + Math::Vector3d new_com = (1 / new_mass ) * (mMass * mCenterOfMass + other_mass * other_com); + + LOG << "other_com = " << std::endl << other_com.transpose() << std::endl; + LOG << "rotation = " << std::endl << transform.E << std::endl; + + // We have to transform the inertia of other_body to the new COM. This + // is done in 4 steps: + // + // 1. Transform the inertia from other origin to other COM + // 2. Rotate the inertia that it is aligned to the frame of this body + // 3. Transform inertia of other_body to the origin of the frame of + // this body + // 4. Sum the two inertias + // 5. Transform the summed inertia to the new COM + + Math::SpatialRigidBodyInertia other_rbi = Math::SpatialRigidBodyInertia::createFromMassComInertiaC (other_body.mMass, other_body.mCenterOfMass, other_body.mInertia); + Math::SpatialRigidBodyInertia this_rbi = Math::SpatialRigidBodyInertia::createFromMassComInertiaC (mMass, mCenterOfMass, mInertia); + + Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3,3>(0,0); + LOG << "inertia_other = " << std::endl << inertia_other << std::endl; + + // 1. Transform the inertia from other origin to other COM + Math::Matrix3d other_com_cross = Math::VectorCrossMatrix(other_body.mCenterOfMass); + Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose(); + LOG << "inertia_other_com = " << std::endl << inertia_other_com << std::endl; + + // 2. Rotate the inertia that it is aligned to the frame of this body + Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E; + LOG << "inertia_other_com_rotated = " << std::endl << inertia_other_com_rotated << std::endl; + + // 3. Transform inertia of other_body to the origin of the frame of this body + Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis (inertia_other_com_rotated, other_mass, other_com); + LOG << "inertia_other_com_rotated_this_origin = " << std::endl << inertia_other_com_rotated_this_origin << std::endl; + + // 4. Sum the two inertias + Math::Matrix3d inertia_summed = Math::Matrix3d (this_rbi.toMatrix().block<3,3>(0,0)) + inertia_other_com_rotated_this_origin; + LOG << "inertia_summed = " << std::endl << inertia_summed << std::endl; + + // 5. Transform the summed inertia to the new COM + Math::Matrix3d new_inertia = inertia_summed - new_mass * Math::VectorCrossMatrix (new_com) * Math::VectorCrossMatrix(new_com).transpose(); + + LOG << "new_mass = " << new_mass << std::endl; + LOG << "new_com = " << new_com.transpose() << std::endl; + LOG << "new_inertia = " << std::endl << new_inertia << std::endl; + + *this = Body (new_mass, new_com, new_inertia); + } + + ~Body() {}; + + /// \brief The mass of the body + double mMass; + /// \brief The position of the center of mass in body coordinates + Math::Vector3d mCenterOfMass; + /// \brief Inertia matrix at the center of mass + Math::Matrix3d mInertia; + + bool mIsVirtual; +}; + +/** \brief Keeps the information of a body and how it is attached to another body. + * + * When using fixed bodies, i.e. a body that is attached to anothe via a + * fixed joint, the attached body is merged onto its parent. By doing so + * adding fixed joints do not have an impact on runtime. + */ +struct RBDL_DLLAPI FixedBody { + /// \brief The mass of the body + double mMass; + /// \brief The position of the center of mass in body coordinates + Math::Vector3d mCenterOfMass; + /// \brief The spatial inertia that contains both mass and inertia information + Math::Matrix3d mInertia; + + /// \brief Id of the movable body that this fixed body is attached to. + unsigned int mMovableParent; + /// \brief Transforms spatial quantities expressed for the parent to the + // fixed body. + Math::SpatialTransform mParentTransform; + Math::SpatialTransform mBaseTransform; + + static FixedBody CreateFromBody (const Body& body) { + FixedBody fbody; + + fbody.mMass = body.mMass; + fbody.mCenterOfMass = body.mCenterOfMass; + fbody.mInertia = body.mInertia; + + return fbody; + } +}; + +} + +/* RBDL_BODY_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Constraints.h b/3rdparty/rbdl/include/rbdl/Constraints.h new file mode 100644 index 0000000..556537f --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Constraints.h @@ -0,0 +1,1100 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_CONSTRAINTS_H +#define RBDL_CONSTRAINTS_H + +#include +#include + +namespace RigidBodyDynamics { + +/** \page constraints_page Constraints + * + * All functions related to contacts are specified in the \ref + * constraints_group "Constraints Module". + + * \defgroup constraints_group Constraints + * + * Constraints are handled by specification of a \link + * RigidBodyDynamics::ConstraintSet + * ConstraintSet \endlink which contains all informations about the + * current constraints and workspace memory. + * + * Separate constraints can be specified by calling + * ConstraintSet::AddContactConstraint(), ConstraintSet::AddLoopConstraint(), + * and ConstraintSet::AddCustomConstraint(). + * After all constraints have been specified, this \link + * RigidBodyDynamics::ConstraintSet + * ConstraintSet \endlink has to be bound to the model via + * ConstraintSet::Bind(). This initializes workspace memory that is + * later used when calling one of the contact functions, such as + * ForwardDynamicsContactsDirects(). + * + * The values in the vectors ConstraintSet::force and + * ConstraintSet::impulse contain the computed force or + * impulse values for each constraint when returning from one of the + * contact functions. + * + * \section solution_constraint_system Solution of the Constraint System + * + * \subsection constraint_system Linear System of the Constrained Dynamics + * + * In the presence of constraints, to compute the + * acceleration one has to solve a linear system of the form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + - \lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ is the constraint jacobian, + * \f$C\f$ the bias force (sometimes called "non-linear + * effects"), and \f$\gamma\f$ the generalized acceleration independent + * part of the constraints. + * + * \subsection collision_system Linear System of the Contact Collision + * + * Similarly to compute the response of the model to a contact gain one has + * to solve a system of the following form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \dot{q}^{+} \\ + \Lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + H \dot{q}^{-} \\ + v^{+} + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the + * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ + * the generalized velocity before the impact, and \f$v^{+}\f$ the desired + * velocity of each constraint after the impact (known beforehand, usually + * 0). The value of \f$v^{+}\f$ can is specified via the variable + * ConstraintSet::v_plus and defaults to 0. + * + * \subsection solution_methods Solution Methods + * + * There are essentially three different approaches to solve these systems: + * -# \b Direct: solve the full system to simultaneously compute + * \f$\ddot{q}\f$ and \f$\lambda\f$. This may be slow for large systems + * and many constraints. + * -# \b Range-Space: solve first for \f$\lambda\f$ and then for + * \f$\ddot{q}\f$. + * -# \b Null-Space: solve furst for \f$\ddot{q}\f$ and then for + * \f$\lambda\f$ + * The methods are the same for the contact gains just with different + * variables on the right-hand-side. + * + * RBDL provides methods for all approaches. The implementation for the + * range-space method also exploits sparsities in the joint space inertia + * matrix using a sparse structure preserving \f$L^TL\f$ decomposition as + * described in Chapter 8.5 of "Rigid Body Dynamics Algorithms". + * + * None of the methods is generally superior to the others and each has + * different trade-offs as factors such as model topology, number of + * constraints, constrained bodies, numerical stability, and performance + * vary and evaluation has to be made on a case-by-case basis. + * + * \subsection solving_constraints_dynamics Methods for Solving Constrained + * Dynamics + * + * RBDL provides the following methods to compute the acceleration of a + * constrained system: + * + * - ForwardDynamicsConstraintsDirect() + * - ForwardDynamicsConstraintsRangeSpaceSparse() + * - ForwardDynamicsConstraintsNullSpace() + * + * \subsection solving_constraints_collisions Methods for Computing Collisions + * + * RBDL provides the following methods to compute the collision response on + * new contact gains: + * + * - ComputeConstraintImpulsesDirect() + * - ComputeConstraintImpulsesRangeSpaceSparse() + * - ComputeConstraintImpulsesNullSpace() + * + * \subsection assembly_q_qdot Computing generalized joint positions and velocities + * satisfying the constraint equations. + * + * When considering a model subject position level constraints expressed by the + * equation \f$\phi (q) = 0\f$, it is often necessary to compute generalized joint + * position and velocities which satisfy the constraints. Even velocity-level + * constraints may have position-level assembly constraints: a rolling-without-slipping + * constraint is a velocity-level constraint, but during assembly it might be desireable + * to put the rolling surfaces in contact with eachother. + * + * In order to compute a vector of generalized joint positions that satisfy + * the constraints it is necessary to solve the following optimization problem: + * \f{eqnarray*}{ + * \text{minimize} && \sum_{i = 0}^{n} (q - q_{0})^T W (q - q_{0}) \\ + * \text{over} && q \\ + * \text{subject to} && \phi (q) = 0 + * \f} + * + * In order to compute a vector of generalized joint velocities that satisfy + * the constraints it is necessary to solve the following optimization problem: + * \f{eqnarray*}{ + * \text{minimize} && \sum_{i = 0}^{n} (\dot{q} - \dot{q}_{0})^T W (\dot{q} - \dot{q}_{0}) \\ + * \text{over} && \dot{q} \\ + * \text{subject to} && \dot{\phi} (q) = \phi _{q}(q) \dot{q} + \phi _{t}(q) = 0 + * \f} + * + * \f$q_{0}\f$ and \f$\dot{q}_{0}\f$ are initial guesses, \f$\phi _{q}\f$ is the + * constraints jacobian (partial derivative of \f$\phi\f$ with respect to \f$q\f$), + * and \f$\phi _{t}(q)\f$ is the partial derivative of \f$\phi\f$ with respect + * to time. \f$W\f$ is a diagonal weighting matrix, which can be exploited + * to prioritize changes in the position/ velocity of specific joints. + * With a solver capable of handling singular matrices, it is possible to set to + * 1 the weight of the joint positions/ velocities that should not be changed + * from the initial guesses, and to 0 those corresponding to the values that + * can be changed. + * + * These problems are solved using the Lagrange multipliers method. For the + * velocity problem the solution is exact. For the position problem the + * constraints are linearized in the form + * \f$ \phi (q_{0}) + \phi _{t}(q0) + \phi _{q_0}(q) (q - q_{0}) \f$ + * and the linearized problem is solved iteratively until the constraint position + * errors are smaller than a given threshold. + * + * RBDL provides two functions to compute feasible joint position and velocities: + * - CalcAssemblyQ() + * - CalcAssemblyQDot() + * + * \subsection baumgarte_stabilization Baumgarte Stabilization + * + * The constrained dynamic equations are correct at the acceleration level + * but will drift at the velocity and position level during numerical + * integration. RBDL implements Baumgarte stabilization to avoid + * the accumulation of position and velocity errors for loop constraints and + * custom constraints. Contact constraints do not have Baumgarte stabilization + * because they are a special case which does not typically suffer from drift. + * The stabilization term can be enabled/disabled using the appropriate + * ConstraintSet::AddLoopConstraint and ConstraintSet::AddCustomConstraint + * functions. + * + * The dynamic equations are changed to the following form: \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + - \lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \gamma _{stab} + \end{array} + \right) + * \f] A term \f$\gamma _{stab}\f$ is added to the right hand side of the + * equation, defined in the following way: \f[ + \gamma _{stab} = - 2 \alpha \dot{\phi} (q) - \beta^2 \phi (q) + * \f] where \f$\phi (q)\f$ are the position level constraint errors and + * \f$\alpha\f$ and \f$\beta\f$ are tuning coefficients. There is + * no clear and simple rule on how to choose them as good values also + * depend on the used integration method and time step. If the values are + * too small the constrained dynamics equation becomes stiff, too big + * values result in errors not being reduced. + * + * A good starting point is to parameterize the coefficients as: + * \f[ + * \alpha = \beta = 1 / T_\textit{stab} + * \f] + * with \f$T_\textit{stab}\f$ specifies a time constant for errors in position + * and velocity errors to reduce. Featherstone suggests in his book "Rigid + * Body Dynamics Algorithms" that for a big industrial robot a value of 0.1 + * is reasonable. When testing different values best is to try different + * orders of magnitude as e.g. doubling a value only has little effect. + * + * For Loop- and CustomConstraints Baumgarte stabilization is enabled by + * default and uses the stabilization parameter \f$T_\textit{stab} = 0.1\f$. + * + * @{ + */ + +struct Model; + +struct RBDL_DLLAPI CustomConstraint; + +/** \brief Structure that contains both constraint information and workspace memory. + * + * This structure is used to reduce the amount of memory allocations that + * are needed when computing constraint forces. + * + * The ConstraintSet has to be bound to a model using ConstraintSet::Bind() + * before it can be used in \link RigidBodyDynamics::ForwardDynamicsContacts + * ForwardDynamicsContacts \endlink. + */ +struct RBDL_DLLAPI ConstraintSet { + ConstraintSet() : + linear_solver (Math::LinearSolverColPivHouseholderQR), + bound (false) {} + + // Enum to describe the type of a constraint. + enum ConstraintType { + ContactConstraint, + LoopConstraint, + ConstraintTypeCustom, + ConstraintTypeLast + }; + + /** \brief Adds a contact constraint to the constraint set. + * + * This type of constraints ensures that the velocity and acceleration of a specified + * body point along a specified axis are null. This constraint does not act + * at the position level. + * + * \param body_id the body which is affected directly by the constraint + * \param body_point the point that is constrained relative to the + * contact body + * \param world_normal the normal along the constraint acts (in base + * coordinates) + * \param constraint_name a human readable name (optional, default: NULL) + * \param normal_acceleration the acceleration of the contact along the normal + * (optional, default: 0.) + */ + unsigned int AddContactConstraint ( + unsigned int body_id, + const Math::Vector3d &body_point, + const Math::Vector3d &world_normal, + const char *constraint_name = NULL, + double normal_acceleration = 0.); + + /** \brief Adds a loop constraint to the constraint set. + * + * This type of constraints ensures that the relative orientation and position, + * spatial velocity, and spatial acceleration between two frames in two bodies + * are null along a specified spatial constraint axis. + * + * \param id_predecessor the identifier of the predecessor body + * \param id_successor the identifier of the successor body + * \param X_predecessor a spatial transform localizing the constrained + * frames on the predecessor body, expressed with respect to the predecessor + * body frame + * \param X_successor a spatial transform localizing the constrained + * frames on the successor body, expressed with respect to the successor + * body frame + * \param axis a spatial vector indicating the axis along which the constraint + * acts + * \param enable_stabilization Whether \ref baumgarte_stabilization + * should be enabled or not. + * \param stabilization_param The value for \f$T_\textit{stab}\f$ used for the + * \ref baumgarte_stabilization (defaults to 0.1). + * \param constraint_name a human readable name (optional, default: NULL) + * + */ + unsigned int AddLoopConstraint( + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + const Math::SpatialVector &axis, + bool enable_stabilization = false, + const double stabilization_param = 0.1, + const char *constraint_name = NULL + ); + + /** \brief Adds a custom constraint to the constraint set. + * + * \param custom_constraint The CustomConstraint to be added to the + * ConstraintSet. + * \param id_predecessor the identifier of the predecessor body + * \param id_successor the identifier of the successor body + * \param X_predecessor a spatial transform localizing the constrained + * frames on the predecessor body, expressed with respect to the predecessor + * body frame + * \param X_successor a spatial transform localizing the constrained + * frames on the successor body, expressed with respect to the successor + * body frame + * \param axis a spatial vector indicating the axis along which the constraint + * acts + * \param enable_stabilization Whether \ref baumgarte_stabilization + * should be enabled or not. + * \param stabilization_param The value for \f$T_\textit{stab}\f$ used for the + * \ref baumgarte_stabilization (defaults to 0.1). + * \param constraint_name a human readable name (optional, default: NULL) + * + */ + unsigned int AddCustomConstraint( + CustomConstraint* custom_constraint, + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + bool enable_stabilization = false, + const double stabilization_param = 0.1, + const char *constraint_name = NULL + ); + + + /** \brief Copies the constraints and resets its ConstraintSet::bound + * flag. + */ + ConstraintSet Copy() { + ConstraintSet result (*this); + result.bound = false; + + return result; + } + + /** \brief Specifies which method should be used for solving undelying linear systems. + */ + void SetSolver (Math::LinearSolver solver) { + linear_solver = solver; + } + + /** \brief Initializes and allocates memory for the constraint set. + * + * This function allocates memory for temporary values and matrices that + * are required for contact force computation. Both model and constraint + * set must not be changed after a binding as the required memory is + * dependent on the model size (i.e. the number of bodies and degrees of + * freedom) and the number of constraints in the Constraint set. + * + * The values of ConstraintSet::acceleration may still be + * modified after the set is bound to the model. + * + */ + bool Bind (const Model &model); + + /** \brief Returns the number of constraints. */ + size_t size() const { + return acceleration.size(); + } + + /** \brief Clears all variables in the constraint set. */ + void clear (); + + /// Method that should be used to solve internal linear systems. + Math::LinearSolver linear_solver; + /// Whether the constraint set was bound to a model (mandatory!). + bool bound; + + // Common constraints variables. + std::vector constraintType; + std::vector name; + std::vector mContactConstraintIndices; + std::vector mLoopConstraintIndices; + std::vector mCustomConstraintIndices; + std::vector< CustomConstraint* > mCustomConstraints; + + // Contact constraints variables. + std::vector body; + std::vector point; + std::vector normal; + + // Loop constraints variables. + std::vector body_p; + std::vector body_s; + std::vector X_p; + std::vector X_s; + std::vector constraintAxis; + /** Baumgarte stabilization parameter */ + std::vector baumgarteParameters; + /** Position error for the Baumgarte stabilization */ + Math::VectorNd err; + /** Velocity error for the Baumgarte stabilization */ + Math::VectorNd errd; + + /** Enforced accelerations of the contact points along the contact + * normal. */ + Math::VectorNd acceleration; + /** Actual constraint forces along the contact normals. */ + Math::VectorNd force; + /** Actual constraint impulses along the contact normals. */ + Math::VectorNd impulse; + /** The velocities we want to have along the contact normals */ + Math::VectorNd v_plus; + + // Variables used by the Lagrangian methods + + /// Workspace for the joint space inertia matrix. + Math::MatrixNd H; + /// Workspace for the coriolis forces. + Math::VectorNd C; + /// Workspace of the lower part of b. + Math::VectorNd gamma; + Math::MatrixNd G; + /// Workspace for the Lagrangian left-hand-side matrix. + Math::MatrixNd A; + /// Workspace for the Lagrangian right-hand-side. + Math::VectorNd b; + /// Workspace for the Lagrangian solution. + Math::VectorNd x; + + /// Workspace when evaluating contact Jacobians + Math::MatrixNd Gi; + /// Workspace when evaluating loop/CustomConstraint Jacobians + Math::MatrixNd GSpi; + /// Workspace when evaluating loop/CustomConstraint Jacobians + Math::MatrixNd GSsi; + /// Workspace when evaluating loop/CustomConstraint Jacobians + Math::MatrixNd GSJ; + + /// Workspace for the QR decomposition of the null-space method +#ifdef RBDL_USE_SIMPLE_MATH + SimpleMath::HouseholderQR GT_qr; +#else + Eigen::HouseholderQR GT_qr; +#endif + + Math::MatrixNd GT_qr_Q; + Math::MatrixNd Y; + Math::MatrixNd Z; + Math::VectorNd qddot_y; + Math::VectorNd qddot_z; + + // Variables used by the IABI methods + + /// Workspace for the Inverse Articulated-Body Inertia. + Math::MatrixNd K; + /// Workspace for the accelerations of due to the test forces + Math::VectorNd a; + /// Workspace for the test accelerations. + Math::VectorNd QDDot_t; + /// Workspace for the default accelerations. + Math::VectorNd QDDot_0; + /// Workspace for the test forces. + std::vector f_t; + /// Workspace for the actual spatial forces. + std::vector f_ext_constraints; + /// Workspace for the default point accelerations. + std::vector point_accel_0; + + /// Workspace for the bias force due to the test force + std::vector d_pA; + /// Workspace for the acceleration due to the test force + std::vector d_a; + Math::VectorNd d_u; + + /// Workspace for the inertia when applying constraint forces + std::vector d_IA; + /// Workspace when applying constraint forces + std::vector d_U; + /// Workspace when applying constraint forces + Math::VectorNd d_d; + + std::vector d_multdof3_u; + + //CustomConstraint variables. + + +}; + +/** \brief Computes the position errors for the given ConstraintSet. + * + * \param model the model + * \param Q the generalized positions of the joints + * \param CS the constraint set for which the error should be computed + * \param err (output) vector where the error will be stored in (should have + * the size of CS). + * \param update_kinematics whether the kinematics of the model should be + * updated from Q. + * + * \note the position error is always 0 for contact constraints. + * + */ +RBDL_DLLAPI +void CalcConstraintsPositionError( + Model& model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics = true +); + +/** \brief Computes the Jacobian for the given ConstraintSet + * + * \param model the model + * \param Q the generalized positions of the joints + * \param CS the constraint set for which the Jacobian should be computed + * \param G (output) matrix where the output will be stored in + * \param update_kinematics whether the kinematics of the model should be + * updated from Q + * + */ +RBDL_DLLAPI +void CalcConstraintsJacobian( + Model &model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::MatrixNd &G, + bool update_kinematics = true +); + +/** \brief Computes the velocity errors for the given ConstraintSet. + * + * + * \param model the model + * \param Q the generalized positions of the joints + * \param QDot the generalized velocities of the joints + * \param CS the constraint set for which the error should be computed + * \param err (output) vector where the error will be stored in (should have + * the size of CS). + * \param update_kinematics whether the kinematics of the model should be + * updated from Q. + * + * \note this is equivalent to multiplying the constraint jacobian by the + * generalized velocities of the joints. + * + */ +RBDL_DLLAPI +void CalcConstraintsVelocityError( + Model& model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics = true +); + +/** \brief Computes the terms \f$H\f$, \f$G\f$, and \f$\gamma\f$ of the + * constrained dynamic problem and stores them in the ConstraintSet. + * + * + * \param model the model + * \param Q the generalized positions of the joints + * \param QDot the generalized velocities of the joints + * \param Tau the generalized forces of the joints + * \param CS the constraint set for which the error should be computed + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + * + * \note This function is normally called automatically in the various + * constrained dynamics functions, the user normally does not have to call it. + * + */ +RBDL_DLLAPI +void CalcConstrainedSystemVariables ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + std::vector *f_ext = NULL +); + +/** \brief Computes a feasible initial value of the generalized joint positions. + * + * \param model the model + * \param QInit initial guess for the generalized positions of the joints + * \param CS the constraint set for which the error should be computed + * \param Q (output) vector of the generalized joint positions. + * \param weights weighting coefficients for the different joint positions. + * \param tolerance the function will return successfully if the constraint + * position error norm is lower than this value. + * \param max_iter the funciton will return unsuccessfully after performing + * this number of iterations. + * + * \return true if the generalized joint positions were computed successfully, + * false otherwise. + * + */ +RBDL_DLLAPI +bool CalcAssemblyQ( + Model &model, + Math::VectorNd QInit, + ConstraintSet &CS, + Math::VectorNd &Q, + const Math::VectorNd &weights, + double tolerance = 1e-12, + unsigned int max_iter = 100 +); + +/** \brief Computes a feasible initial value of the generalized joint velocities. + * + * \param model the model + * \param Q the generalized joint position of the joints. It is assumed that + * this vector satisfies the position level assemblt constraints. + * \param QDotInit initial guess for the generalized velocities of the joints + * \param CS the constraint set for which the error should be computed + * \param QDot (output) vector of the generalized joint velocities. + * \param weights weighting coefficients for the different joint positions. + * + */ +RBDL_DLLAPI +void CalcAssemblyQDot( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotInit, + ConstraintSet &CS, + Math::VectorNd &QDot, + const Math::VectorNd &weights +); + +/** \brief Computes forward dynamics with contact by constructing and solving + * the full lagrangian equation + * + * This method builds and solves the linear system \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \ddot{q} \\ + -\lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + -C + \tau \\ + \gamma + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$C\f$ the bias force (sometimes called "non-linear + * effects"), and \f$\gamma\f$ the generalized acceleration independent + * part of the contact point accelerations. + * + * \note This function works with ContactConstraints, LoopConstraints and + * Custom Constraints. Nonetheless, this method will not tolerate redundant + * constraints. + * + * \par + * + * \note To increase performance group constraints body and pointwise such + * that constraints acting on the same body point are sequentially in + * ConstraintSet. This can save computation of point jacobians \f$G\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param CS the description of all acting constraints + * \param QDDot accelerations of the internals joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + * \note During execution of this function values such as + * ConstraintSet::force get modified and will contain the value + * of the force acting along the normal. + * + */ +RBDL_DLLAPI +void ForwardDynamicsConstraintsDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot, + std::vector *f_ext = NULL +); + +RBDL_DLLAPI +void ForwardDynamicsConstraintsRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot, + std::vector *f_ext = NULL +); + +RBDL_DLLAPI +void ForwardDynamicsConstraintsNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot, + std::vector *f_ext = NULL +); + +/** \brief Computes forward dynamics that accounts for active contacts in + * ConstraintSet. + * + * The method used here is the one described by Kokkevis and Metaxas in the + * Paper "Practical Physics for Articulated Characters", Game Developers + * Conference, 2004. + * + * It does this by recursively computing the inverse articulated-body inertia (IABI) + * \f$\Phi_{i,j}\f$ which is then used to build and solve a system of the form: + \f[ + \left( + \begin{array}{c} + \dot{v}_1 \\ + \dot{v}_2 \\ + \vdots \\ + \dot{v}_n + \end{array} + \right) + = + \left( + \begin{array}{cccc} + \Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\ + \Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\ + \cdots & \cdots & \cdots & \vdots \\ + \Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n} + \end{array} + \right) + \left( + \begin{array}{c} + f_1 \\ + f_2 \\ + \vdots \\ + f_n + \end{array} + \right) + + + \left( + \begin{array}{c} + \phi_1 \\ + \phi_2 \\ + \vdots \\ + \phi_n + \end{array} + \right). + \f] + Here \f$n\f$ is the number of constraints and the method for building the system + uses the Articulated Body Algorithm to efficiently compute entries of the system. The + values \f$\dot{v}_i\f$ are the constraint accelerations, \f$f_i\f$ the constraint forces, + and \f$\phi_i\f$ are the constraint bias forces. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param CS a list of all contact points + * \param QDDot accelerations of the internals joints (output) + * + * \note During execution of this function values such as + * ConstraintSet::force get modified and will contain the value + * of the force acting along the normal. + * + * \note This function supports only contact constraints. + * + * \todo Allow for external forces + */ +RBDL_DLLAPI +void ForwardDynamicsContactsKokkevis ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot +); + +/** \brief Computes contact gain by constructing and solving the full lagrangian + * equation + * + * This method builds and solves the linear system \f[ + \left( + \begin{array}{cc} + H & G^T \\ + G & 0 + \end{array} + \right) + \left( + \begin{array}{c} + \dot{q}^{+} \\ + \Lambda + \end{array} + \right) + = + \left( + \begin{array}{c} + H \dot{q}^{-} \\ + v^{+} + \end{array} + \right) + * \f] where \f$H\f$ is the joint space inertia matrix computed with the + * CompositeRigidBodyAlgorithm(), \f$G\f$ are the point jacobians of the + * contact points, \f$\dot{q}^{+}\f$ the generalized velocity after the + * impact, \f$\Lambda\f$ the impulses at each constraint, \f$\dot{q}^{-}\f$ + * the generalized velocity before the impact, and \f$v^{+}\f$ the desired + * velocity of each constraint after the impact (known beforehand, usually + * 0). The value of \f$v^{+}\f$ can is specified via the variable + * ConstraintSet::v_plus and defaults to 0. + * + * \note So far, only constraints acting along cartesian coordinate axes + * are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must + * not specify redundant constraints! + * + * \par + * + * \note To increase performance group constraints body and pointwise such + * that constraints acting on the same body point are sequentially in + * ConstraintSet. This can save computation of point Jacobians \f$G\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDotMinus velocity vector of the internal joints before the impact + * \param CS the set of active constraints + * \param QDotPlus velocities of the internals joints after the impact (output) + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Resolves contact gain using SolveContactSystemRangeSpaceSparse() + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Resolves contact gain using SolveContactSystemNullSpace() + */ +RBDL_DLLAPI +void ComputeConstraintImpulsesNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus +); + +/** \brief Solves the full contact system directly, i.e. simultaneously for + * contact forces and joint accelerations. + * + * This solves a \f$ (n_\textit{dof} + + * n_c) \times (n_\textit{dof} + n_c\f$ linear system. + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param A work-space for the matrix of the linear system + * \param b work-space for the right-hand-side of the linear system + * \param x work-space for the solution of the linear system + * \param type of solver that should be used to solve the system + */ +RBDL_DLLAPI +void SolveConstrainedSystemDirect ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &A, + Math::VectorNd &b, + Math::VectorNd &x, + Math::LinearSolver &linear_solver +); + +/** \brief Solves the contact system by first solving for the the joint + * accelerations and then the contact forces using a sparse matrix + * decomposition of the joint space inertia matrix. + * + * This method exploits the branch-induced sparsity by the structure + * preserving \f$L^TL \f$ decomposition described in RBDL, Section 6.5. + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param K work-space for the matrix of the constraint force linear system + * \param a work-space for the right-hand-side of the constraint force linear + * system + * \param linear_solver type of solver that should be used to solve the + * constraint force system + */ +RBDL_DLLAPI +void SolveConstrainedSystemRangeSpaceSparse ( + Model &model, + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &K, + Math::VectorNd &a, + Math::LinearSolver linear_solver +); + +/** \brief Solves the contact system by first solving for the joint + * accelerations and then for the constraint forces. + * + * This methods requires a \f$n_\textit{dof} \times n_\textit{dof}\f$ + * matrix of the form \f$\left[ \ Y \ | Z \ \right]\f$ with the property + * \f$GZ = 0\f$ that can be computed using a QR decomposition (e.g. see + * code for ForwardDynamicsContactsNullSpace()). + * + * \param H the joint space inertia matrix + * \param G the constraint jacobian + * \param c the \f$ \mathbb{R}^{n_\textit{dof}}\f$ vector of the upper part of + * the right hand side of the system + * \param gamma the \f$ \mathbb{R}^{n_c}\f$ vector of the lower part of the + * right hand side of the system + * \param qddot result: joint accelerations + * \param lambda result: constraint forces + * \param Y basis for the range-space of the constraints + * \param Z basis for the null-space of the constraints + * \param qddot_y work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ + * \param qddot_z work-space of size \f$\mathbb{R}^{n_\textit{dof}}\f$ + * \param linear_solver type of solver that should be used to solve the system + */ +RBDL_DLLAPI +void SolveConstrainedSystemNullSpace ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &Y, + Math::MatrixNd &Z, + Math::VectorNd &qddot_y, + Math::VectorNd &qddot_z, + Math::LinearSolver &linear_solver +); + + +/** \brief Interface to define general-purpose constraints. + * +* The CustomConstraint interface is a general-purpose interface that is rich +* enough to define time-varying constraints at the position-level \f$\phi_p(q,t)=0\f$, +* the velocity-level \f$\phi_v(\dot{q},t)=0\f$, or the acceleration-level +* \f$\phi_a(\ddot{q},t)=0\f$. These constraints all end up being applied at the +* acceleration-level by taking successive derivatives until we are left with +* \f$\Phi(\ddot{q},\dot{q},q,t)=0\f$ +* The interface requires that the user populate +* +* - G: \f$ \dfrac{ \partial \Phi(\ddot{q},\dot{q},q,t)}{ \partial \ddot{q}}\f$ +* - constraintAxis: the axis that the constraints are applied along +* - gamma: \f$ \gamma = - ( \Phi(\ddot{q},\dot{q},q,t) - G \ddot{q})\f$ +* - errPos: The vector of \f$\phi_p(\dot{q},t)\f$. If the constraint is a velocity-level +* constraint or higher this should be set to zero. +* - errVel: The vector of \f$\phi_v(\dot{q},t)\f$. If the constraint is an acceleration-level +* constraint this should be set to zero +* +* The matrix G and the vector gamma are required to compute accelerations that satisfy +* the desired constraints. The vectors errPos and errVel are required to apply Baumgarte +* stabilization (a constraint stabilization method) to stabilize the constraint. The +* interface provides (optional) interfaces for the functions +* +* - CalcAssemblyPositionError +* - CalcAssemblyPositionErrorJacobian +* - CalcAssemblyVelocityError +* - CalcAssemblyVelocityErrorJacobian +* +* These optional functions are used only during system assembly. This permits a velocity-level +* constraint (e.g. such as a rolling-without-slipping constraint) to define a position-level +* constraint to assemble the system (e.g. bring the rolling surfaces into contact with +* eachother). If you are defining a position-level constraint these optional functions +* should simply return errPos, G, errVel, and G respectively. +* +* It must be stated that this is an advanced feature of RBDL: you must have an in-depth +* knowledge of multibody-dynamics in order to write a custom constraint, and to write +* the corresponding test code to validate that the constraint is working. As a hint the +* test code in tests/CustomConstraintsTest.cc should contain a forward simulation of a simple +* system using the constraint and then check to see that system energy is conserved with only +* a small error and the constraint is also satisfied with only a small error. The observed +* error should drop as the integrator tolerances are lowered. +*/ +struct RBDL_DLLAPI CustomConstraint { + unsigned int mConstraintCount; + //unsigned int mAssemblyConstraintCount; + + CustomConstraint(unsigned int constraintCount):mConstraintCount(constraintCount){} + + virtual ~CustomConstraint(){}; + + virtual void CalcConstraintsJacobianAndConstraintAxis( + Model &model, + unsigned int custom_constraint_id, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::MatrixNd &G, + unsigned int GrowStart, + unsigned int GcolStart) = 0; + + virtual void CalcGamma( Model &model, + unsigned int custom_constraint_id, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + const Math::MatrixNd &Gblock, + Math::VectorNd &gamma, + unsigned int gammaStartIndex) = 0; + + + virtual void CalcPositionError( Model &model, + unsigned int custom_constraint_id, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::VectorNd &err, + unsigned int errStartIdx) = 0; + + virtual void CalcVelocityError( Model &model, + unsigned int custom_constraint_id, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + const Math::MatrixNd &Gblock, + Math::VectorNd &err, + unsigned int errStartIndex) = 0; + +}; + + + +/** @} */ + +} /* namespace RigidBodyDynamics */ + +/* RBDL_CONSTRAINTS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Dynamics.h b/3rdparty/rbdl/include/rbdl/Dynamics.h new file mode 100644 index 0000000..e039bdb --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Dynamics.h @@ -0,0 +1,184 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_DYNAMICS_H +#define RBDL_DYNAMICS_H + +#include +#include + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +struct Model; + +/** \page dynamics_page Dynamics + * + * All functions related to kinematics are specified in the \ref + * dynamics_group "Dynamics Module". + * + * \defgroup dynamics_group Dynamics + * @{ + */ + +/** \brief Computes inverse dynamics with the Newton-Euler Algorithm + * + * This function computes the generalized forces from given generalized + * states, velocities, and accelerations: + * \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$ + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot accelerations of the internals joints + * \param Tau actuations of the internal joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + */ +RBDL_DLLAPI void InverseDynamics ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + Math::VectorNd &Tau, + std::vector *f_ext = NULL + ); + +/** \brief Computes the coriolis forces + * + * This function computes the generalized forces from given generalized + * states, velocities, and accelerations: + * \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$ + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + */ +RBDL_DLLAPI void NonlinearEffects ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + Math::VectorNd &Tau, + std::vector *f_ext = NULL + ); + +/** \brief Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm + * + * This function computes the joint space inertia matrix from a given model and + * the generalized state vector: + * \f$ M(q) \f$ + * + * \param model rigid body model + * \param Q state vector of the model + * \param H a matrix where the result will be stored in + * \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost!) + * + * \note This function only evaluates the entries of H that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling H.setZero(). + */ +RBDL_DLLAPI void CompositeRigidBodyAlgorithm ( + Model& model, + const Math::VectorNd &Q, + Math::MatrixNd &H, + bool update_kinematics = true + ); + +/** \brief Computes forward dynamics with the Articulated Body Algorithm + * + * This function computes the generalized accelerations from given + * generalized states, velocities and forces: + * \f$ \ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)\f$ + * It does this by using the recursive Articulated Body Algorithm that runs + * in \f$O(n_{dof})\f$ with \f$n_{dof}\f$ being the number of joints. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param QDDot accelerations of the internal joints (output) + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + */ +RBDL_DLLAPI void ForwardDynamics ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + std::vector *f_ext = NULL + ); + +/** \brief Computes forward dynamics by building and solving the full Lagrangian equation + * + * This method builds and solves the linear system + * \f[ H \ddot{q} = -C + \tau \f] + * for \f$\ddot{q}\f$ where \f$H\f$ is the joint space inertia matrix + * computed with the CompositeRigidBodyAlgorithm(), \f$C\f$ the bias + * force (sometimes called "non-linear effects"). + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param Tau actuations of the internal joints + * \param QDDot accelerations of the internal joints (output) + * \param linear_solver specification which method should be used for solving the linear system + * \param f_ext External forces acting on the body in base coordinates (optional, defaults to NULL) + * \param H preallocated workspace area for the joint space inertia matrix of size dof_count x dof_count (optional, defaults to NULL and allocates temporary matrix) + * \param C preallocated workspace area for the right hand side vector of size dof_count x 1 (optional, defaults to NULL and allocates temporary vector) + */ +RBDL_DLLAPI void ForwardDynamicsLagrangian ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR, + std::vector *f_ext = NULL, + Math::MatrixNd *H = NULL, + Math::VectorNd *C = NULL + ); + +/** \brief Computes the effect of multiplying the inverse of the joint + * space inertia matrix with a vector in linear time. + * + * \param model rigid body model + * \param Q state vector of the generalized positions + * \param Tau the vector that should be multiplied with the inverse of + * the joint space inertia matrix + * \param QDDot vector where the result will be stored + * \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost) + * + * This function uses a reduced version of the Articulated %Body Algorithm + * to compute: + * + * \f$ \ddot{q} = M(q)^{-1} \tau\f$ + * + * for given \f$q\f$ and \f$\tau\f$ in \f$O(n_{\textit{dof}})\f$ time. + * + * \note When calling this function repeatedly for the same values of Q make sure + * to set the last parameter to false as this avoids expensive + * recomputations of transformations and articulated body inertias. + */ +RBDL_DLLAPI void CalcMInvTimesTau ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &Tau, + Math::VectorNd &QDDot, + bool update_kinematics=true + ); + +/** @} */ + +} + +/* RBDL_DYNAMICS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Joint.h b/3rdparty/rbdl/include/rbdl/Joint.h new file mode 100644 index 0000000..4989232 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Joint.h @@ -0,0 +1,705 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_JOINT_H +#define RBDL_JOINT_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +struct Model; + +/** \page joint_description Joint Modeling + * + * \section joint_overview Overview + * + * The Rigid Body Dynamics Library supports a multitude of joints: + * revolute, planar, fixed, singularity-free spherical joints and joints + * with multiple degrees of freedom in any combinations. + * + * Fixed joints do not cause any overhead in RBDL as the bodies that are + * rigidly connected are merged into a single body. For details see \ref + * joint_models_fixed. + * + * Joints with multiple degrees of freedom are emulated by default which + * means that they are split up into multiple single degree of freedom + * joints which results in equivalent models. This has the benefit that it + * simplifies the required algebra and also code branching in RBDL. A + * special case are joints with three degrees of freedom for which specific + * joints are available that should be used for performance reasons + * whenever possible. See \ref joint_three_dof for details. + * + * Joints are defined by their motion subspace. For each degree of freedom + * a one dimensional motion subspace is specified as a Math::SpatialVector. + * This vector follows the following convention: \f[ (r_x, r_y, r_z, t_x, + * t_y, t_z) \f] + * + * To specify a planar joint with three degrees of freedom for which the + * first two are translations in \f$x\f$ and \f$y\f$ direction and the last + * is a rotation around \f$z\f$, the following joint definition can be + * used: + + * \code Joint planar_joint = Joint ( + * Math::SpatialVector (0., 0., 0., 1., 0., 0.), + * Math::SpatialVector (0., 0., 0., 0., 1., 0.), + * Math::SpatialVector (0., 0., 1., 0., 0., 0.) + * ); + * \endcode + + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + * + * \section joint_models_fixed Fixed Joints + * + * Fixed joints do not add an additional degree of freedom to the model. + * When adding a body that via a fixed joint (i.e. when the type is + * JointTypeFixed) then the dynamical parameters mass and inertia are + * merged onto its moving parent. By doing so fixed bodies do not add + * computational costs when performing dynamics computations. + + * To ensure a consistent API for the Kinematics such fixed bodies have a + * different range of ids. Where as the ids start at 1 get incremented for + * each added body, fixed bodies start at Model::fixed_body_discriminator + * which has a default value of std::numeric_limits::max() / + * 2. This means theoretical a maximum of each 2147483646 movable and fixed + * bodies are possible. + + * To check whether a body is connected by a fixed joint you can use the + * function Model::IsFixedBodyId(). + + * \section joint_three_dof 3-DoF Joints + * + * RBDL has highly efficient implementations for the following three degree + * of freedom joints: + *
    + *
  • \ref JointTypeTranslationXYZ which first translates along X, then + * Y, and finally Z.
    • + *
  • \ref JointTypeEulerZYX which first rotates around Z, then Y, and + * then X.
    • + *
  • \ref JointTypeEulerXYZ which first rotates around X, then Y, and + * then Z.
    • + *
  • \ref JointTypeEulerYXZ which first rotates around Y, then X, and + * then Z.
    • + *
  • \ref JointTypeEulerZXY which first rotates around Z, then X, and + * then Y.
    • + *
  • \ref JointTypeSpherical which is a singularity free joint that + * uses a Quaternion and the bodies angular velocity (see \ref + * joint_singularities for details).
    • + *
+ * + * These joints can be created by providing the joint type as an argument + * to the Joint constructor, e.g.: + * + * \code Joint joint_rot_zyx = Joint ( JointTypeEulerZYX ); \endcode + * + * Using 3-Dof joints is always favourable over using their emulated + * counterparts as they are considerably faster and describe the same + * kinematics and dynamics. + + * \section joint_floatingbase Floating-Base Joint (a.k.a. Freeflyer Joint) + * + * RBDL has a special joint type for floating-base systems that uses the + * enum JointTypeFloatingBase. The first three DoF are translations along + * X,Y, and Z. For the rotational part it uses a JointTypeSpherical joint. + * It is internally modeled by a JointTypeTranslationXYZ and a + * JointTypeSpherical joint. It is recommended to only use this joint for + * the very first body added to the model. + * + * Positional variables are translations along X, Y, and Z, and for + * rotations it uses Quaternions. To set/get the orientation use + * Model::SetQuaternion () / Model::GetQuaternion() with the body id + * returned when adding the floating base (i.e. the call to + * Model::AddBody() or Model::AppendBody()). + + * \section joint_singularities Joint Singularities + + * Singularities in the models arise when a joint has three rotational + * degrees of freedom and the rotations are described by Euler- or + * Cardan-angles. The singularities present in these rotation + * parametrizations (e.g. for ZYX Euler-angles for rotations where a + * +/- 90 degrees rotation around the Y-axis) may cause problems in + * dynamics calculations, such as a rank-deficit joint-space inertia matrix + * or exploding accelerations in the forward dynamics calculations. + * + * For this case RBDL has the special joint type + * RigidBodyDynamics::JointTypeSpherical. When using this joint type the + * model does not suffer from singularities, however this also results in + * a change of interpretation for the values \f$\mathbf{q}, \mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, and \f$\mathbf{\tau}\f$: + * + *
    + *
  • The values in \f$\mathbf{q}\f$ for the joint parameterizes the orientation of a joint using a + * Quaternion \f$q_i\f$
    • + *
  • The values in \f$\mathbf{\dot{q}}\f$ for the joint describe the angular + * velocity \f$\omega\f$ of the joint in body coordinates
    • + *
  • The values in \f$\mathbf{\ddot{q}}\f$ for the joint describe the angular + * acceleration \f$\dot{\omega}\f$ of the joint in body coordinates
    • + *
  • The values in \f$\mathbf{\tau}\f$ for the joint describe the three couples + * acting on the body in body coordinates that are actuated by the joint.
    • + *
+ + * As a result, the dimension of the vector \f$\mathbf{q}\f$ is higher than + * of the vector of the velocity variables. Additionally, the values in + * \f$\mathbf{\dot{q}}\f$ are \b not the derivative of \f$q\f$ and are therefore + * denoted by \f$\mathbf{\bar{q}}\f$ (and similarly for the joint + * accelerations). + + * RBDL stores the Quaternions in \f$\mathbf{q}\f$ such that the 4th component of + * the joint is appended to \f$\mathbf{q}\f$. E.g. for a model with the joints: + * TX, Spherical, TY, Spherical, the values of \f$\mathbf{q},\mathbf{\bar{q}},\mathbf{\bar{\bar{q}}},\mathbf{\tau}\f$ are: + * + + \f{eqnarray*} + \mathbf{q} &=& ( q_{tx}, q_{q1,x}, q_{q1,y}, q_{q1,z}, q_{ty}, q_{q2,x}, q_{q2,y}, q_{q2,z}, q_{q1,w}, q_{q2,w})^T \\ + \mathbf{\bar{q}} &=& ( \dot{q}_{tx}, \omega_{1,x}, \omega_{1,y}, \omega_{1,z}, \dot{q}_{ty}, \omega_{2,x}, \omega_{2,y}, \omega_{2,z} )^T \\ + \mathbf{\bar{\bar{q}}} &=& ( \ddot{q}_{tx}, \dot{\omega}_{1,x}, \dot{\omega}_{1,y}, \dot{\omega}_{1,z}, \ddot{q}_{ty}, \dot{\omega}_{2,x}, \dot{\omega}_{2,y}, \dot{\omega}_{2,z} )^T \\ + \mathbf{\tau} &=& ( \tau_{tx}, \tau_{1,x}, \tau_{1,y}, \tau_{1,z}, \tau_{ty}, \tau_{2,x}, \tau_{2,y}, \tau_{2,z} )^T + \f} + + * \subsection spherical_integration Numerical Integration of Quaternions + * + * An additional consequence of this is, that special treatment is required + * when numerically integrating the angular velocities. One possibility is + * to interpret the angular velocity as an axis-angle pair scaled by the + * timestep and use it create a quaternion that is applied to the previous + * Quaternion. Another is to compute the quaternion rates from the angular + * velocity. For details see James Diebel "Representing Attitude: Euler + * Angles, Unit Quaternions, and Rotation Vectors", 2006, + * http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134. + * + */ + + /** \brief General types of joints + */ + enum JointType { + JointTypeUndefined = 0, + JointTypeRevolute, + JointTypePrismatic, + JointTypeRevoluteX, + JointTypeRevoluteY, + JointTypeRevoluteZ, + JointTypeSpherical, ///< 3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity variables. + JointTypeEulerZYX, ///< 3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints). + JointTypeEulerXYZ, ///< 3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints). + JointTypeEulerYXZ, ///< 3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints). + JointTypeEulerZXY, ///< 3 DoF joint that uses Euler ZXY convention (faster than emulated multi DoF joints). + JointTypeTranslationXYZ, + JointTypeFloatingBase, ///< A 6-DoF joint for floating-base (or freeflyer) systems. + JointTypeFixed, ///< Fixed joint which causes the inertial properties to be merged with the parent body. + JointTypeHelical, //1 DoF joint with both rotational and translational motion + JointType1DoF, + JointType2DoF, ///< Emulated 2 DoF joint. + JointType3DoF, ///< Emulated 3 DoF joint. + JointType4DoF, ///< Emulated 4 DoF joint. + JointType5DoF, ///< Emulated 5 DoF joint. + JointType6DoF, ///< Emulated 6 DoF joint. + JointTypeCustom ///< User defined joints of varying size + }; + +/** \brief Describes a joint relative to the predecessor body. + * + * This class contains all information required for one single joint. This + * contains the joint type and the axis of the joint. See \ref joint_description for detailed description. + * + */ +struct RBDL_DLLAPI Joint { + Joint() : + mJointAxes (NULL), + mJointType (JointTypeUndefined), + mDoFCount (0), + q_index (0) {}; + Joint (JointType type) : + mJointAxes (NULL), + mJointType (type), + mDoFCount (0), + q_index (0), + custom_joint_index(-1) { + if (type == JointTypeRevoluteX) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeRevoluteY) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + } else if (type == JointTypeRevoluteZ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeSpherical) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeEulerZYX) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + } else if (type == JointTypeEulerXYZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeEulerYXZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + } else if (type == JointTypeEulerZXY) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.); + mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.); + mJointAxes[2] = Math::SpatialVector (0., 1., 0., 0., 0., 0.); + } else if (type == JointTypeTranslationXYZ) { + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = Math::SpatialVector (0., 0., 0., 1., 0., 0.); + mJointAxes[1] = Math::SpatialVector (0., 0., 0., 0., 1., 0.); + mJointAxes[2] = Math::SpatialVector (0., 0., 0., 0., 0., 1.); + } else if (type >= JointType1DoF && type <= JointType6DoF) { + // create a joint and allocate memory for it. + // Warning: the memory does not get initialized by this function! + mDoFCount = type - JointType1DoF + 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + std::cerr << "Warning: uninitalized vector" << std::endl; + } else if (type == JointTypeCustom) { + //This constructor cannot be used for a JointTypeCustom because + //we have no idea what mDoFCount is. + std::cerr << "Error: Invalid use of Joint constructor Joint(JointType" + << " type). Only allowed when type != JointTypeCustom" + << std::endl; + assert(0); + abort(); + } else if (type != JointTypeFixed && type != JointTypeFloatingBase) { + std::cerr << "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed or JointTypeSpherical." << std::endl; + assert (0); + abort(); + } + } + Joint (JointType type, int degreesOfFreedom) : + mJointAxes (NULL), + mJointType (type), + mDoFCount (0), + q_index (0), + custom_joint_index(-1) { + if (type == JointTypeCustom) { + mDoFCount = degreesOfFreedom; + mJointAxes = new Math::SpatialVector[mDoFCount]; + for(unsigned int i=0; i 0) { + assert (mJointAxes); + delete[] mJointAxes; + } + mJointType = joint.mJointType; + mDoFCount = joint.mDoFCount; + custom_joint_index = joint.custom_joint_index; + mJointAxes = new Math::SpatialVector[mDoFCount]; + + for (unsigned int i = 0; i < mDoFCount; i++) + mJointAxes[i] = joint.mJointAxes[i]; + + q_index = joint.q_index; + } + return *this; + } + ~Joint() { + if (mJointAxes) { + assert (mJointAxes); + delete[] mJointAxes; + mJointAxes = NULL; + mDoFCount = 0; + custom_joint_index = -1; + } + } + + /** \brief Constructs a joint from the given cartesian parameters. + * + * This constructor creates all the required spatial values for the given + * cartesian parameters. + * + * \param joint_type whether the joint is revolute or prismatic + * \param joint_axis the axis of rotation or translation + */ + Joint ( + const JointType joint_type, + const Math::Vector3d &joint_axis + ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + + // Some assertions, as we concentrate on simple cases + + // Only rotation around the Z-axis + assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic ); + + mJointType = joint_type; + + if (joint_type == JointTypeRevolute) { + // make sure we have a unit axis + mJointAxes[0].set ( + joint_axis[0], + joint_axis[1], + joint_axis[2], + 0., 0., 0. + ); + + } else if (joint_type == JointTypePrismatic) { + // make sure we have a unit axis + assert (joint_axis.squaredNorm() == 1.); + + mJointAxes[0].set ( + 0., 0., 0., + joint_axis[0], + joint_axis[1], + joint_axis[2] + ); + } + } + + /** \brief Constructs a 1 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \param axis_0 Motion subspace for axis 0 + */ + Joint ( + const Math::SpatialVector &axis_0 + ) { + mDoFCount = 1; + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = Math::SpatialVector (axis_0); + if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.)) { + mJointType = JointTypeRevoluteX; + } else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.)) { + mJointType = JointTypeRevoluteY; + } else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.)) { + mJointType = JointTypeRevoluteZ; + } else if (axis_0[0] == 0 && + axis_0[1] == 0 && + axis_0[2] == 0) { + mJointType = JointTypePrismatic; + } else { + mJointType = JointTypeHelical; + } + validate_spatial_axis (mJointAxes[0]); + } + + /** \brief Constructs a 2 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1 + ) { + mJointType = JointType2DoF; + mDoFCount = 2; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + } + + /** \brief Constructs a 3 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2 + ) { + mJointType = JointType3DoF; + mDoFCount = 3; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + } + + /** \brief Constructs a 4 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3 + ) { + mJointType = JointType4DoF; + mDoFCount = 4; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + } + + /** \brief Constructs a 5 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + * \param axis_4 Motion subspace for axis 4 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3, + const Math::SpatialVector &axis_4 + ) { + mJointType = JointType5DoF; + mDoFCount = 5; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + mJointAxes[4] = axis_4; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + validate_spatial_axis (mJointAxes[4]); + } + + /** \brief Constructs a 6 DoF joint with the given motion subspaces. + * + * The motion subspaces are of the format: + * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f] + * + * \note So far only pure rotations or pure translations are supported. + * + * \param axis_0 Motion subspace for axis 0 + * \param axis_1 Motion subspace for axis 1 + * \param axis_2 Motion subspace for axis 2 + * \param axis_3 Motion subspace for axis 3 + * \param axis_4 Motion subspace for axis 4 + * \param axis_5 Motion subspace for axis 5 + */ + Joint ( + const Math::SpatialVector &axis_0, + const Math::SpatialVector &axis_1, + const Math::SpatialVector &axis_2, + const Math::SpatialVector &axis_3, + const Math::SpatialVector &axis_4, + const Math::SpatialVector &axis_5 + ) { + mJointType = JointType6DoF; + mDoFCount = 6; + + mJointAxes = new Math::SpatialVector[mDoFCount]; + + mJointAxes[0] = axis_0; + mJointAxes[1] = axis_1; + mJointAxes[2] = axis_2; + mJointAxes[3] = axis_3; + mJointAxes[4] = axis_4; + mJointAxes[5] = axis_5; + + validate_spatial_axis (mJointAxes[0]); + validate_spatial_axis (mJointAxes[1]); + validate_spatial_axis (mJointAxes[2]); + validate_spatial_axis (mJointAxes[3]); + validate_spatial_axis (mJointAxes[4]); + validate_spatial_axis (mJointAxes[5]); + } + + /** \brief Checks whether we have pure rotational or translational axis. + * + * This function is mainly used to print out warnings when specifying an + * axis that might not be intended. + */ + bool validate_spatial_axis (Math::SpatialVector &axis) { + + bool axis_rotational = false; + bool axis_translational = false; + + Math::Vector3d rotation (axis[0], axis[1], axis[2]); + Math::Vector3d translation (axis[3], axis[4], axis[5]); + + if (fabs(rotation.norm()) > 1.0e-8) + axis_rotational = true; + + if (fabs(translation.norm()) > 1.0e-8) + axis_translational = true; + + if(axis_rotational && rotation.norm() - 1.0 > 1.0e-8) { + std::cerr << "Warning: joint rotation axis is not unit!" << std::endl; + } + + if(axis_translational && translation.norm() - 1.0 > 1.0e-8) { + std::cerr << "Warning: joint translation axis is not unit!" << std::endl; + } + + return axis_rotational != axis_translational; + } + + /// \brief The spatial axes of the joint + Math::SpatialVector* mJointAxes; + /// \brief Type of joint + JointType mJointType; + /// \brief Number of degrees of freedom of the joint. Note: CustomJoints + // have here a value of 0 and their actual numbers of degrees of freedom + // can be obtained using the CustomJoint structure. + unsigned int mDoFCount; + unsigned int q_index; + int custom_joint_index; +}; + +/** \brief Computes all variables for a joint model + * + * By appropriate modification of this function all types of joints can be + * modeled. See RBDA Section 4.4 for details. + * + * \param model the rigid body model + * \param joint_id the id of the joint we are interested in. This will be used to determine the type of joint and also the entries of \f[ q, \dot{q} \f]. + * \param q joint state variables + * \param qdot joint velocity variables + */ +RBDL_DLLAPI +void jcalc ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ); + +RBDL_DLLAPI +Math::SpatialTransform jcalc_XJ ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q); + +RBDL_DLLAPI +void jcalc_X_lambda_S ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ); + +struct RBDL_DLLAPI CustomJoint { + CustomJoint() + { } + virtual ~CustomJoint() {}; + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ) = 0; + virtual void jcalc_X_lambda_S (Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ) = 0; + + unsigned int mDoFCount; + Math::SpatialTransform XJ; + Math::MatrixNd S; + Math::MatrixNd U; + Math::MatrixNd Dinv; + Math::VectorNd u; + Math::VectorNd d_u; +}; + +} + +/* RBDL_JOINT_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Kinematics.h b/3rdparty/rbdl/include/rbdl/Kinematics.h new file mode 100644 index 0000000..51bf123 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Kinematics.h @@ -0,0 +1,415 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_KINEMATICS_H +#define RBDL_KINEMATICS_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { + +/** \page kinematics_page Kinematics + * All functions related to kinematics are specified in the \ref + * kinematics_group "Kinematics Module". + * + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + * + * \defgroup kinematics_group Kinematics + * @{ + * + * \note Please note that in the Rigid %Body Dynamics Library all angles + * are specified in radians. + */ + +/** \brief Updates and computes velocities and accelerations of the bodies + * + * This function updates the kinematic variables such as body velocities + * and accelerations in the model to reflect the variables passed to this function. + * + * \param model the model + * \param Q the positional variables of the model + * \param QDot the generalized velocities of the joints + * \param QDDot the generalized accelerations of the joints + */ +RBDL_DLLAPI void UpdateKinematics (Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot + ); + +/** \brief Selectively updates model internal states of body positions, velocities and/or accelerations. + * + * This function updates the kinematic variables such as body velocities and + * accelerations in the model to reflect the variables passed to this function. + * + * In contrast to UpdateKinematics() this function allows to update the model + * state with values one is interested and thus reduce computations (e.g. only + * positions, only positions + accelerations, only velocities, etc.). + + * \param model the model + * \param Q the positional variables of the model + * \param QDot the generalized velocities of the joints + * \param QDDot the generalized accelerations of the joints + */ +RBDL_DLLAPI void UpdateKinematicsCustom (Model &model, + const Math::VectorNd *Q, + const Math::VectorNd *QDot, + const Math::VectorNd *QDDot + ); + +/** \brief Returns the base coordinates of a point given in body coordinates. + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param body_point_position coordinates of the point in body coordinates + * \param update_kinematics whether UpdateKinematics() should be called + * or not (default: true) + * + * \returns a 3-D vector with coordinates of the point in base coordinates + */ +RBDL_DLLAPI Math::Vector3d CalcBodyToBaseCoordinates ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &body_point_position, + bool update_kinematics = true); + +/** \brief Returns the body coordinates of a point given in base coordinates. + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param base_point_position coordinates of the point in base coordinates + * \param update_kinematics whether UpdateKinematics() should be called or not + * (default: true). + * + * \returns a 3-D vector with coordinates of the point in body coordinates + */ +RBDL_DLLAPI Math::Vector3d CalcBaseToBodyCoordinates ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &base_point_position, + bool update_kinematics = true); + +/** \brief Returns the orientation of a given body as 3x3 matrix + * + * \param model the rigid body model + * \param Q the curent genereralized positions + * \param body_id id of the body for which the point coordinates are expressed + * \param update_kinematics whether UpdateKinematics() should be called or not + * (default: true). + * + * \returns An orthonormal 3x3 matrix that rotates vectors from base coordinates + * to body coordinates. + */ +RBDL_DLLAPI Math::Matrix3d CalcBodyWorldOrientation ( + Model &model, + const Math::VectorNd &Q, + const unsigned int body_id, + bool update_kinematics = true); + +/** \brief Computes the point jacobian for a point on a body + * + * If a position of a point is computed by a function \f$g(q(t))\f$ for which its + * time derivative is \f$\frac{d}{dt} g(q(t)) = G(q)\dot{q}\f$ then this + * function computes the jacobian matrix \f$G(q)\f$. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param G a matrix of dimensions 3 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + * + */ +RBDL_DLLAPI void CalcPointJacobian (Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &point_position, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes a 6-D Jacobian for a point on a body + * + * Computes the 6-D Jacobian \f$G(q)\f$ that when multiplied with + * \f$\dot{q}\f$ gives a 6-D vector that has the angular velocity as the + * first three entries and the linear velocity as the last three entries. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param G a matrix of dimensions 6 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + * + */ +RBDL_DLLAPI void CalcPointJacobian6D (Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + const Math::Vector3d &point_position, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes the spatial jacobian for a body + * + * The spatial velocity of a body at the origin of coordinate system of + * body \f$i\f$ can be expressed as \f${}^i \hat{v}_i = G(q) * \dot{q}\f$. + * The matrix \f$G(q)\f$ is called the spatial body jacobian of the body + * and can be computed using this function. + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param body_id the id of the body + * \param G a matrix of size 6 x \#qdot_size where the result will be stored in + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * The result will be returned via the G argument and represents the + * body Jacobian expressed at the origin of the body. + * + * \note This function only evaluates the entries of G that are non-zero. One + * Before calling this function one has to ensure that all other values + * have been set to zero, e.g. by calling G.setZero(). + */ +RBDL_DLLAPI void CalcBodySpatialJacobian ( + Model &model, + const Math::VectorNd &Q, + unsigned int body_id, + Math::MatrixNd &G, + bool update_kinematics = true + ); + +/** \brief Computes the velocity of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The cartesian velocity of the point in global frame (output) + */ +RBDL_DLLAPI Math::Vector3d CalcPointVelocity ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes angular and linear velocity of a point that is fixed on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The a 6-D vector for which the first three elements are the + * angular velocity and the last three elements the linear velocity in the + * global reference system. + */ +RBDL_DLLAPI + Math::SpatialVector CalcPointVelocity6D ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes the linear acceleration of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns The cartesian acceleration of the point in global frame (output) + * + * The kinematic state of the model has to be updated before valid + * values can be obtained. This can either be done by calling + * UpdateKinematics() or setting the last parameter update_kinematics to + * true (default). + * + * \warning If this function is called after ForwardDynamics() without + * an update of the kinematic state one has to add the gravity + * acceleration has to be added to the result. + */ +RBDL_DLLAPI + Math::Vector3d CalcPointAcceleration ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes linear and angular acceleration of a point on a body + * + * \param model rigid body model + * \param Q state vector of the internal joints + * \param QDot velocity vector of the internal joints + * \param QDDot velocity vector of the internal joints + * \param body_id the id of the body + * \param point_position the position of the point in body-local data + * \param update_kinematics whether UpdateKinematics() should be called or not (default: true) + * + * \returns A 6-D vector where the first three elements are the angular + * acceleration and the last three elements the linear accelerations of + * the point. + * + * The kinematic state of the model has to be updated before valid + * values can be obtained. This can either be done by calling + * UpdateKinematics() or setting the last parameter update_kinematics to + * true (default). + * + * \warning If this function is called after ForwardDynamics() without + * an update of the kinematic state one has to add the gravity + * acceleration has to be added to the result. + */ +RBDL_DLLAPI + Math::SpatialVector CalcPointAcceleration6D ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &QDDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics = true + ); + +/** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) + * + * \param model rigid body model + * \param Qinit initial guess for the state + * \param body_id a vector of all bodies for which we we have kinematic target positions + * \param body_point a vector of points in body local coordinates that are + * to be matched to target positions + * \param target_pos a vector of target positions + * \param Qres output of the computed inverse kinematics + * \param step_tol tolerance used for convergence detection + * \param lambda damping factor for the least squares function + * \param max_iter maximum number of steps that should be performed + * \returns true on success, false otherwise + * + * This function repeatedly computes + * \f[ Qres = Qres + \Delta \theta\f] + * \f[ \Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e \f] + * where \f$G = G(q) = \frac{d}{dt} g(q(t))\f$ and \f$e\f$ is the + * correction of the body points so that they coincide with the target + * positions. The function returns true when \f$||\Delta \theta||_2 \le\f$ + * step_tol or if the error between body points and target gets smaller + * than step_tol. Otherwise it returns false. + * + * The parameter \f$\lambda\f$ is the damping factor that has to + * be chosen carefully. In case of unreachable positions higher values (e.g + * 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might + * yield good results. See the literature for best practices. + * + * \warning The actual accuracy might be rather low (~1.0e-2)! Use this function with a + * grain of suspicion. + */ +RBDL_DLLAPI + bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + const std::vector& body_id, + const std::vector& body_point, + const std::vector& target_pos, + Math::VectorNd &Qres, + double step_tol = 1.0e-12, + double lambda = 0.01, + unsigned int max_iter = 50 + ); + +RBDL_DLLAPI Math::Vector3d CalcAngularVelocityfromMatrix ( + const Math::Matrix3d &RotMat); + +struct RBDL_DLLAPI InverseKinematicsConstraintSet { + enum ConstraintType { + ConstraintTypePosition = 0, + ConstraintTypeOrientation, + ConstraintTypeFull + }; + + InverseKinematicsConstraintSet(); + + Math::MatrixNd J; /// the Jacobian of all constraints + Math::MatrixNd G; /// temporary storage of a single body Jacobian + Math::VectorNd e; /// Vector with all the constraint residuals. + + unsigned int num_constraints; //size of all constraints + double lambda; /// Damping factor, the default value of 1.0e-6 is reasonable for most problems + unsigned int num_steps; // The number of iterations performed + unsigned int max_steps; // Maximum number of steps (default 300), abort if more steps are performed. + double step_tol; // Step tolerance (default = 1.0e-12). If the computed step length is smaller than this value the algorithm terminates successfully (i.e. returns true). If error_norm is still larger than constraint_tol then this usually means that the target is unreachable. + double constraint_tol; // Constraint tolerance (default = 1.0e-12). If error_norm is smaller than this value the algorithm terminates successfully, i.e. all constraints are satisfied. + double error_norm; // Norm of the constraint residual vector. + + // everything to define a IKin constraint + std::vector constraint_type; + std::vector body_ids; + std::vector body_points; + std::vector target_positions; + std::vector target_orientations; + std::vector constraint_row_index; + + // Adds a point constraint that tries to get a body point close to a + // point described in base coordinates. + unsigned int AddPointConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos); + // Adds an orientation constraint that tries to align a body to the + // orientation specified as a rotation matrix expressed in base + // coordinates. + unsigned int AddOrientationConstraint (unsigned int body_id, const Math::Matrix3d &target_orientation); + // Adds a constraint on both location and orientation of a body. + unsigned int AddFullConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, const Math::Matrix3d &target_orientation); + // Clears all entries of the constraint setting + unsigned int ClearConstraints(); +}; + +RBDL_DLLAPI bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + InverseKinematicsConstraintSet &CS, + Math::VectorNd &Qres + ); + +/** @} */ + +} + +/* RBDL_KINEMATICS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Logging.h b/3rdparty/rbdl/include/rbdl/Logging.h new file mode 100644 index 0000000..b7dd751 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Logging.h @@ -0,0 +1,74 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_LOGGING_H +#define RBDL_LOGGING_H + +#include +#include + +class LoggingGuard; + +/** \def RBDL_ENABLE_LOGGING + * + * Enables/Disables logging + * + * \warning Logging has a huge impact on performance. + */ +#ifndef RBDL_ENABLE_LOGGING +#define LOG if (false) LogOutput +#define SUPPRESS_LOGGING ; +#else +#define LOG LogOutput +#define SUPPRESS_LOGGING LoggingGuard _nolog +#endif + +extern RBDL_DLLAPI std::ostringstream LogOutput; +RBDL_DLLAPI void ClearLogOutput (); + +/** \brief Helper object to ignore any logs that happen during its lifetime + * + * If an instance of this class exists all logging gets suppressed. This + * allows to disable logging for a certain scope or a single function call, + * e.g. + * + * \code + * { + * // logging will be active + * do_some_stuff(); + * + * // now create a new scope in which a LoggingGuard instance exists + * { + * LoggingGuard ignore_logging; + * + * // as a _Nologging instance exists, all logging will be discarded + * do_some_crazy_stuff(); + * } + * + * // logging will be active again + * do_some_more_stuff(); + * } + * \endcode + * + */ +class RBDL_DLLAPI LoggingGuard { + public: + LoggingGuard() { + log_backup.str(""); + log_backup << LogOutput.str(); + } + ~LoggingGuard() { + LogOutput.str(""); + LogOutput << log_backup.str(); + } + + private: + std::ostringstream log_backup; +}; + +/* RBDL_LOGGING_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Model.h b/3rdparty/rbdl/include/rbdl/Model.h new file mode 100644 index 0000000..7fd27c6 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Model.h @@ -0,0 +1,492 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MODEL_H +#define RBDL_MODEL_H + +#include "rbdl/rbdl_math.h" +#include +#include +#include +#include +#include +#include + +#include "rbdl/Logging.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" + +// std::vectors containing any objects that have Eigen matrices or vectors +// as members need to have a special allocater. This can be achieved with +// the following macro. + +#ifdef EIGEN_CORE_H +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody) +#endif + +/** \brief Namespace for all structures of the RigidBodyDynamics library +*/ +namespace RigidBodyDynamics { + +/** \page modeling_page Model + * + * \section model_structure Model Structure + * + * RBDL stores the model internally in the \link RigidBodyDynamics::Model + * Model Structure\endlink. For each \link RigidBodyDynamics::Body Body + * \endlink it contains spatial velocities, accelerations and other + * variables that describe the state of the rigid body system. Furthermore + * it contains variables that are used as temporary variables in the + * algorithms. + * + * There are multiple ways of creating \link RigidBodyDynamics::Model Models\endlink for RBDL: + * + * \li Loading models from Lua files using the \ref luamodel_introduction + * "LuaModel" addon + * \li Loading models from URDF (the Unified Robot Description Format) xml + * files or strings using the URDFReader addon + * \li using the C++ interface. + * + * The first approach requires the addon \ref luamodel_introduction to be + * activated which is done by enabling BUILD_ADDON_LUAMODEL in CMake and is + * recommended when one is not interested in the details of RBDL and simply + * wants to create a model. + * + * \section modeling_cpp Modeling using C++ + * + * The construction of \link RigidBodyDynamics::Model Model Structures + * \endlink makes use of carefully designed constructors of the classes + * \link RigidBodyDynamics::Body Body \endlink and \link + * RigidBodyDynamics::Joint Joint \endlink to ease the process of + * creating articulated models. + * + * \link RigidBodyDynamics::Body Bodies \endlink are created by calling one + * of its constructors. Usually they are created by specifying the mass, + * center of mass and the inertia at the center of mass. + * \link RigidBodyDynamics::Joint Joints \endlink are similarly created and is + * described in detail in \ref joint_description. + * + * Adding bodies to the model is done by specifying the + * parent body by its id, the transformation from the parent origin to the + * joint origin, the joint specification as an object, and the body itself. + * These parameters are then fed to the function + * RigidBodyDynamics::Model::AddBody() or + * RigidBodyDynamics::Model::AppendBody(). + * + * To create a model with a floating base (a.k.a a model with a free-flyer + * joint) it is recommended to use a joint of type JointTypeFloatingBase as + * the first joint. + * + * Once this is done, the model structure can be used with the functions of \ref + * kinematics_group, \ref dynamics_group, \ref constraints_group, to perform + * computations. + * + * A simple example can be found \ref SimpleExample "here". + * + * \section modeling_lua Using LuaModels + * + * For this see the documentation of \ref luamodel_introduction,\link + * RigidBodyDynamics::Addons::LuaModelReadFromFile \endlink, and \link + * RigidBodyDynamics::Addons::LuaModelReadFromFileWithConstraints \endlink. + + * \section modeling_urdf Using URDF + * + * For this see the documentation see \link + * RigidBodyDynamics::Addons::URDFReadFromFile \endlink. + */ + +/** \brief Contains all information about the rigid body model + * + * This class contains all information required to perform the forward + * dynamics calculation. The variables in this class are also used for + * storage of temporary values. It is designed for use of the Articulated + * Rigid Body Algorithm (which is implemented in ForwardDynamics()) and + * follows the numbering as described in Featherstones book. + * + * Please note that body 0 is the root body and the moving bodies start at + * index 1. This numbering scheme is very beneficial in terms of + * readability of the code as the resulting code is very similar to the + * pseudo-code in the RBDA book. The generalized variables q, qdot, qddot + * and tau however start at 0 such that the first entry (e.g. q[0]) always + * specifies the value for the first moving body. + * + * \note To query the number of degrees of freedom use Model::dof_count. + */ +struct RBDL_DLLAPI Model { + Model(); + + // Structural information + + /// \brief The id of the parents body + std::vector lambda; + /** \brief The index of the parent degree of freedom that is directly + influencing the current one*/ + std::vector lambda_q; + /// \brief Contains the ids of all the children of a given body + std::vector >mu; + + /** \brief number of degrees of freedoms of the model + * + * This value contains the number of entries in the generalized state (q) + * velocity (qdot), acceleration (qddot), and force (tau) vector. + */ + unsigned int dof_count; + + /** \brief The size of the \f$\mathbf{q}\f$-vector. + * For models without spherical joints the value is the same as + * Model::dof_count, otherwise additional values for the w-component of the + * Quaternion is stored at the end of \f$\mathbf{q}\f$. + * + * \sa \ref joint_description for more details. + */ + unsigned int q_size; + /** \brief The size of the + * + * (\f$\mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, + * and \f$\mathbf{\tau}\f$-vector. + * + * \sa \ref joint_description for more details. + */ + unsigned int qdot_size; + + /// \brief Id of the previously added body, required for Model::AppendBody() + unsigned int previously_added_body_id; + + /// \brief the cartesian vector of the gravity + Math::Vector3d gravity; + + // State information + /// \brief The spatial velocity of the bodies + std::vector v; + /// \brief The spatial acceleration of the bodies + std::vector a; + + //////////////////////////////////// + // Joints + + /// \brief All joints + + std::vector mJoints; + /// \brief The joint axis for joint i + std::vector S; + + // Joint state variables + std::vector v_J; + std::vector c_J; + + std::vector mJointUpdateOrder; + + /// \brief Transformations from the parent body to the frame of the joint. + // It is expressed in the coordinate frame of the parent. + std::vector X_T; + /// \brief The number of fixed joints that have been declared before + /// each joint. + std::vector mFixedJointCount; + + //////////////////////////////////// + // Special variables for joints with 3 degrees of freedom + /// \brief Motion subspace for joints with 3 degrees of freedom + std::vector multdof3_S; + std::vector multdof3_U; + std::vector multdof3_Dinv; + std::vector multdof3_u; + std::vector multdof3_w_index; + + std::vector mCustomJoints; + + //////////////////////////////////// + // Dynamics variables + + /// \brief The velocity dependent spatial acceleration + std::vector c; + /// \brief The spatial inertia of the bodies + std::vector IA; + /// \brief The spatial bias force + std::vector pA; + /// \brief Temporary variable U_i (RBDA p. 130) + std::vector U; + /// \brief Temporary variable D_i (RBDA p. 130) + Math::VectorNd d; + /// \brief Temporary variable u (RBDA p. 130) + Math::VectorNd u; + /// \brief Internal forces on the body (used only InverseDynamics()) + std::vector f; + /// \brief The spatial inertia of body i (used only in + /// CompositeRigidBodyAlgorithm()) + std::vector I; + std::vector Ic; + std::vector hc; + std::vector hdotc; + + //////////////////////////////////// + // Bodies + + /** \brief Transformation from the parent body to the current body + * \f[ + * X_{\lambda(i)} = {}^{i} X_{\lambda(i)} + * \f] + */ + std::vector X_lambda; + /// \brief Transformation from the base to bodies reference frame + std::vector X_base; + + /// \brief All bodies that are attached to a body via a fixed joint. + std::vector mFixedBodies; + /** \brief Value that is used to discriminate between fixed and movable + * bodies. + * + * Bodies with id 1 .. (fixed_body_discriminator - 1) are moving bodies + * while bodies with id fixed_body_discriminator .. max (unsigned int) + * are fixed to a moving body. The value of max(unsigned int) is + * determined via std::numeric_limits::max() and the + * default value of fixed_body_discriminator is max (unsigned int) / 2. + * + * On normal systems max (unsigned int) is 4294967294 which means there + * could be a total of 2147483646 movable and / or fixed bodies. + */ + unsigned int fixed_body_discriminator; + + /** \brief All bodies 0 ... N_B, including the base + * + * mBodies[0] - base body
+ * mBodies[1] - 1st moveable body
+ * ...
+ * mBodies[N_B] - N_Bth moveable body
+ */ + std::vector mBodies; + + /// \brief Human readable names for the bodies + std::map mBodyNameMap; + + /** \brief Connects a given body to the model + * + * When adding a body there are basically informations required: + * - what kind of body will be added? + * - where is the new body to be added? + * - by what kind of joint should the body be added? + * + * The first information "what kind of body will be added" is contained + * in the Body class that is given as a parameter. + * + * The question "where is the new body to be added?" is split up in two + * parts: first the parent (or successor) body to which it is added and + * second the transformation to the origin of the joint that connects the + * two bodies. With these two informations one specifies the relative + * positions of the bodies when the joint is in neutral position.gk + * + * The last question "by what kind of joint should the body be added?" is + * again simply contained in the Joint class. + * + * \param parent_id id of the parent body + * \param joint_frame the transformation from the parent frame to the origin + * of the joint frame (represents X_T in RBDA) + * \param joint specification for the joint that describes the + * connection + * \param body specification of the body itself + * \param body_name human readable name for the body (can be used to + * retrieve its id with GetBodyId()) + * + * \returns id of the added body + */ + unsigned int AddBody ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + unsigned int AddBodySphericalJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + /** \brief Adds a Body to the model such that the previously added Body + * is the Parent. + * + * This function is basically the same as Model::AddBody() however the + * most recently added body (or body 0) is taken as parent. + */ + unsigned int AppendBody ( + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name = "" + ); + + unsigned int AddBodyCustomJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + CustomJoint *custom_joint, + const Body &body, + std::string body_name = "" + ); + + /** \brief Returns the id of a body that was passed to AddBody() + * + * Bodies can be given a human readable name. This function allows to + * resolve its name to the numeric id. + * + * \note Instead of querying this function repeatedly, it might be + * advisable to query it once and reuse the returned id. + * + * \returns the id of the body or \c std::numeric_limits\::max() if the id was not found. + */ + unsigned int GetBodyId (const char *body_name) const { + if (mBodyNameMap.count(body_name) == 0) { + return std::numeric_limits::max(); + } + + return mBodyNameMap.find(body_name)->second; + } + + /** \brief Returns the name of a body for a given body id */ + std::string GetBodyName (unsigned int body_id) const { + std::map::const_iterator iter + = mBodyNameMap.begin(); + + while (iter != mBodyNameMap.end()) { + if (iter->second == body_id) + return iter->first; + + iter++; + } + + return ""; + } + + /** \brief Checks whether the body is rigidly attached to another body. + */ + bool IsFixedBodyId (unsigned int body_id) { + if (body_id >= fixed_body_discriminator + && body_id < std::numeric_limits::max() + && body_id - fixed_body_discriminator < mFixedBodies.size()) { + return true; + } + return false; + } + + bool IsBodyId (unsigned int id) { + if (id > 0 && id < mBodies.size()) + return true; + if (id >= fixed_body_discriminator + && id < std::numeric_limits::max()) { + if (id - fixed_body_discriminator < mFixedBodies.size()) + return true; + } + return false; + } + + /** Determines id the actual parent body. + * + * When adding bodies using joints with multiple degrees of + * freedom, additional virtual bodies are added for each degree of + * freedom. This function returns the id of the actual + * non-virtual parent body. + */ + unsigned int GetParentBodyId (unsigned int id) { + if (id >= fixed_body_discriminator) { + return mFixedBodies[id - fixed_body_discriminator].mMovableParent; + } + + unsigned int parent_id = lambda[id]; + + while (mBodies[parent_id].mIsVirtual) { + parent_id = lambda[parent_id]; + } + + return parent_id; + } + + /** Returns the joint frame transformtion, i.e. the second argument to + Model::AddBody(). + */ + Math::SpatialTransform GetJointFrame (unsigned int id) { + if (id >= fixed_body_discriminator) { + return mFixedBodies[id - fixed_body_discriminator].mParentTransform; + } + + unsigned int child_id = id; + unsigned int parent_id = lambda[id]; + if (mBodies[parent_id].mIsVirtual) { + while (mBodies[parent_id].mIsVirtual) { + child_id = parent_id; + parent_id = lambda[child_id]; + } + return X_T[child_id]; + } else + return X_T[id]; + } + + /** Sets the joint frame transformtion, i.e. the second argument to + Model::AddBody(). + */ + void SetJointFrame (unsigned int id, + const Math::SpatialTransform &transform) { + if (id >= fixed_body_discriminator) { + std::cerr << "Error: setting of parent transform " + << "not supported for fixed bodies!" << std::endl; + abort(); + } + + unsigned int child_id = id; + unsigned int parent_id = lambda[id]; + if (mBodies[parent_id].mIsVirtual) { + while (mBodies[parent_id].mIsVirtual) { + child_id = parent_id; + parent_id = lambda[child_id]; + } + X_T[child_id] = transform; + } else if (id > 0) { + X_T[id] = transform; + } + } + + /** Gets the quaternion for body i (only valid if body i is connected by + * a JointTypeSpherical joint) + * + * See \ref joint_singularities for details. + */ + Math::Quaternion GetQuaternion (unsigned int i, + const Math::VectorNd &Q) const { + assert (mJoints[i].mJointType == JointTypeSpherical); + unsigned int q_index = mJoints[i].q_index; + return Math::Quaternion ( Q[q_index], + Q[q_index + 1], + Q[q_index + 2], + Q[multdof3_w_index[i]]); + } + + /** Sets the quaternion for body i (only valid if body i is connected by + * a JointTypeSpherical joint) + * + * See \ref joint_singularities for details. + */ + void SetQuaternion (unsigned int i, + const Math::Quaternion &quat, + Math::VectorNd &Q) const { + assert (mJoints[i].mJointType == JointTypeSpherical); + unsigned int q_index = mJoints[i].q_index; + + Q[q_index] = quat[0]; + Q[q_index + 1] = quat[1]; + Q[q_index + 2] = quat[2]; + Q[multdof3_w_index[i]] = quat[3]; + } +}; + +/** @} */ +} + +/* _MODEL_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/Quaternion.h b/3rdparty/rbdl/include/rbdl/Quaternion.h new file mode 100644 index 0000000..324a554 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/Quaternion.h @@ -0,0 +1,211 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_QUATERNION_H +#define RBDL_QUATERNION_H + +#include + +namespace RigidBodyDynamics { + +namespace Math { + +/** \brief Quaternion that are used for \ref joint_singularities "singularity free" joints. + * + * order: x,y,z,w + */ +class Quaternion : public Vector4d { + public: + Quaternion () : + Vector4d (0., 0., 0., 1.) + {} + Quaternion (const Vector4d &vec4) : + Vector4d (vec4) + {} + Quaternion (double x, double y, double z, double w): + Vector4d (x, y, z, w) + {} + Quaternion operator* (const double &s) const { + return Quaternion ( + (*this)[0] * s, + (*this)[1] * s, + (*this)[2] * s, + (*this)[3] * s + ); + } + /** This function is equivalent to multiplicate their corresponding rotation matrices */ + Quaternion operator* (const Quaternion &q) const { + return Quaternion ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + } + Quaternion& operator*=(const Quaternion &q) { + set ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + return *this; + } + + static Quaternion fromGLRotate (double angle, double x, double y, double z) { + double st = std::sin (angle * M_PI / 360.); + return Quaternion ( + st * x, + st * y, + st * z, + std::cos (angle * M_PI / 360.) + ); + } + + Quaternion slerp (double alpha, const Quaternion &quat) const { + // check whether one of the two has 0 length + double s = std::sqrt (squaredNorm() * quat.squaredNorm()); + + // division by 0.f is unhealthy! + assert (s != 0.); + + double angle = acos (dot(quat) / s); + if (angle == 0. || std::isnan(angle)) { + return *this; + } + assert(!std::isnan(angle)); + + double d = 1. / std::sin (angle); + double p0 = std::sin ((1. - alpha) * angle); + double p1 = std::sin (alpha * angle); + + if (dot (quat) < 0.) { + return Quaternion( ((*this) * p0 - quat * p1) * d); + } + return Quaternion( ((*this) * p0 + quat * p1) * d); + } + + static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) { + double d = axis.norm(); + double s2 = std::sin (angle_rad * 0.5) / d; + return Quaternion ( + axis[0] * s2, + axis[1] * s2, + axis[2] * s2, + std::cos(angle_rad * 0.5) + ); + } + + static Quaternion fromMatrix (const Matrix3d &mat) { + double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5; + return Quaternion ( + (mat(1,2) - mat(2,1)) / (w * 4.), + (mat(2,0) - mat(0,2)) / (w * 4.), + (mat(0,1) - mat(1,0)) / (w * 4.), + w); + } + + static Quaternion fromZYXAngles (const Vector3d &zyx_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]); + } + + static Quaternion fromYXZAngles (const Vector3d &yxz_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), yxz_angles[0]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), yxz_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), yxz_angles[2]); + } + + static Quaternion fromXYZAngles (const Vector3d &xyz_angles) { + return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), xyz_angles[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), xyz_angles[0]); + } + + Matrix3d toMatrix() const { + double x = (*this)[0]; + double y = (*this)[1]; + double z = (*this)[2]; + double w = (*this)[3]; + return Matrix3d ( + 1 - 2*y*y - 2*z*z, + 2*x*y + 2*w*z, + 2*x*z - 2*w*y, + + 2*x*y - 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z + 2*w*x, + + 2*x*z + 2*w*y, + 2*y*z - 2*w*x, + 1 - 2*x*x - 2*y*y + + /* + 1 - 2*y*y - 2*z*z, + 2*x*y - 2*w*z, + 2*x*z + 2*w*y, + + 2*x*y + 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z - 2*w*x, + + 2*x*z - 2*w*y, + 2*y*z + 2*w*x, + 1 - 2*x*x - 2*y*y + */ + ); + } + + Quaternion conjugate() const { + return Quaternion ( + -(*this)[0], + -(*this)[1], + -(*this)[2], + (*this)[3]); + } + + Quaternion timeStep (const Vector3d &omega, double dt) { + double omega_norm = omega.norm(); + return Quaternion::fromAxisAngle (omega / omega_norm, dt * omega_norm) * (*this); + } + + Vector3d rotate (const Vector3d &vec) const { + Vector3d vn (vec); + Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; + + res_quat = vec_quat * (*this); + res_quat = conjugate() * res_quat; + + return Vector3d (res_quat[0], res_quat[1], res_quat[2]); + } + + /** \brief Converts a 3d angular velocity vector into a 4d derivative of the + * components of the quaternion. + * + * \param omega the angular velocity. + * + * \return a 4d vector containing the derivatives of the 4 components of the + * quaternion corresponding to omega. + * + */ + Vector4d omegaToQDot(const Vector3d& omega) const { + Math::Matrix43 m; + m(0, 0) = (*this)[3]; m(0, 1) = -(*this)[2]; m(0, 2) = (*this)[1]; + m(1, 0) = (*this)[2]; m(1, 1) = (*this)[3]; m(1, 2) = -(*this)[0]; + m(2, 0) = -(*this)[1]; m(2, 1) = (*this)[0]; m(2, 2) = (*this)[3]; + m(3, 0) = -(*this)[0]; m(3, 1) = -(*this)[1]; m(3, 2) = -(*this)[2]; + return Quaternion(0.5 * m * omega); + } +}; + +} + +} + +/* RBDL_QUATERNION_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h new file mode 100644 index 0000000..3205feb --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h @@ -0,0 +1,2537 @@ +/* + * SimpleMath - A simple highly inefficient single header C++ math library + * Copyright (c) 2019 Martin Felis + * + * This is a highly inefficient math library. It was conceived while he was + * waiting for code to compile which used a highly efficient math library. + * + * It is intended to be used as a fast compiling substitute for the + * blazingly fast Eigen3 + * http://eigen.tuxfamily.org/index.php?title=Main_Page library and tries + * to mimic its API to a certain extent. + * + * Feel free to use it wherever you like (even claim it as yours!). However, + * no guarantees are given that this code does what it says it would. + * + * Should you need a more formal license go with the following (zlib license): + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * +*/ + +#pragma once + +#include +#include +#include +#include // for memcpy +#include +#include +#include + +namespace SimpleMath { + +// +// Forward Declarations +// +enum { + Dynamic = -1 +}; + +template +struct Matrix; + +template +struct CommaInitializer; + +template +struct Block; + +template +struct Transpose; + +typedef Matrix Matrix33f; +typedef Matrix Vector3f; + +template +class LLT; + +template +class PartialPivLU; + +template +class HouseholderQR; + +template +class ColPivHouseholderQR; + + +// +// Main MatrixBase class which defines all functions available on the +// derived matrix types. +// +template +struct MatrixBase { + typedef MatrixBase MatrixType; + typedef ScalarType value_type; + + enum { + RowsAtCompileTime = Rows, + ColsAtCompileTime = Cols + }; + + + Derived& operator=(const Derived& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + return *this; + } + + + template + Derived& operator=(const MatrixBase& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + return *this; + } + + // + // operators with scalars + // + Matrix operator*(const double& scalar) const { + Matrix result (rows(), cols()); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) * static_cast(scalar); + } + } + return result; + } + + Matrix operator*(const float& scalar) const { + Matrix result (rows(), cols()); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + result (i,j) = operator()(i,j) * static_cast(scalar); + } + } + return result; + } + + // + // operators with other matrices + // + template + bool operator==(const MatrixBase& other) const { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + if (this->operator()(i,j) != other(i,j)) + return false; + } + } + + return true; + } + + template + bool operator!=(const MatrixBase& other) const { + return !(operator==(other)); + } + + CommaInitializer operator<< (const value_type& value) { + return CommaInitializer (*(static_cast(this)), value); + } + + template + Derived operator+(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result += other; + return result; + } + + template + Derived operator-(const OtherDerived& other) { + Derived result (*(static_cast(this))); + result -= other; + return result; + } + + template + Derived operator-(const OtherDerived& other) const { + Derived result (*(static_cast(this))); + result -= other; + return result; + } + + template + Matrix + operator*(const MatrixBase &other) const { + Matrix result(Matrix::Zero(rows(), other.cols())); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + for (size_t k = 0; k < other.rows(); k++) { + result(i, j) += operator()(i, k) * other(k, j); + } + } + } + return result; + } + + template + Derived operator*=(const MatrixBase &other) { + Derived copy (*static_cast(this)); + this->setZero(); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + for (size_t k = 0; k < other.rows(); k++) { + this->operator()(i, j) += copy.operator()(i, k) * other(k, j); + } + } + } + return *this; + } + + Matrix operator-() const { + Matrix copy (*static_cast(this)); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + copy(i,j) *= static_cast(-1.); + } + } + return copy; + } + + Derived operator*=(const ScalarType& s) { + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + operator()(i,j) *= s; + } + } + + return *this; + } + + template + inline void evalTo (OtherDerived& dest) const { + for (unsigned int i = 0; i < rows(); i++) { + for (unsigned int j = 0; j < cols(); j++) { + dest(i,j) = this->operator()(i, j); + } + } + } + + void resize(unsigned int nrows, unsigned int ncols = 1) { + static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed."); + + // Resize the this matrix (so far only possible for subclasses of the + // Matrix class) + Matrix* this_matrix = static_cast*>(this); + this_matrix->mStorage.resize(nrows, ncols); + } + + void conservativeResize(unsigned int nrows, unsigned int ncols = 1) { + static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed."); + + Derived copy(*this); + + unsigned int arows = std::min(nrows, (unsigned int) rows()); + unsigned int acols = std::min(ncols, (unsigned int) cols()); + + resize(nrows, ncols); + setZero(); + + // TODO: set entries to zero within the loop + for (unsigned int i = 0; i < arows; i++) { + for (unsigned int j = 0; j < acols; j++) { + this->operator()(i, j) = copy(i,j); + } + } + } + + void setZero() { + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + operator()(i,j) = static_cast(0.0); + } + } + } + + void set(const ScalarType& v0) { + static_assert(cols() * rows() == 1, "Invalid matrix size"); + data()[0] = v0; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2 + ) { + assert(cols() * rows() == 3); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3 + ) { + assert(cols() * rows() == 4); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + data()[3] = v3; + } + + void set( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3, + const ScalarType& v4, + const ScalarType& v5 + ) { + assert(cols() * rows() == 6); + + data()[0] = v0; + data()[1] = v1; + data()[2] = v2; + data()[3] = v3; + data()[4] = v4; + data()[5] = v5; + } + + void set( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v04, + const ScalarType& v05, + + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v14, + const ScalarType& v15, + + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v24, + const ScalarType& v25, + + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33, + const ScalarType& v34, + const ScalarType& v35, + + const ScalarType& v40, + const ScalarType& v41, + const ScalarType& v42, + const ScalarType& v43, + const ScalarType& v44, + const ScalarType& v45, + + const ScalarType& v50, + const ScalarType& v51, + const ScalarType& v52, + const ScalarType& v53, + const ScalarType& v54, + const ScalarType& v55 + ) { + assert(cols() == 6 && rows() == 6); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + operator()(0,4) = v04; + operator()(0,5) = v05; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + operator()(1,4) = v14; + operator()(1,5) = v15; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + operator()(2,4) = v24; + operator()(2,5) = v25; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + operator()(3,4) = v34; + operator()(3,5) = v35; + + operator()(4,0) = v40; + operator()(4,1) = v41; + operator()(4,2) = v42; + operator()(4,3) = v43; + operator()(4,4) = v44; + operator()(4,5) = v45; + + operator()(5,0) = v50; + operator()(5,1) = v51; + operator()(5,2) = v52; + operator()(5,3) = v53; + operator()(5,4) = v54; + operator()(5,5) = v55; + } + + size_t rows() const { + return static_cast(this)->rows(); + } + + size_t cols() const { + return static_cast(this)->cols(); + } + + size_t size() const { + return static_cast(this)->rows() * static_cast(this)->cols(); + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(this)->operator()(i,j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(this)->operator()(i,j); + } + + const ScalarType& operator[](const size_t& i) const { + assert(cols() == 1); + return static_cast(this)->operator()(i,0); + } + ScalarType& operator[](const size_t& i) { + assert(cols() == 1); + return static_cast(this)->operator()(i,0); + } + + operator ScalarType() const { +#ifndef NDEBUG + if ( static_cast(this)->cols() != 1 + || static_cast(this)->rows() != 1) { + std::cout << "Error trying to cast to scalar type. Dimensions are: " + << static_cast(this)->rows() << ", " + << static_cast(this)->cols() << "." + << std::endl; + } +#endif + assert ( static_cast(this)->cols() == 1 + && static_cast(this)->rows() == 1); + return static_cast(this)->operator()(0,0); + } + + // + // Numerical functions + // + + // TODO: separate functions for float or ScalarType matrices + ScalarType dot(const Derived& other) const { + assert ((rows() == 1 || cols() == 1) && (other.rows() == 1 || other.cols() == 1)); + + ScalarType result = 0.0; + + size_t n = rows() * cols(); + for (size_t i = 0; i < n; ++i) { + result += operator[](i) * other[i]; + } + + return result; + } + + // TODO: separate functions for float or ScalarType matrices + ScalarType squaredNorm() const { + ScalarType result = static_cast(0.0); + + size_t nr = rows(); + size_t nc = cols(); + for (size_t i = 0; i < nr; ++i) { + for (size_t j = 0; j < nc; ++j) { + result += operator()(i, j) * operator()(i, j); + } + } + + return result; + } + + // TODO: separate functions for float or ScalarType matrices + ScalarType norm() const { + return static_cast(std::sqrt(squaredNorm())); + } + + // TODO: separate functions for float or ScalarType matrices + Derived normalized() const { + Derived result (*this); + + ScalarType length = this->norm(); + + return result / length; + } + + // TODO: separate functions for float or ScalarType matrices + Derived normalize() { + ScalarType length = norm(); + + *this *= static_cast(1.0) / length; + + return *this; + } + + Derived cross(const Derived& other) const { + assert(cols() * rows() == 3); + + Derived result(rows(), cols()); + result[0] = operator[](1) * other[2] - operator[](2) * other[1]; + result[1] = operator[](2) * other[0] - operator[](0) * other[2]; + result[2] = operator[](0) * other[1] - operator[](1) * other[0]; + + return result; + } + + Derived inverse() const { + if (rows() == cols()) { + if (rows() == 1) { + Derived result(rows(), cols()); + result(0,0) = static_cast(1.) / operator()(0,0); + + return result; + } else if (rows() == 2) { + const ScalarType& a = operator()(0,0); + const ScalarType& b = operator()(0,1); + const ScalarType& c = operator()(1,0); + const ScalarType& d = operator()(1,1); + + Derived result(rows(), cols()); + + ScalarType detinv = static_cast(1.) / (a * d - b * c); + + result(0,0) = d * detinv; + result(0,1) = -b * detinv; + result(1,0) = -c * detinv; + result(1,1) = d * detinv; + + return result; + } else if (rows() == 3) { + // source: + // https://stackoverflow.com/questions/983999/simple-3x3-matrix-inverse-code-c + + // computes the inverse of a matrix m + ScalarType det = operator()(0, 0) * (operator()(1, 1) * operator()(2, 2) + - operator()(2, 1) * operator()(1, 2)) + - operator()(0, 1) * (operator()(1, 0) * operator()(2, 2) + - operator()(1, 2) * operator()(2, 0)) + + operator()(0, 2) * (operator()(1, 0) * operator()(2, 1) + - operator()(1, 1) * operator()(2, 0)); + + ScalarType invdet = 1. / det; + + Derived result(rows(), cols()); + + result(0,0) = (operator()(1, 1) * operator()(2, 2) - operator()(2, 1) * operator()(1, 2)) * invdet; + result(0,1) = (operator()(0, 2) * operator()(2, 1) - operator()(0, 1) * operator()(2, 2)) * invdet; + result(0,2) = (operator()(0, 1) * operator()(1, 2) - operator()(0, 2) * operator()(1, 1)) * invdet; + result(1,0) = (operator()(1, 2) * operator()(2, 0) - operator()(1, 0) * operator()(2, 2)) * invdet; + result(1,1) = (operator()(0, 0) * operator()(2, 2) - operator()(0, 2) * operator()(2, 0)) * invdet; + result(1,2) = (operator()(1, 0) * operator()(0, 2) - operator()(0, 0) * operator()(1, 2)) * invdet; + result(2,0) = (operator()(1, 0) * operator()(2, 1) - operator()(2, 0) * operator()(1, 1)) * invdet; + result(2,1) = (operator()(2, 0) * operator()(0, 1) - operator()(0, 0) * operator()(2, 1)) * invdet; + result(2,2) = (operator()(0, 0) * operator()(1, 1) - operator()(1, 0) * operator()(0, 1)) * invdet; + + return result; + } + } + return colPivHouseholderQr().inverse(); + } + + ScalarType trace() const { + assert(rows() == cols()); + + ScalarType result = static_cast(0.0); + + for (unsigned int i = 0; i < rows(); i++) { + result += operator()(i,i); + } + + return result; + } + + ScalarType mean() const { + assert(rows() == 1 || cols() == 1); + ScalarType result = static_cast(0.0); + for (unsigned int i = 0; i < rows(); i++) { + result += operator[](i); + } + + return result / static_cast(rows() * cols()); + } + + ScalarType sum() const { + assert(rows() == 1 || cols() == 1); + ScalarType result = static_cast(0.0); + for (unsigned int i = 0; i < rows(); i++) { + result += operator[](i); + } + + return result; + } + + ScalarType minCoeff() const { + assert(rows() > 0 && cols() > 0); + ScalarType result = operator()(0, 0); + const unsigned int ni = rows(); + const unsigned int nj = cols(); + for (unsigned int i = 0; i < ni; i++) { + for (unsigned int j = 0; j < nj; j++) { + if (operator()(i, j) < result) { + result = operator()(i, j); + } + } + } + return result; + } + + ScalarType maxCoeff() const { + assert(rows() > 0 && cols() > 0); + ScalarType result = operator()(0, 0); + const unsigned int ni = rows(); + const unsigned int nj = cols(); + for (unsigned int i = 0; i < ni; i++) { + for (unsigned int j = 0; j < nj; j++) { + if (operator()(i, j) > result) { + result = operator()(i, j); + } + } + } + return result; + } + + const LLT llt() const { + return LLT(*this); + } + + const PartialPivLU partialPivLu() const { + return PartialPivLU(*this); + } + + const HouseholderQR householderQr() const { + return HouseholderQR(*this); + } + + const ColPivHouseholderQR colPivHouseholderQr() const { + return ColPivHouseholderQR(*this); + } + + ScalarType* data() { + return static_cast(this)->data(); + } + + const ScalarType* data() const { + return static_cast(this)->data(); + } + + // + // Special Constructors + // + static Derived Zero(size_t NumRows = (Rows == Dynamic) ? 1 : Rows, + size_t NumCols = (Cols == Dynamic) ? 1 : Cols) { + Derived result (NumRows, NumCols); + + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = static_cast(0.0); + } + } + + return result; + } + + static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) { + Derived result (Derived::Zero(NumRows, NumCols)); + + for (size_t i = 0; i < NumRows; i++) { + result(i,i) = static_cast(1.0); + } + + return result; + } + + static Derived Constant(size_t NumRows, const ScalarType &value) { + Derived result (NumRows, 1); + + for (size_t i = 0; i < NumRows; i++) { + result(i,0) = value; + } + + return result; + } + + static Derived Constant(size_t NumRows, size_t NumCols, const ScalarType &value) { + Derived result (NumRows, NumCols); + + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = value; + } + } + + return result; + } + + static Derived Random(size_t NumRows = (Rows == Dynamic) ? 1 : Rows, size_t NumCols = (Cols == Dynamic) ? 1 : Cols) { + Derived result (NumRows, NumCols); + + for (size_t i = 0; i < NumRows; i++) { + for (size_t j = 0; j < NumCols; j++) { + result(i,j) = (static_cast(rand()) / static_cast(RAND_MAX)) * 2.0 - 1.0; + } + } + + return result; + } + + + // + // Block accessors + // + template < + int block_rows, + int block_cols + > + Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index); + } + + template < + int block_rows, + int block_cols + > + const Block< + Derived, + ScalarType, + block_rows, + block_cols + > block(int block_row_index, int block_col_index) const { + assert(block_row_index + block_rows <= rows()); + assert(block_col_index + block_cols <= cols()); + return Block(const_cast(static_cast(this)), block_row_index, block_col_index); + } + + Block< + Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + const Block< + const Derived, + ScalarType + > block(int block_row_index, int block_col_index, + int block_num_rows, int block_num_cols) const { + assert(block_row_index + block_num_rows <= rows()); + assert(block_col_index + block_num_cols <= cols()); + return Block(static_cast(this), block_row_index, block_col_index, block_num_rows, block_num_cols); + } + + // TODO: head, tail + + // + // Transpose + // + Transpose transpose() { + return Transpose(static_cast(this)); + } + + const Transpose transpose() const { + return Transpose(static_cast(this)); + } +}; + + +template +struct Storage; + +template +struct Storage : public Storage {}; + +// fixed storage +template +struct Storage { + ScalarType mData[SizeAtCompileTime]; + + Storage() {} + + Storage(int rows, int cols) { + resize(rows, cols); + } + + inline size_t rows() const { return NumRows; } + + inline size_t cols() const { return NumCols; } + +#ifndef NDEBUG + void resize(int num_rows, int num_cols) { + if (num_rows != NumRows || num_cols != NumCols) { + std::cout << "Error: trying to resize fixed matrix from " + << NumRows << ", " << NumCols << " to " + << num_rows << ", " << num_cols << "." << std::endl; + } + assert (num_rows == NumRows && num_cols == NumCols); +#else + void resize(int UNUSED(num_rows), int UNUSED(num_cols)) { +#endif + // Resizing of fixed size matrices not allowed + + } + + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= NumRows); +// assert (col_index >= 0 && col_index <= NumCols); + return mData[row_index * NumCols + col_index]; + } + + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= NumRows); +// assert (col_index >= 0 && col_index <= NumCols); + return mData[row_index * NumCols + col_index]; + } +}; + +template +struct Storage { + ScalarType* mData = nullptr; + int mRows = 0; + int mCols = 0; + + Storage() {} + + ~Storage() { + delete[] mData; + } + + Storage(int rows, int cols) { + resize(rows, cols); + } + + inline size_t rows() const { return mRows; } + inline size_t cols() const { return mCols; } + + void resize(int num_rows, int num_cols) { + if (mRows != num_rows || mCols != num_cols) { + if (mData != nullptr) { + delete[] mData; + } + + mData = new ScalarType[num_rows * num_cols]; + mRows = num_rows; + mCols = num_cols; + } + } + + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); + return mData[row_index * mCols + col_index]; + } + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); + return mData[row_index * mCols + col_index]; + } +}; + + +template +struct Storage { + ScalarType* mData = nullptr; + int mRows = 0; + int mCols = 0; + + Storage() {} + + ~Storage() { + delete[] mData; + } + + Storage(int rows, int cols) { + resize(rows, cols); + } + + inline size_t rows() const { return mRows; } + inline size_t cols() const { return mCols; } + + void resize(int num_rows, int num_cols) { + if (mRows != num_rows || mCols != num_cols) { + if (mData != nullptr) { + delete[] mData; + } + + mData = new ScalarType[num_rows * num_cols]; + mRows = num_rows; + mCols = num_cols; + } + } + + inline ScalarType& coeff(int row_index, int col_index) { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); + return mData[row_index * mCols + col_index]; + } + inline const ScalarType& coeff(int row_index, int col_index) const { +// assert (row_index >= 0 && row_index <= mRows); +// assert (col_index >= 0 && col_index <= mCols); + return mData[row_index * mCols + col_index]; + } +}; + + +template +struct Matrix : public MatrixBase, ScalarType, NumRows, NumCols> { + typedef Matrix DerivedBase; + + enum { + RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows, + ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols, + SizeAtCompileTime = (NumRows != Dynamic && NumCols != Dynamic) ? NumRows * NumCols : 0 + }; + + Storage mStorage; + + Matrix() : + mStorage ( + SizeAtCompileTime / ColsAtCompileTime, + SizeAtCompileTime / RowsAtCompileTime + ) {} + + explicit Matrix(int rows) : mStorage (rows, 1) {} + explicit Matrix(unsigned int rows) : mStorage (rows, 1) {} + explicit Matrix(size_t rows) : mStorage (rows, 1) {} + + explicit Matrix(int rows, int cols) : + mStorage(rows, cols) {} + + explicit Matrix(int rows, unsigned int cols) : + mStorage(rows, cols) {} + + explicit Matrix(int rows, size_t cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, int cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, unsigned int cols) : + mStorage(rows, cols) {} + + explicit Matrix(unsigned int rows, size_t cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, int cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, unsigned int cols) : + mStorage(rows, cols) {} + + explicit Matrix(size_t rows, size_t cols) : + mStorage(rows, cols) {} + + template + Matrix(const MatrixBase &other) { + mStorage.resize(other.rows(), other.cols()); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i, j) = other(i, j); + } + } + } + + Matrix (const Matrix& other) : + mStorage(other.rows(), other.cols()){ + memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols()); + } + + Matrix& operator=(const Matrix& other) { + if (&other != this) { + mStorage.resize(other.rows(), other.cols()); + memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols()); + } + return *this; + } + + // + // Constructor for vectors + // + explicit Matrix ( + const ScalarType& v0, + const ScalarType& v1 + ) { + static_assert (NumRows * NumCols == 2, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2 + ) { + static_assert (NumRows * NumCols == 3, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3 + ) { + static_assert (NumRows * NumCols == 4, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + operator()(3,0) = v3; + } + + Matrix ( + const ScalarType& v0, + const ScalarType& v1, + const ScalarType& v2, + const ScalarType& v3, + const ScalarType& v4, + const ScalarType& v5 + ) { + static_assert (NumRows * NumCols == 6, "Invalid matrix size"); + + operator()(0,0) = v0; + operator()(1,0) = v1; + operator()(2,0) = v2; + operator()(3,0) = v3; + operator()(4,0) = v4; + operator()(5,0) = v5; + } + + // + // Constructor for matrices + // + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22 + ) { + static_assert (NumRows == 3 && NumCols == 3, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + } + + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33 + ) { + static_assert (NumRows == 4 && NumCols == 4, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + } + + Matrix ( + const ScalarType& v00, + const ScalarType& v01, + const ScalarType& v02, + const ScalarType& v03, + const ScalarType& v04, + const ScalarType& v05, + + const ScalarType& v10, + const ScalarType& v11, + const ScalarType& v12, + const ScalarType& v13, + const ScalarType& v14, + const ScalarType& v15, + + const ScalarType& v20, + const ScalarType& v21, + const ScalarType& v22, + const ScalarType& v23, + const ScalarType& v24, + const ScalarType& v25, + + const ScalarType& v30, + const ScalarType& v31, + const ScalarType& v32, + const ScalarType& v33, + const ScalarType& v34, + const ScalarType& v35, + + const ScalarType& v40, + const ScalarType& v41, + const ScalarType& v42, + const ScalarType& v43, + const ScalarType& v44, + const ScalarType& v45, + + const ScalarType& v50, + const ScalarType& v51, + const ScalarType& v52, + const ScalarType& v53, + const ScalarType& v54, + const ScalarType& v55 + ) { + static_assert (NumRows == 6 && NumCols == 6, "Invalid matrix size"); + + operator()(0,0) = v00; + operator()(0,1) = v01; + operator()(0,2) = v02; + operator()(0,3) = v03; + operator()(0,4) = v04; + operator()(0,5) = v05; + + operator()(1,0) = v10; + operator()(1,1) = v11; + operator()(1,2) = v12; + operator()(1,3) = v13; + operator()(1,4) = v14; + operator()(1,5) = v15; + + operator()(2,0) = v20; + operator()(2,1) = v21; + operator()(2,2) = v22; + operator()(2,3) = v23; + operator()(2,4) = v24; + operator()(2,5) = v25; + + operator()(3,0) = v30; + operator()(3,1) = v31; + operator()(3,2) = v32; + operator()(3,3) = v33; + operator()(3,4) = v34; + operator()(3,5) = v35; + + operator()(4,0) = v40; + operator()(4,1) = v41; + operator()(4,2) = v42; + operator()(4,3) = v43; + operator()(4,4) = v44; + operator()(4,5) = v45; + + operator()(5,0) = v50; + operator()(5,1) = v51; + operator()(5,2) = v52; + operator()(5,3) = v53; + operator()(5,4) = v54; + operator()(5,5) = v55; + } + + template + Matrix& operator+=(const OtherDerived& other) { + assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) += other(i,j); + } + } + return *this; + } + + template + Matrix& operator-=(const OtherDerived& other) { + assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) -= other(i,j); + } + } + return *this; + } + + inline ScalarType& operator()(const size_t& i, const size_t& j) { + return mStorage.coeff(i, j); + } + + inline const ScalarType& operator()(const size_t& i, const size_t& j) const { + return mStorage.coeff(i, j); + } + + ScalarType* data() { + return mStorage.mData; + } + + const ScalarType* data() const { + return mStorage.mData; + } + + size_t cols() const { + return mStorage.cols(); + } + + size_t rows() const { + return mStorage.rows(); + } +}; + + +// +// CommaInitializer +// +template +struct CommaInitializer { + typedef typename Derived::value_type value_type; + + private: + CommaInitializer() {} + + Derived *mParentMatrix; + unsigned int mRowIndex; + unsigned int mColIndex; + bool mElementWasAdded; + + public: + + CommaInitializer(Derived &matrix, const value_type &value) : + mParentMatrix(&matrix), + mRowIndex(0), + mColIndex(0), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + mParentMatrix->operator()(0,0) = value; + } + + CommaInitializer(Derived &matrix, unsigned int row_index, unsigned int col_index) : + mParentMatrix(&matrix), + mRowIndex(row_index), + mColIndex(col_index), + mElementWasAdded(false) + { + assert (matrix.rows() > 0 && matrix.cols() > 0); + } + + ~CommaInitializer() { + if (!mElementWasAdded + && (mColIndex + 1 < mParentMatrix->cols() + || mRowIndex + 1 < mParentMatrix->rows())) { + std::cerr + << "Error: too few elements passed to CommaInitializer Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + } + + CommaInitializer operator, (const value_type &value) { + mColIndex++; + if (mColIndex >= mParentMatrix->cols()) { + mRowIndex++; + mColIndex = 0; + } + + if (mRowIndex == mParentMatrix->rows() && mColIndex == 0) { + std::cerr + << "Error: too many elements passed to CommaInitializer!Expected " + << mParentMatrix->rows() * mParentMatrix->cols() + << " but was given " + << mRowIndex *mParentMatrix->cols() + mColIndex + 1 << std::endl; + abort(); + } + (*mParentMatrix)(mRowIndex, mColIndex) = value; + mElementWasAdded = true; + + return CommaInitializer(*mParentMatrix, mRowIndex, mColIndex); + } +}; + +// +// Transpose +// +template +struct Transpose : public MatrixBase, ScalarType, NumRows, NumCols> { + Derived* mTransposeSource; + + Transpose(Derived* transpose_source) : + mTransposeSource(transpose_source) + { } + + Transpose(const Transpose &other) : + mTransposeSource(other.mTransposeSource) + { } + + template + Matrix operator*(const MatrixBase& other) const { + Matrix result (Matrix::Zero(rows(), other.cols())); + + unsigned int i,j,k; + unsigned int nrows = rows(); + unsigned int other_ncols = other.cols(); + unsigned int other_nrows = other.rows(); + + for (i = 0; i < nrows; i++) { + for (j = 0; j < other_ncols; j++) { + for (k = 0; k < other_nrows; k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } + + template + Transpose& operator=(const MatrixBase& other) { + if (static_cast(this) != static_cast(&other)) { + for (size_t i = 0; i < other.rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + } + + return *this; + } + + size_t rows() const { + return static_cast(mTransposeSource)->cols(); + } + size_t cols() const { + return static_cast(mTransposeSource)->rows(); + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mTransposeSource)->operator()(j, i); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mTransposeSource)->operator()(j, i); + } +}; + +// +// Block +// +template +struct Block : public MatrixBase, ScalarType, NumRows, NumCols> { + typedef Block matrix_type; + + Derived* mBlockSource; + int row_index; + int col_index; + int nrows; + int ncols; + + Block(Derived* block_source, int row_index, int col_index) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows(NumRows), ncols(NumCols) + { + static_assert(NumRows != -1 && NumCols != -1, "Invalid block specifications: unknown number of rows and columns!"); + } + + Block(Derived* block_source, int row_index, int col_index, int num_rows, int num_cols) : + mBlockSource(block_source), + row_index(row_index), + col_index(col_index), + nrows (num_rows), + ncols (num_cols) + { + } + + Block(const Block &other) : + mBlockSource(other.mBlockSource), + row_index(other.row_index), + col_index(other.col_index) + { } + + Block& operator=(const Block &other) { + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + return *this; + } + + template + Block& operator=(const MatrixBase& other) { + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) = other(i,j); + } + } + + return *this; + } + + template + Block& operator+=(const MatrixBase& other) { + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < cols(); j++) { + this->operator()(i,j) += other(i,j); + } + } + + return *this; + } + + + template + Matrix operator*(const MatrixBase& other) const { + Matrix result (rows(), other.cols()); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + for (size_t k = 0; k < other.rows(); k++) { + result (i,j) += operator()(i,k) * other(k,j); + } + } + } + return result; + } + + size_t rows() const { + return nrows; + } + size_t cols() const { + return ncols; + } + + const ScalarType& operator()(const size_t& i, const size_t& j) const { + return static_cast(mBlockSource)->operator()(row_index + i, col_index + j); + } + ScalarType& operator()(const size_t& i, const size_t& j) { + return static_cast(mBlockSource)->operator()(row_index + i,col_index + j); + } + + template + Matrix operator+(const MatrixBase& other) const { + Matrix result (rows(), other.cols()); + for (size_t i = 0; i < rows(); i++) { + for (size_t j = 0; j < other.cols(); j++) { + result (i,j) = operator()(i,j) + other(i,j); + } + } + return result; + } + + private: + Block() { assert(0 && "Invalid call!"); }; + + ScalarType* data() { + assert("invalid call"); + return NULL; + } + + const ScalarType* data() const { + assert("invalid call"); + return NULL; + } +}; + +// +// LLT Decomposition +// +template +class LLT { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + + LLT() : + mIsFactorized(false) + {} + +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + + bool mIsFactorized; + Matrix mQ; + Derived mL; + +public: + LLT(const Derived &matrix) : + mIsFactorized(false), + mL(matrix) + { + compute(); + } + LLT compute() { + for (unsigned int i = 0; i < mL.rows(); i++) { + for (unsigned int j = 0; j < mL.rows(); j++) { + if (j > i) { + mL(i,j) = 0.; + continue; + } + double s = mL(i,j); + for (unsigned int k = 0; k < j; k++) { + s = s - mL(i,k) * mL(j,k); + } + if (i > j) { + mL(i,j) = s / mL(j,j); + } else if (s > 0.) { + mL (i,i) = sqrt (s); + } else { + std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl; + assert (false); + } + } + } + + + mIsFactorized = true; + + return *this; + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + ColumnVector y (mL.rows()); + for (unsigned int i = 0; i < mL.rows(); i++) { + double temp = rhs[i]; + + for (unsigned int j = 0; j < i; j++) { + temp = temp - mL(i,j) * y[j]; + } + + y[i] = temp / mL(i,i); + } + + ColumnVector x (mL.rows()); + for (int i = mL.rows() - 1; i >= 0; i--) { + double temp = y[i]; + + for (unsigned int j = i + 1; j < mL.rows(); j++) { + temp = temp - mL(j, i) * x[j]; + } + + x[i] = temp / mL(i,i); + } + + return x; + } + Derived inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + Derived matrixL () const { + return mL; + } +}; + + +// +// Partial Pivoting LU Decomposition +// +template +class PartialPivLU { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + PartialPivLU() : + mIsFactorized(false) + {} +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + typedef Matrix RowVector; + bool mIsFactorized; + unsigned int *mPermutations = nullptr; + Derived mLU; + +public: + ~PartialPivLU() { + delete[] mPermutations; + } + + PartialPivLU(const Derived &matrix) : + mIsFactorized(false), + mLU (matrix) + { + mPermutations = new unsigned int [matrix.cols() + 1]; + for (unsigned int i = 0; i <= matrix.cols(); i++) { + mPermutations[i] = i; + } + compute(matrix); + } + + PartialPivLU& compute(const Derived &matrix) { + unsigned int n = matrix.rows(); + + double v_abs; + RowVector temp_vec; + + unsigned int i,j,k; + + // over all columns + for (i = 0; i < n; i++) { + double max_v = 0.0; + unsigned int max_i = i; + + // Find the row pivoting index + for (k = i; k < n; k++) { + if ((v_abs = fabs(mLU(k, i))) > max_v) { + max_v = v_abs; + max_i = k; + } + } + + if (max_v < std::numeric_limits::epsilon()) { + std::cerr << "Error: pivoting failed for matrix A = " << std::endl; + std::cerr << "A = " << matrix << std::endl; + abort(); + } + + // Perform the permutation + if (max_i != i) { + // update permutation vector + j = mPermutations[i]; + mPermutations[i] = mPermutations[max_i]; + mPermutations[max_i] = j; + + // swap columns + temp_vec = mLU.block(i,0,1,n); + mLU.block(i, 0, 1, n) = mLU.block(max_i, 0, 1, n); + mLU.block(max_i, 0, 1, n) = temp_vec; + + // Increase number of permutations + mPermutations[n]++; + } + + // eliminate i'th column of k'th row + for (k = i+1; k < n; k++) { + mLU(k,i) = mLU(k,i) / mLU(i,i); + + // iterate over all columns + for (j = i+1; j < n; j++) { + mLU(k,j) = mLU(k,j) - mLU(i,j) * mLU(k,i); + } + } + } + + mIsFactorized = true; + + return *this; + } + + Derived matrixL() const { + Derived result (Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + + for (int i = 0; i < n; i++) { + for (int j = 0; j < i; j++) { + result(i,j) = mLU(i,j); + } + + result(i,i) = 1.0; + } + + return result; + } + + Derived matrixU() const { + Derived result (Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + + for (int i = 0; i < n; i++) { + for (int j = i; j < n; j++) { + result(i,j) = mLU(i,j); + } + } + + return result; + } + + Derived matrixP() const { + Derived result(Derived::Zero(mLU.rows(), mLU.cols())); + + unsigned int n = mLU.rows(); + for (int i = 0; i < n; i++) { + result(i, mPermutations[i]) = 1.0; + } + + return result; + } + + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + unsigned int n = mLU.rows(); + + // Backsolve L^-1 * rhs + ColumnVector result(n, 1); + + for (unsigned int i = 0; i < n; i++) { + result[i] = rhs[mPermutations[i]]; + for (unsigned int j = 0; j < i; j++) { + result[i] = result[i] - result[j] * mLU(i,j); + } + } + + // Solve U^-1 * result + for (int i = n - 1; i >= 0; i--) { + for (unsigned int j = i + 1; j < n; j++) { + result[i] = result[i] - result[j] * mLU(i,j); + } + + result[i] = result[i] / mLU(i,i); + } + + return result; + } + + Derived inverse() const { + assert (mIsFactorized); + VectorXd rhs_temp = VectorXd::Zero(mLU.cols()); + MatrixXXd result (mLU.cols(), mLU.cols()); + for (unsigned int i = 0; i < mLU.cols(); i++) { + rhs_temp[i] = 1.; + result.block(0, i, mLU.cols(), 1) = solve(rhs_temp); + rhs_temp[i] = 0.; + } + return result; + } +}; + +// +// QR Decomposition +// +template +class HouseholderQR { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + + HouseholderQR() : + mIsFactorized(false) + {} + +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + + bool mIsFactorized; + Matrix mQ; + Derived mR; + +public: + HouseholderQR(const Derived &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()) + { + compute(matrix); + } + HouseholderQR compute(const Derived& matrix) { + mR = matrix; + mQ = MatrixType::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5)); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + ColumnVector y = mQ.transpose() * rhs; + ColumnVector x = ColumnVector::Zero(mR.cols()); + + unsigned int ncols = mR.cols(); + for (unsigned int i = ncols - 1; i != 0; i--) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < ncols; j++) { + z = z - x[j] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl; + abort(); + } + x[i] = z / mR(i,i); + } + + assert (!std::isnan(x.squaredNorm())); + + return x; + } + Derived inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + MatrixXXd result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + Matrix householderQ () const { + return mQ; + } + Derived matrixR () const { + return mR; + } +}; + +template +class ColPivHouseholderQR { +public: + typedef typename Derived::value_type value_type; + typedef MatrixBase MatrixType; + + +private: + typedef Matrix VectorXd; + typedef Matrix MatrixXXd; + typedef Matrix ColumnVector; + + bool mIsFactorized; + Matrix mQ; + Derived mR; + + unsigned int *mPermutations; + value_type mThreshold; + unsigned int mRank; + +public: + ColPivHouseholderQR(): + mIsFactorized(false) { + mPermutations = new unsigned int[1]; + } + + ColPivHouseholderQR (const ColPivHouseholderQR& other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) { + if (this != &other) { + mIsFactorized = other.mIsFactorized; + mQ = other.mQ; + mR = other.mR; + delete[] mPermutations; + mPermutations = new unsigned int[mQ.cols()]; + mThreshold = other.mThreshold; + mRank = other.mRank; + } + + return *this; + } + + ColPivHouseholderQR(const MatrixType &matrix) : + mIsFactorized(false), + mQ(matrix.rows(), matrix.rows()), + mThreshold (std::numeric_limits::epsilon() * matrix.cols()) { + mPermutations = new unsigned int [matrix.cols()]; + for (unsigned int i = 0; i < matrix.cols(); i++) { + mPermutations[i] = i; + } + compute(matrix); + } + ~ColPivHouseholderQR() { + delete[] mPermutations; + } + + ColPivHouseholderQR& setThreshold (const value_type& threshold) { + mThreshold = threshold; + + return *this; + } + ColPivHouseholderQR& compute(const MatrixType &matrix) { + mR = matrix; + mQ = MatrixType::Identity (mR.rows(), mR.rows()); + + for (unsigned int i = 0; i < mR.cols(); i++) { + unsigned int block_rows = mR.rows() - i; + unsigned int block_cols = mR.cols() - i; + + // find and swap the column with the highest norm + unsigned int col_index_norm_max = i; + value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm(); + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + VectorXd column = mR.block(i, j, block_rows, 1); + + if (column.squaredNorm() > col_norm_max) { + col_index_norm_max = j; + col_norm_max = column.squaredNorm(); + } + } + + if (col_norm_max < mThreshold) { + // if all entries of the column is close to zero, we bail out + break; + } + + + if (col_index_norm_max != i) { + VectorXd temp_col = mR.block(0, i, mR.rows(), 1); + mR.block(0, i, mR.rows(), 1) = mR.block(0, col_index_norm_max, mR.rows(), 1);; + mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col; + + unsigned int temp_index = mPermutations[i]; + mPermutations[i] = mPermutations[col_index_norm_max]; + mPermutations[col_index_norm_max] = temp_index; + } + + MatrixXXd current_block = mR.block(i,i, block_rows, block_cols); + VectorXd column = current_block.block(0, 0, block_rows, 1); + + value_type alpha = - column.norm(); + if (current_block(0,0) < 0) { + alpha = - alpha; + } + + VectorXd v = current_block.block(0, 0, block_rows, 1); + v[0] = v[0] - alpha; + + MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows())); + + Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows)) + - (v * v.transpose()) / (v.squaredNorm() * static_cast(0.5)); + + mR = Q * mR; + + // Normalize so that we have positive diagonal elements + if (mR(i,i) < 0) { + mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.; + Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.; + } + + mQ = mQ * Q; + } + + mIsFactorized = true; + + return *this; + } + ColumnVector solve ( + const ColumnVector &rhs + ) const { + assert (mIsFactorized); + + ColumnVector y = mQ.transpose() * rhs; + ColumnVector x = ColumnVector::Zero(mR.cols()); + + for (int i = mR.cols() - 1; i >= 0; --i) { + value_type z = y[i]; + + for (unsigned int j = i + 1; j < mR.cols(); j++) { + z = z - x[mPermutations[j]] * mR(i,j); + } + + if (fabs(mR(i,i)) < std::numeric_limits::epsilon() * 10) { + std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero:" << fabs(mR(i,i))<< std::endl; + abort(); + } + x[mPermutations[i]] = z / mR(i,i); + } + + assert (!std::isnan(x.squaredNorm())); + + return x; + } + Derived inverse() const { + assert (mIsFactorized); + + VectorXd rhs_temp = VectorXd::Zero(mQ.cols()); + Derived result (mQ.cols(), mQ.cols()); + + for (unsigned int i = 0; i < mQ.cols(); i++) { + rhs_temp[i] = 1.; + + result.block(0, i, mQ.cols(), 1) = solve(rhs_temp); + + rhs_temp[i] = 0.; + } + + return result; + } + + Matrix householderQ () const { + return mQ; + } + Derived matrixR () const { + return mR; + } + Matrix matrixP () const { + MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols()); + MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols()); + for (unsigned int i = 0; i < mR.cols(); i++) { + P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1); + } + return P; + } + + unsigned int rank() const { + value_type abs_threshold = fabs(mR(0,0)) * mThreshold; + + for (unsigned int i = 1; i < mR.cols(); i++) { + if (fabs(mR(i,i)) < abs_threshold) + return i; + } + + return mR.cols(); + } +}; + +template +inline Matrix operator*(const ScalarType& scalar, const MatrixBase &matrix) { + return matrix * scalar; +} + +template +inline Matrix operator*(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * scalar; +} + +template +inline Matrix operator/(const MatrixBase &matrix, const ScalarType& scalar) { + return matrix * (1.0 / scalar); +} + +template +inline Matrix operator/=(MatrixBase &matrix, const ScalarType& scalar) { + return matrix *= (1.0 / scalar); +} + +// +// OpenGL Matrices and Quaternions +// + +namespace GL { + +typedef Matrix Vector3f; +typedef Matrix Matrix33f; + +typedef Matrix Vector4f; +typedef Matrix Matrix44f; + +inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) { + float c = cosf (rot_deg * M_PI / 180.f); + float s = sinf (rot_deg * M_PI / 180.f); + return Matrix33f ( + x * x * (1.0f - c) + c, + y * x * (1.0f - c) + z * s, + x * z * (1.0f - c) - y * s, + + x * y * (1.0f - c) - z * s, + y * y * (1.0f - c) + c, + y * z * (1.0f - c) + x * s, + + x * z * (1.0f - c) + y * s, + y * z * (1.0f - c) - x * s, + z * z * (1.0f - c) + c + + ); +} + + +inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) { + float c = cosf (rot_deg * M_PI / 180.f); + float s = sinf (rot_deg * M_PI / 180.f); + return Matrix44f ( + x * x * (1.0f - c) + c, + y * x * (1.0f - c) + z * s, + x * z * (1.0f - c) - y * s, + 0.f, + + x * y * (1.0f - c) - z * s, + y * y * (1.0f - c) + c, + y * z * (1.0f - c) + x * s, + 0.f, + + x * z * (1.0f - c) + y * s, + y * z * (1.0f - c) - x * s, + z * z * (1.0f - c) + c, + 0.f, + + 0.f, 0.f, 0.f, 1.f + ); +} + +inline Matrix44f TranslateMat44 (float x, float y, float z) { + return Matrix44f ( + 1.f, 0.f, 0.f, 0.f, + 0.f, 1.f, 0.f, 0.f, + 0.f, 0.f, 1.f, 0.f, + x, y, z, 1.f + ); +} + +inline Matrix44f ScaleMat44 (float x, float y, float z) { + return Matrix44f ( + x, 0.f, 0.f, 0.f, + 0.f, y, 0.f, 0.f, + 0.f, 0.f, z, 0.f, + 0.f, 0.f, 0.f, 1.f + ); +} + +inline Matrix44f Ortho( + float left, float right, + float bottom, float top, + float near, float far) { + float tx = -(right + left) / (right - left); + float ty = -(top + bottom) / (top - bottom); + float tz = -(far + near) / (far - near); + return Matrix44f( + 2.0f / (right - left), 0.0f, 0.0f, 0.0f, + 0, 2.0f / (top - bottom), 0.0f, 0.0f, + 0.0f, 0.0f, -2.0f / (far - near), 0.0f, + tx, ty, tz, 1.0f + ); +} + +inline Matrix44f Perspective(float fovy, float aspect, + float near, float far) { + float x = (fovy * M_PI / 180.0) / 2.0f; + float f = cos(x) / sin(x); + + return Matrix44f( + f / aspect, 0.0f, 0.0f, 0.0f, + 0.0f, f, 0.0f, 0.0f, + 0.0f, 0.0f, (far + near) / (near - far), -1.0f, + 0.0f, 0.0f, (2.0f * far * near) / (near - far), 0.0f + ); +} + +inline Matrix44f Frustum(float left, float right, + float bottom, float top, + float near, float far) { + float A = (right + left) / (right - left); + float B = (top + bottom) / (top - bottom); + float C = -(far + near) / (far - near); + float D = - (2.0f * far * near) / (far - near); + + return Matrix44f( + 2.0f * near / (right - left), 0.0f, 0.0f, 0.0f, + 0.0f, 2.0f * near / (top - bottom), 0.0f, 0.0f, + A, B, C, -1.0f, + 0.0f, 0.0f, D, 0.0f + ); +} + +inline Matrix44f LookAt( + const Vector3f& eye, + const Vector3f& poi, + const Vector3f& up) { + Vector3f d = (poi - eye).normalized(); + Vector3f s = d.cross(up.normalized()).normalized(); + Vector3f u = s.cross(d).normalized(); + + return TranslateMat44(-eye[0], -eye[1], -eye[2]) * Matrix44f( + s[0], u[0], -d[0], 0.0f, + s[1], u[1], -d[1], 0.0f, + s[2], u[2], -d[2], 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f + ); +} + +// +// Quaternion +// +// order: x,y,z,w + +class Quaternion : public Vector4f { + public: + Quaternion () : + Vector4f (0.f, 0.f, 0.f, 1.f) + {} + Quaternion (const Vector4f vec4) : + Vector4f (vec4) + {} + Quaternion (float x, float y, float z, float w): + Vector4f (x, y, z, w) + {} + // This function is equivalent to multiplicate their corresponding rotation matrices + Quaternion operator* (const Quaternion &q) const { + return Quaternion ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + } + Quaternion& operator*=(const Quaternion &q) { + set ( + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] + ); + return *this; + } + + static Quaternion fromGLRotate (float angle, float x, float y, float z) { + float st = sinf (angle * M_PI / 360.f); + return Quaternion ( + st * x, + st * y, + st * z, + cosf (angle * M_PI / 360.f) + ); + } + + Quaternion normalize() { + return Vector4f::normalize(); + } + + Quaternion slerp (float alpha, const Quaternion &quat) const { + // check whether one of the two has 0 length + float s = sqrt (squaredNorm() * quat.squaredNorm()); + + // division by 0.f is unhealthy! + assert (s != 0.f); + + float angle = acos (dot(quat) / s); + if (angle == 0.f || std::isnan(angle)) { + return *this; + } + assert(!std::isnan(angle)); + + float d = 1.f / sinf (angle); + float p0 = sinf ((1.f - alpha) * angle); + float p1 = sinf (alpha * angle); + + if (dot (quat) < 0.f) { + return Quaternion( ((*this) * p0 - quat * p1) * d); + } + return Quaternion( ((*this) * p0 + quat * p1) * d); + } + + Matrix44f toGLMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix44f ( + 1 - 2*y*y - 2*z*z, + 2*x*y + 2*w*z, + 2*x*z - 2*w*y, + 0.f, + + 2*x*y - 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z + 2*w*x, + 0.f, + + 2*x*z + 2*w*y, + 2*y*z - 2*w*x, + 1 - 2*x*x - 2*y*y, + 0.f, + + 0.f, + 0.f, + 0.f, + 1.f); + } + + static Quaternion fromGLMatrix(const Matrix44f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + -(mat(2,1) - mat(1,2)) / (w * 4.f), + -(mat(0,2) - mat(2,0)) / (w * 4.f), + -(mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromMatrix (const Matrix33f &mat) { + float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f; + return Quaternion ( + (mat(2,1) - mat(1,2)) / (w * 4.f), + (mat(0,2) - mat(2,0)) / (w * 4.f), + (mat(1,0) - mat(0,1)) / (w * 4.f), + w); + } + + static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) { + double d = axis.norm(); + double s2 = std::sin (angle_rad * 0.5) / d; + return Quaternion ( + axis[0] * s2, + axis[1] * s2, + axis[2] * s2, + std::cos(angle_rad * 0.5) + ); + } + + static Quaternion fromEulerZYX (const Vector3f &zyx_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0]) + * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]); + } + + static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]); + } + + static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) { + return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2]) + * Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1]) + * Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]); + } + + Vector3f toEulerZYX () const { + return Vector3f (1.0f, 2.0f, 3.0f + ); + } + + Vector3f toEulerYXZ() const { + return Vector3f ( + atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1], + (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] + -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]), + asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]), + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2] + +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0] + ) + ); + }; + + Matrix33f toMatrix() const { + float x = (*this)[0]; + float y = (*this)[1]; + float z = (*this)[2]; + float w = (*this)[3]; + return Matrix33f ( + 1 - 2*y*y - 2*z*z, + 2*x*y - 2*w*z, + 2*x*z + 2*w*y, + + 2*x*y + 2*w*z, + 1 - 2*x*x - 2*z*z, + 2*y*z - 2*w*x, + + 2*x*z - 2*w*y, + 2*y*z + 2*w*x, + 1 - 2*x*x - 2*y*y + ); + } + + Quaternion conjugate() const { + return Quaternion ( + -(*this)[0], + -(*this)[1], + -(*this)[2], + (*this)[3]); + } + + Vector3f rotate (const Vector3f &vec) const { + Vector3f vn (vec); + Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat; + + res_quat = (*this) * vec_quat; + res_quat = res_quat * conjugate(); + + return Vector3f (res_quat[0], res_quat[1], res_quat[2]); + } +}; + +} /* namespace GL */ + + +// +// Stream operators +// +template +inline std::ostream& operator<<(std::ostream& output, const MatrixBase &matrix) { + size_t max_width = 0; + size_t out_width = output.width(); + + // get the widest number + for (size_t i = 0; i < matrix.rows(); i++) { + for (size_t j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream << matrix(i,j); + max_width = std::max (out_stream.str().size(),max_width); + } + } + + // overwrite width if it was explicitly prescribed + if (out_width != 0) { + max_width = out_width; + } + + for (unsigned int i = 0; i < matrix.rows(); i++) { + output.width(0); + output.width(out_width); + for (unsigned int j = 0; j < matrix.cols(); j++) { + std::stringstream out_stream; + out_stream.width (max_width); + out_stream << matrix(i,j); + output << out_stream.str(); + + if (j < matrix.cols() - 1) + output << " "; + } + + if (matrix.rows() > 1 && i < matrix.rows() - 1) + output << std::endl; + } + return output; +} + +} diff --git a/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h b/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h new file mode 100644 index 0000000..5da4865 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/SpatialAlgebraOperators.h @@ -0,0 +1,452 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_SPATIALALGEBRAOPERATORS_H +#define RBDL_SPATIALALGEBRAOPERATORS_H + +#include +#include + +namespace RigidBodyDynamics { + +namespace Math { + +inline Matrix3d VectorCrossMatrix (const Vector3d &vector) { + return Matrix3d ( + 0., -vector[2], vector[1], + vector[2], 0., -vector[0], + -vector[1], vector[0], 0. + ); +} + +/** \brief Compact representation for Spatial Inertia. */ +struct RBDL_DLLAPI SpatialRigidBodyInertia { + SpatialRigidBodyInertia() : + m (0.), + h (Vector3d::Zero(3,1)), + Ixx (0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.) + {} + SpatialRigidBodyInertia ( + double mass, const Vector3d &com_mass, const Matrix3d &inertia) : + m (mass), h (com_mass), + Ixx (inertia(0,0)), + Iyx (inertia(1,0)), Iyy(inertia(1,1)), + Izx (inertia(2,0)), Izy(inertia(2,1)), Izz(inertia(2,2)) + { } + SpatialRigidBodyInertia (double m, const Vector3d &h, + const double &Ixx, + const double &Iyx, const double &Iyy, + const double &Izx, const double &Izy, const double &Izz + ) : + m (m), h (h), + Ixx (Ixx), + Iyx (Iyx), Iyy(Iyy), + Izx (Izx), Izy(Izy), Izz(Izz) + { } + + SpatialVector operator* (const SpatialVector &mv) { + Vector3d mv_lower (mv[3], mv[4], mv[5]); + + Vector3d res_upper = Vector3d ( + Ixx * mv[0] + Iyx * mv[1] + Izx * mv[2], + Iyx * mv[0] + Iyy * mv[1] + Izy * mv[2], + Izx * mv[0] + Izy * mv[1] + Izz * mv[2] + ) + h.cross(mv_lower); + Vector3d res_lower = m * mv_lower - h.cross (Vector3d (mv[0], mv[1], mv[2])); + + return SpatialVector ( + res_upper[0], res_upper[1], res_upper[2], + res_lower[0], res_lower[1], res_lower[2] + ); + } + + SpatialRigidBodyInertia operator+ (const SpatialRigidBodyInertia &rbi) { + return SpatialRigidBodyInertia ( + m + rbi.m, + h + rbi.h, + Ixx + rbi.Ixx, + Iyx + rbi.Iyx, Iyy + rbi.Iyy, + Izx + rbi.Izx, Izy + rbi.Izy, Izz + rbi.Izz + ); + } + + void createFromMatrix (const SpatialMatrix &Ic) { + m = Ic(3,3); + h.set (-Ic(1,5), Ic(0,5), -Ic(0,4)); + Ixx = Ic(0,0); + Iyx = Ic(1,0); Iyy = Ic(1,1); + Izx = Ic(2,0); Izy = Ic(2,1); Izz = Ic(2,2); + } + + SpatialMatrix toMatrix() const { + SpatialMatrix result; + result(0,0) = Ixx; result(0,1) = Iyx; result(0,2) = Izx; + result(1,0) = Iyx; result(1,1) = Iyy; result(1,2) = Izy; + result(2,0) = Izx; result(2,1) = Izy; result(2,2) = Izz; + + result.block<3,3>(0,3) = VectorCrossMatrix(h); + result.block<3,3>(3,0) = - VectorCrossMatrix(h); + result.block<3,3>(3,3) = Matrix3d::Identity(3,3) * m; + + return result; + } + + void setSpatialMatrix (SpatialMatrix &mat) const { + mat(0,0) = Ixx; mat(0,1) = Iyx; mat(0,2) = Izx; + mat(1,0) = Iyx; mat(1,1) = Iyy; mat(1,2) = Izy; + mat(2,0) = Izx; mat(2,1) = Izy; mat(2,2) = Izz; + + mat(3,0) = 0.; mat(3,1) = h[2]; mat(3,2) = -h[1]; + mat(4,0) = -h[2]; mat(4,1) = 0.; mat(4,2) = h[0]; + mat(5,0) = h[1]; mat(5,1) = -h[0]; mat(5,2) = 0.; + + mat(0,3) = 0.; mat(0,4) = -h[2]; mat(0,5) = h[1]; + mat(1,3) = h[2]; mat(1,4) = 0.; mat(1,5) = -h[0]; + mat(2,3) = -h[1]; mat(2,4) = h[0]; mat(2,5) = 0.; + + mat(3,3) = m; mat(3,4) = 0.; mat(3,5) = 0.; + mat(4,3) = 0.; mat(4,4) = m; mat(4,5) = 0.; + mat(5,3) = 0.; mat(5,4) = 0.; mat(5,5) = m; + } + + static SpatialRigidBodyInertia createFromMassComInertiaC (double mass, const Vector3d &com, const Matrix3d &inertia_C) { + SpatialRigidBodyInertia result; + result.m = mass; + result.h = com * mass; + Matrix3d I = inertia_C + VectorCrossMatrix (com) * VectorCrossMatrix(com).transpose() * mass; + result.Ixx = I(0,0); + result.Iyx = I(1,0); + result.Iyy = I(1,1); + result.Izx = I(2,0); + result.Izy = I(2,1); + result.Izz = I(2,2); + return result; + } + + /// Mass + double m; + /// Coordinates of the center of mass + Vector3d h; + /// Inertia expressed at the origin + double Ixx, Iyx, Iyy, Izx, Izy, Izz; +}; + +/** \brief Compact representation of spatial transformations. + * + * Instead of using a verbose 6x6 matrix, this structure only stores a 3x3 + * matrix and a 3-d vector to store spatial transformations. It also + * encapsulates efficient operations such as concatenations and + * transformation of spatial vectors. + */ +struct RBDL_DLLAPI SpatialTransform { + SpatialTransform() : + E (Matrix3d::Identity(3,3)), + r (Vector3d::Zero(3,1)) + {} + SpatialTransform (const Matrix3d &rotation, const Vector3d &translation) : + E (rotation), + r (translation) + {} + + /** Same as X * v. + * + * \returns (E * w, - E * rxw + E * v) + */ + SpatialVector apply (const SpatialVector &v_sp) { + Vector3d v_rxw ( + v_sp[3] - r[1]*v_sp[2] + r[2]*v_sp[1], + v_sp[4] - r[2]*v_sp[0] + r[0]*v_sp[2], + v_sp[5] - r[0]*v_sp[1] + r[1]*v_sp[0] + ); + return SpatialVector ( + E(0,0) * v_sp[0] + E(0,1) * v_sp[1] + E(0,2) * v_sp[2], + E(1,0) * v_sp[0] + E(1,1) * v_sp[1] + E(1,2) * v_sp[2], + E(2,0) * v_sp[0] + E(2,1) * v_sp[1] + E(2,2) * v_sp[2], + E(0,0) * v_rxw[0] + E(0,1) * v_rxw[1] + E(0,2) * v_rxw[2], + E(1,0) * v_rxw[0] + E(1,1) * v_rxw[1] + E(1,2) * v_rxw[2], + E(2,0) * v_rxw[0] + E(2,1) * v_rxw[1] + E(2,2) * v_rxw[2] + ); + } + + /** Same as X^T * f. + * + * \returns (E^T * n + rx * E^T * f, E^T * f) + */ + SpatialVector applyTranspose (const SpatialVector &f_sp) { + Vector3d E_T_f ( + E(0,0) * f_sp[3] + E(1,0) * f_sp[4] + E(2,0) * f_sp[5], + E(0,1) * f_sp[3] + E(1,1) * f_sp[4] + E(2,1) * f_sp[5], + E(0,2) * f_sp[3] + E(1,2) * f_sp[4] + E(2,2) * f_sp[5] + ); + + return SpatialVector ( + E(0,0) * f_sp[0] + E(1,0) * f_sp[1] + E(2,0) * f_sp[2] - r[2] * E_T_f[1] + r[1] * E_T_f[2], + E(0,1) * f_sp[0] + E(1,1) * f_sp[1] + E(2,1) * f_sp[2] + r[2] * E_T_f[0] - r[0] * E_T_f[2], + E(0,2) * f_sp[0] + E(1,2) * f_sp[1] + E(2,2) * f_sp[2] - r[1] * E_T_f[0] + r[0] * E_T_f[1], + E_T_f [0], + E_T_f [1], + E_T_f [2] + ); + } + + /** Same as X^* I X^{-1} + */ + SpatialRigidBodyInertia apply (const SpatialRigidBodyInertia &rbi) { + return SpatialRigidBodyInertia ( + rbi.m, + E * (rbi.h - rbi.m * r), + E * + ( + Matrix3d ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ) + + VectorCrossMatrix (r) * VectorCrossMatrix (rbi.h) + + (VectorCrossMatrix(rbi.h - rbi.m * r) * VectorCrossMatrix (r)) + ) + * E.transpose() + ); + } + + /** Same as X^T I X + */ + SpatialRigidBodyInertia applyTranspose (const SpatialRigidBodyInertia &rbi) { + Vector3d E_T_mr = E.transpose() * rbi.h + rbi.m * r; + return SpatialRigidBodyInertia ( + rbi.m, + E_T_mr, + E.transpose() * + Matrix3d ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ) * E + - VectorCrossMatrix(r) * VectorCrossMatrix (E.transpose() * rbi.h) + - VectorCrossMatrix (E_T_mr) * VectorCrossMatrix (r)); + } + + SpatialVector applyAdjoint (const SpatialVector &f_sp) { + Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Vector3d (f_sp[3], f_sp[4], f_sp[5]))); + // Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Eigen::Map (&(f_sp[3])))); + + return SpatialVector ( + En_rxf[0], + En_rxf[1], + En_rxf[2], + E(0,0) * f_sp[3] + E(0,1) * f_sp[4] + E(0,2) * f_sp[5], + E(1,0) * f_sp[3] + E(1,1) * f_sp[4] + E(1,2) * f_sp[5], + E(2,0) * f_sp[3] + E(2,1) * f_sp[4] + E(2,2) * f_sp[5] + ); + } + + SpatialMatrix toMatrix () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E; + result.block<3,3>(0,3) = Matrix3d::Zero(3,3); + result.block<3,3>(3,0) = -_Erx; + result.block<3,3>(3,3) = E; + + return result; + } + + SpatialMatrix toMatrixAdjoint () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E; + result.block<3,3>(0,3) = -_Erx; + result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,3) = E; + + return result; + } + + SpatialMatrix toMatrixTranspose () const { + Matrix3d _Erx = + E * Matrix3d ( + 0., -r[2], r[1], + r[2], 0., -r[0], + -r[1], r[0], 0. + ); + SpatialMatrix result; + result.block<3,3>(0,0) = E.transpose(); + result.block<3,3>(0,3) = -_Erx.transpose(); + result.block<3,3>(3,0) = Matrix3d::Zero(3,3); + result.block<3,3>(3,3) = E.transpose(); + + return result; + } + + SpatialTransform inverse() const { + return SpatialTransform ( + E.transpose(), + - E * r + ); + } + + SpatialTransform operator* (const SpatialTransform &XT) const { + return SpatialTransform (E * XT.E, XT.r + XT.E.transpose() * r); + } + + void operator*= (const SpatialTransform &XT) { + r = XT.r + XT.E.transpose() * r; + E *= XT.E; + } + + Matrix3d E; + Vector3d r; +}; + +inline std::ostream& operator<<(std::ostream& output, const SpatialRigidBodyInertia &rbi) { + output << "rbi.m = " << rbi.m << std::endl; + output << "rbi.h = " << rbi.h.transpose(); + output << "rbi.Ixx = " << rbi.Ixx << std::endl; + output << "rbi.Iyx = " << rbi.Iyx << " rbi.Iyy = " << rbi.Iyy << std::endl; + output << "rbi.Izx = " << rbi.Izx << " rbi.Izy = " << rbi.Izy << " rbi.Izz = " << rbi.Izz << std::endl; + return output; +} + +inline std::ostream& operator<<(std::ostream& output, const SpatialTransform &X) { + output << "X.E = " << std::endl << X.E << std::endl; + output << "X.r = " << X.r.transpose(); + return output; +} + +inline SpatialTransform Xrot (double angle_rad, const Vector3d &axis) { + double s, c; + s = sin(angle_rad); + c = cos(angle_rad); + + return SpatialTransform ( + Matrix3d ( + axis[0] * axis[0] * (1.0f - c) + c, + axis[1] * axis[0] * (1.0f - c) + axis[2] * s, + axis[0] * axis[2] * (1.0f - c) - axis[1] * s, + + axis[0] * axis[1] * (1.0f - c) - axis[2] * s, + axis[1] * axis[1] * (1.0f - c) + c, + axis[1] * axis[2] * (1.0f - c) + axis[0] * s, + + axis[0] * axis[2] * (1.0f - c) + axis[1] * s, + axis[1] * axis[2] * (1.0f - c) - axis[0] * s, + axis[2] * axis[2] * (1.0f - c) + c + + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xrotx (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + return SpatialTransform ( + Matrix3d ( + 1., 0., 0., + 0., c, s, + 0., -s, c + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xroty (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + return SpatialTransform ( + Matrix3d ( + c, 0., -s, + 0., 1., 0., + s, 0., c + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xrotz (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + return SpatialTransform ( + Matrix3d ( + c, s, 0., + -s, c, 0., + 0., 0., 1. + ), + Vector3d (0., 0., 0.) + ); +} + +inline SpatialTransform Xtrans (const Vector3d &r) { + return SpatialTransform ( + Matrix3d::Identity(3,3), + r + ); +} + +inline SpatialMatrix crossm (const SpatialVector &v) { + return SpatialMatrix ( + 0, -v[2], v[1], 0, 0, 0, + v[2], 0, -v[0], 0, 0, 0, + -v[1], v[0], 0, 0, 0, 0, + 0, -v[5], v[4], 0, -v[2], v[1], + v[5], 0, -v[3], v[2], 0, -v[0], + -v[4], v[3], 0, -v[1], v[0], 0 + ); +} + +inline SpatialVector crossm (const SpatialVector &v1, const SpatialVector &v2) { + return SpatialVector ( + -v1[2] * v2[1] + v1[1] * v2[2], + v1[2] * v2[0] - v1[0] * v2[2], + -v1[1] * v2[0] + v1[0] * v2[1], + -v1[5] * v2[1] + v1[4] * v2[2] - v1[2] * v2[4] + v1[1] * v2[5], + v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5], + -v1[4] * v2[0] + v1[3] * v2[1] - v1[1] * v2[3] + v1[0] * v2[4] + ); +} + +inline SpatialMatrix crossf (const SpatialVector &v) { + return SpatialMatrix ( + 0, -v[2], v[1], 0, -v[5], v[4], + v[2], 0, -v[0], v[5], 0, -v[3], + -v[1], v[0], 0, -v[4], v[3], 0, + 0, 0, 0, 0, -v[2], v[1], + 0, 0, 0, v[2], 0, -v[0], + 0, 0, 0, -v[1], v[0], 0 + ); +} + +inline SpatialVector crossf (const SpatialVector &v1, const SpatialVector &v2) { + return SpatialVector ( + -v1[2] * v2[1] + v1[1] * v2[2] - v1[5] * v2[4] + v1[4] * v2[5], + v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5], + -v1[1] * v2[0] + v1[0] * v2[1] - v1[4] * v2[3] + v1[3] * v2[4], + - v1[2] * v2[4] + v1[1] * v2[5], + + v1[2] * v2[3] - v1[0] * v2[5], + - v1[1] * v2[3] + v1[0] * v2[4] + ); +} + +} /* Math */ + +} /* RigidBodyDynamics */ + +/* RBDL_SPATIALALGEBRAOPERATORS_H*/ +#endif diff --git a/3rdparty/rbdl/include/rbdl/compileassert.h b/3rdparty/rbdl/include/rbdl/compileassert.h new file mode 100644 index 0000000..59023f8 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/compileassert.h @@ -0,0 +1,46 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_COMPILE_ASSERT_H +#define RBDL_COMPILE_ASSERT_H + +/* + * This is a simple compile time assertion tool taken from: + * http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx + * written by Abhinaba Basu! + * + * Thanks! + */ + +#ifdef __cplusplus + +#define JOIN( X, Y ) JOIN2(X,Y) +#define JOIN2( X, Y ) X##Y + +namespace custom_static_assert +{ +template struct STATIC_ASSERT_FAILURE; +template <> struct STATIC_ASSERT_FAILURE { enum { value = 1 }; }; + +template struct custom_static_assert_test{}; +} + +#define COMPILE_ASSERT(x) \ + typedef ::custom_static_assert::custom_static_assert_test<\ +sizeof(::custom_static_assert::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\ +JOIN(_custom_static_assert_typedef, __LINE__) + +#else // __cplusplus + +#define COMPILE_ASSERT(x) extern int __dummy[(int)x] + +#endif // __cplusplus + +#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) + +// RBDL_COMPILE_ASSERT_H_ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl.h b/3rdparty/rbdl/include/rbdl/rbdl.h new file mode 100644 index 0000000..baccd77 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl.h @@ -0,0 +1,69 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_H +#define RBDL_H + +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +#include "rbdl/Body.h" +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Joint.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Constraints.h" + +#include "rbdl/rbdl_utils.h" + +/** \page api_version_checking_page API Changes + * @{ + * + * This documentation was created for API version 2.2.0. + * + * Here is a list of changes introduced by the different versions and what + * adjustements have to be made to migrate. + * + * \include api_changes.txt + */ + +/** Returns the API version at compile time of the library. */ +RBDL_DLLAPI int rbdl_get_api_version(); + +/** Ensures whether the RBDL library we are linking against is compatible + * with the the version we have from rbdl.h. + * + * To perform the check run: + * \code + * rbdl_check_api_version(API_VERSION); + * \endcode + * + * This function will abort if compatibility is not met or warn if you run + * a version that might not be entirely compatible. + * + * In most cases you want to specify a specific version to ensure you are + * using a compatible version. To do so replace API_VERSION by a + * value of the form 0xAABBCC where AA is the major, BB the minor, and CC + * the patch version in hex-format, e.g: + * + * \code + * rbdl_check_api_version(0x020A0C); + * \endcode + * + * Would abort if the API major version is not 2 (= 0x02), warn if the + * linked minor version is not 10 (= 0x0A). The patch version 12 (= 0x12) + * does not have an influence on compatibility. + */ +RBDL_DLLAPI void rbdl_check_api_version(int version); + +/** Prints version information to standard output */ +RBDL_DLLAPI void rbdl_print_version(); + +/* RBDL_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake b/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake new file mode 100644 index 0000000..cccd51f --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_config.h.cmake @@ -0,0 +1,85 @@ +/* +* 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_USE_SIMPLE_MATH +#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_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_BUILD_ADDON_MUSCLE_FITTING + +/* compatibility defines */ +#ifdef _WIN32 +#define __func__ __FUNCTION__ +#define M_PI 3.1415926535897932384 +#endif + +// Handle portable symbol export. +// Defining manually which symbol should be exported is required +// under Windows whether MinGW or MSVC is used. +// +// The headers then have to be able to work in two different modes: +// - dllexport when one is building the library, +// - dllimport for clients using the library. +// +// On Linux, set the visibility accordingly. If C++ symbol visibility +// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility +# if defined _WIN32 || defined __CYGWIN__ +// On Microsoft Windows, use dllimport and dllexport to tag symbols. +# define RBDL_DLLIMPORT __declspec(dllimport) +# define RBDL_DLLEXPORT __declspec(dllexport) +# define RBDL_DLLLOCAL +# else +// On Linux, for GCC >= 4, tag symbols using GCC extension. +# if __GNUC__ >= 4 +# define RBDL_DLLIMPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLEXPORT __attribute__ ((visibility("default"))) +# define RBDL_DLLLOCAL __attribute__ ((visibility("hidden"))) +# else +// Otherwise (GCC < 4 or another compiler is used), export everything. +# define RBDL_DLLIMPORT +# define RBDL_DLLEXPORT +# define RBDL_DLLLOCAL +# endif // __GNUC__ >= 4 +# endif // defined _WIN32 || defined __CYGWIN__ + +# ifdef RBDL_BUILD_STATIC +// If one is using the library statically, get rid of +// extra information. +# define RBDL_DLLAPI +# define RBDL_LOCAL +# else +// Depending on whether one is building or using the +// library define DLLAPI to import or export. +# ifdef rbdl_EXPORTS +# define RBDL_DLLAPI RBDL_DLLEXPORT +# else +# define RBDL_DLLAPI RBDL_DLLIMPORT +# endif // RBDL_EXPORTS +# define RBDL_LOCAL RBDL_DLLLOCAL +# endif // RBDL_BUILD_STATIC + +#ifdef __GNUC__ +# define UNUSED(x) UNUSED_ ## x __attribute__((__unused__)) +# define UNUSED_FUNCTION(x) __attribute__((__unused__)) UNUSED_ ## x +#else +# define UNUSED(x) UNUSED_ ## x +# define UNUSED_FUNCTION(x) UNUSED_ ## x +#endif + +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h b/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h new file mode 100644 index 0000000..4f374f2 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_eigenmath.h @@ -0,0 +1,273 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_EIGENMATH_H +#define RBDL_EIGENMATH_H + +/* Exporting templated symbols is tricky when using MSVC. The following lines + * causes the classes in this file not to be explicitly exported. Instead + * they are already implicitly exported. + */ +#if defined(WIN32) && defined(rbdl_EXPORTS) +#define RBDL_TEMPLATE_DLLAPI +#else +#define RBDL_TEMPLATE_DLLAPI RBDL_DLLAPI +#endif + +class RBDL_TEMPLATE_DLLAPI Vector2_t : public Eigen::Vector2d +{ + public: + typedef Eigen::Vector2d Base; + + template + Vector2_t(const Eigen::MatrixBase& other) + : Eigen::Vector2d(other) + {} + + template + Vector2_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Vector2_t() + {} + + EIGEN_STRONG_INLINE Vector2_t( + const double& v0, const double& v1 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1; + } + + void set(const double& v0, const double& v1) + { + Base::_check_template_params(); + + (*this) << v0, v1; + } +}; + +class RBDL_TEMPLATE_DLLAPI Vector3_t : public Eigen::Vector3d +{ + public: + typedef Eigen::Vector3d Base; + + template + Vector3_t(const Eigen::MatrixBase& other) + : Eigen::Vector3d(other) + {} + + template + Vector3_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Vector3_t() + {} + + EIGEN_STRONG_INLINE Vector3_t( + const double& v0, const double& v1, const double& v2 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2; + } + + void set(const double& v0, const double& v1, const double& v2) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2; + } +}; + +class RBDL_TEMPLATE_DLLAPI Matrix3_t : public Eigen::Matrix3d +{ + public: + typedef Eigen::Matrix3d Base; + + template + Matrix3_t(const Eigen::MatrixBase& other) + : Eigen::Matrix3d(other) + {} + + template + Matrix3_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Matrix3_t() + {} + + EIGEN_STRONG_INLINE Matrix3_t( + const double& m00, const double& m01, const double& m02, + const double& m10, const double& m11, const double& m12, + const double& m20, const double& m21, const double& m22 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, + m10, m11, m12, + m20, m21, m22 + ; + } +}; + +class RBDL_TEMPLATE_DLLAPI Vector4_t : public Eigen::Vector4d +{ + public: + typedef Eigen::Vector4d Base; + + template + Vector4_t(const Eigen::MatrixBase& other) + : Eigen::Vector4d(other) + {} + + template + Vector4_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE Vector4_t() + {} + + EIGEN_STRONG_INLINE Vector4_t( + const double& v0, const double& v1, const double& v2, const double& v3 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3; + } + + void set(const double& v0, const double& v1, const double& v2, const double& v3) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3; + } +}; + +class RBDL_TEMPLATE_DLLAPI SpatialVector_t : public Eigen::Matrix +{ + public: + typedef Eigen::Matrix Base; + + template + SpatialVector_t(const Eigen::MatrixBase& other) + : Eigen::Matrix(other) + {} + + template + SpatialVector_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE SpatialVector_t() + {} + + EIGEN_STRONG_INLINE SpatialVector_t( + const double& v0, const double& v1, const double& v2, + const double& v3, const double& v4, const double& v5 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3, v4, v5; + } + + void set( + const double& v0, const double& v1, const double& v2, + const double& v3, const double& v4, const double& v5 + ) + { + Base::_check_template_params(); + + (*this) << v0, v1, v2, v3, v4, v5; + } +}; + +class RBDL_TEMPLATE_DLLAPI SpatialMatrix_t : public Eigen::Matrix +{ + public: + typedef Eigen::Matrix Base; + + template + SpatialMatrix_t(const Eigen::MatrixBase& other) + : Eigen::Matrix(other) + {} + + template + SpatialMatrix_t& operator=(const Eigen::MatrixBase& other) + { + this->Base::operator=(other); + return *this; + } + + EIGEN_STRONG_INLINE SpatialMatrix_t() + {} + + EIGEN_STRONG_INLINE SpatialMatrix_t( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35, + const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, + const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03, m04, m05 + , m10, m11, m12, m13, m14, m15 + , m20, m21, m22, m23, m24, m25 + , m30, m31, m32, m33, m34, m35 + , m40, m41, m42, m43, m44, m45 + , m50, m51, m52, m53, m54, m55 + ; + } + + void set( + const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, + const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, + const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, + const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35, + const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, + const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55 + ) + { + Base::_check_template_params(); + + (*this) + << m00, m01, m02, m03, m04, m05 + , m10, m11, m12, m13, m14, m15 + , m20, m21, m22, m23, m24, m25 + , m30, m31, m32, m33, m34, m35 + , m40, m41, m42, m43, m44, m45 + , m50, m51, m52, m53, m54, m55 + ; + } +}; + +/* _RBDL_EIGENMATH_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_math.h b/3rdparty/rbdl/include/rbdl/rbdl_math.h new file mode 100644 index 0000000..106c33c --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_math.h @@ -0,0 +1,81 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MATH_H +#define RBDL_MATH_H + +#include "rbdl/rbdl_config.h" + +#ifdef RBDL_USE_SIMPLE_MATH + +#include "rbdl/SimpleMath/SimpleMath.h" +#include + +typedef SimpleMath::Matrix Vector2_t; +typedef SimpleMath::Matrix Vector3_t; +typedef SimpleMath::Matrix Matrix3_t; +typedef SimpleMath::Matrix Vector4_t; + +typedef SimpleMath::Matrix SpatialVector_t; +typedef SimpleMath::Matrix SpatialMatrix_t; + +typedef SimpleMath::Matrix Matrix63_t; +typedef SimpleMath::Matrix Matrix43_t; + +typedef SimpleMath::Matrix MatrixN_t; +typedef SimpleMath::Matrix VectorN_t; + +#else // RBDL_USE_SIMPLE_MATH + +#include +#include +#include + +#include "rbdl/rbdl_eigenmath.h" + +typedef Eigen::Matrix Matrix63_t; +typedef Eigen::Matrix Matrix43_t; + +typedef Eigen::VectorXd VectorN_t; +typedef Eigen::MatrixXd MatrixN_t; + +#endif // RBDL_USE_SIMPLE_MATH + +namespace RigidBodyDynamics { + +/** \brief Math types such as vectors and matrices and utility functions. */ +namespace Math { +typedef Vector2_t Vector2d; +typedef Vector3_t Vector3d; +typedef Vector4_t Vector4d; +typedef Matrix3_t Matrix3d; +typedef SpatialVector_t SpatialVector; +typedef SpatialMatrix_t SpatialMatrix; +typedef Matrix63_t Matrix63; +typedef Matrix43_t Matrix43; +typedef VectorN_t VectorNd; +typedef MatrixN_t MatrixNd; +} /* Math */ + +} /* RigidBodyDynamics */ + +#include "rbdl/Quaternion.h" +#include "rbdl/SpatialAlgebraOperators.h" + +// If we use Eigen3 we have to create specializations of the STL +// std::vector such that the alignment is done properly. +#ifndef RBDL_USE_SIMPLE_MATH + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix63) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix43) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform) + EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia) +#endif + + /* RBDL_MATH_H_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h b/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h new file mode 100644 index 0000000..d6a200b --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_mathutils.h @@ -0,0 +1,265 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_MATHUTILS_H +#define RBDL_MATHUTILS_H + +#include +#include + +#include "rbdl/rbdl_math.h" + +namespace RigidBodyDynamics { +struct Model; + +namespace Math { + +/** \brief Available solver methods for the linear systems. + * + * Please note that these methods are only available when Eigen3 is used. + * When the math library SimpleMath is used it will always use a slow + * column pivoting gauss elimination. + */ +enum RBDL_DLLAPI LinearSolver { + LinearSolverUnknown = 0, + LinearSolverPartialPivLU, + LinearSolverColPivHouseholderQR, + LinearSolverHouseholderQR, + LinearSolverLLT, + LinearSolverLast +}; + +extern RBDL_DLLAPI Vector3d Vector3dZero; +extern RBDL_DLLAPI Matrix3d Matrix3dIdentity; +extern RBDL_DLLAPI Matrix3d Matrix3dZero; + +RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) { + // TODO: use memory mapping operators for Eigen + VectorNd result (n); + + for (unsigned int i = 0; i < n; i++) { + result[i] = ptr[i]; + } + + return result; +} + +RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major = true) { + MatrixNd result (rows, cols); + + if (row_major) { + for (unsigned int i = 0; i < rows; i++) { + for (unsigned int j = 0; j < cols; j++) { + result(i,j) = ptr[i * cols + j]; + } + } + } else { + for (unsigned int i = 0; i < rows; i++) { + for (unsigned int j = 0; j < cols; j++) { + result(i,j) = ptr[i + j * rows]; + } + } + } + + return result; +} + +/// \brief Solves a linear system using gaussian elimination with pivoting +RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x); + +// \todo write test +RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix); + +RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a, + const SpatialMatrix &matrix_b, double epsilon); +RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a, + const SpatialVector &vector_b, double epsilon); + +/** \brief Translates the inertia matrix to a new center. */ +RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com); + +/** \brief Creates a transformation of a linear displacement + * + * This can be used to specify the translation to the joint center when + * adding a body to a model. See also section 2.8 in RBDA. + * + * \note The transformation returned is for motions. For a transformation for forces + * \note one has to conjugate the matrix. + * + * \param displacement The displacement as a 3D vector + */ +RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement); + +/** \brief Creates a rotational transformation around the Z-axis + * + * Creates a rotation around the current Z-axis by the given angle + * (specified in radians). + * + * \param zrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot); + +/** \brief Creates a rotational transformation around the Y-axis + * + * Creates a rotation around the current Y-axis by the given angle + * (specified in radians). + * + * \param yrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot); + +/** \brief Creates a rotational transformation around the X-axis + * + * Creates a rotation around the current X-axis by the given angle + * (specified in radians). + * + * \param xrot Rotation angle in radians. + */ +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot); + +/** \brief Creates a spatial transformation for given parameters + * + * Creates a transformation to a coordinate system that is first rotated + * and then translated. + * + * \param displacement The displacement to the new origin + * \param zyx_euler The orientation of the new coordinate system, specifyed + * by ZYX-Euler angles. + */ +RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler); + +RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + return Matrix3d ( + 1., 0., 0., + 0., c, s, + 0., -s, c + ); +} + +RBDL_DLLAPI inline Matrix3d roty (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + return Matrix3d ( + c, 0., -s, + 0., 1., 0., + s, 0., c + ); +} + +RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + return Matrix3d ( + c, s, 0., + -s, c, 0., + 0., 0., 1. + ); +} + +RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) { + double s, c; + s = sin (x); + c = cos (x); + return Matrix3d ( + 0., 0., 0., + 0., -s * xdot, c * xdot, + 0., -c * xdot,-s * xdot + ); +} + +RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) { + double s, c; + s = sin (y); + c = cos (y); + return Matrix3d ( + -s * ydot, 0., - c * ydot, + 0., 0., 0., + c * ydot, 0., - s * ydot + ); +} + +RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) { + double s, c; + s = sin (z); + c = cos (z); + return Matrix3d ( + -s * zdot, c * zdot, 0., + -c * zdot, -s * zdot, 0., + 0., 0., 0. + ); +} + +RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) { + double sy = sin(zyx_angles[1]); + double cy = cos(zyx_angles[1]); + double sx = sin(zyx_angles[2]); + double cx = cos(zyx_angles[2]); + + return Vector3d ( + zyx_angle_rates[2] - sy * zyx_angle_rates[0], + cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0], + -sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0] + ); +} + +RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) { + Matrix3d RzT = rotz(zyx_angles[0]).transpose(); + Matrix3d RyT = roty(zyx_angles[1]).transpose(); + + return Vector3d ( + Vector3d (0., 0., zyx_rates[0]) + + RzT * Vector3d (0., zyx_rates[1], 0.) + + RzT * RyT * Vector3d (zyx_rates[2], 0., 0.) + ); +} + +RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) { + double sy = sin(zyx_angles[1]); + double cy = cos(zyx_angles[1]); + double sx = sin(zyx_angles[2]); + double cx = cos(zyx_angles[2]); + double xdot = zyx_angle_rates[2]; + double ydot = zyx_angle_rates[1]; + double zdot = zyx_angle_rates[0]; + double xddot = zyx_angle_rates_dot[2]; + double yddot = zyx_angle_rates_dot[1]; + double zddot = zyx_angle_rates_dot[0]; + + return Vector3d ( + xddot - (cy * ydot * zdot + sy * zddot), + -sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot), + -cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot) + ); +} + +RBDL_DLLAPI +void SparseFactorizeLTL (Model &model, Math::MatrixNd &H); + +RBDL_DLLAPI +void SparseMultiplyHx (Model &model, Math::MatrixNd &L); + +RBDL_DLLAPI +void SparseMultiplyLx (Model &model, Math::MatrixNd &L); +RBDL_DLLAPI +void SparseMultiplyLTx (Model &model, Math::MatrixNd &L); + +RBDL_DLLAPI +void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x); +RBDL_DLLAPI +void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x); + +} /* Math */ + +} /* RigidBodyDynamics */ + +/* RBDL_MATHUTILS_H */ +#endif diff --git a/3rdparty/rbdl/include/rbdl/rbdl_utils.h b/3rdparty/rbdl/include/rbdl/rbdl_utils.h new file mode 100644 index 0000000..dfeb291 --- /dev/null +++ b/3rdparty/rbdl/include/rbdl/rbdl_utils.h @@ -0,0 +1,93 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#ifndef RBDL_UTILS_H +#define RBDL_UTILS_H + +#include +#include +#include + +namespace RigidBodyDynamics { + +struct Model; + +/** \brief Namespace that contains optional helper functions */ +namespace Utils { +/** \brief Creates a human readable overview of the model. */ +RBDL_DLLAPI std::string GetModelHierarchy (const Model &model); +/** \brief Creates a human readable overview of the Degrees of Freedom. */ +RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model); +/** \brief Creates a human readable overview of the locations of all bodies that have names. */ +RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model); + +/** \brief Computes the Center of Mass (COM) and optionally its linear velocity. + * + * When only interested in computing the location of the COM you can use + * NULL as value for com_velocity. + * + * \param model The model for which we want to compute the COM + * \param q The current joint positions + * \param qdot The current joint velocities + * \param mass (output) total mass of the model + * \param com (output) location of the Center of Mass of the model in base coordinates + * \param qddot (optional input) A pointer to the current joint accelerations + * \param com_velocity (optional output) linear velocity of the COM in base coordinates + * \param com_acceleration (optional output) linear acceleration of the COM in base coordinates + * \param angular_momentum (optional output) angular momentum of the model at the COM in base coordinates + * \param change_of_angular_momentum (optional output) change of angular momentum of the model at the COM in base coordinates + * \param update_kinematics (optional input) whether the kinematics should be updated (defaults to true) + * + * \note When wanting to compute com_acceleration or change_of_angular + * momentum one has to provide qddot. In all other cases one can use NULL + * as argument for qddot. + */ +RBDL_DLLAPI void CalcCenterOfMass ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + const Math::VectorNd *qddot, + double &mass, + Math::Vector3d &com, + Math::Vector3d *com_velocity = NULL, + Math::Vector3d *com_acceleration = NULL, + Math::Vector3d *angular_momentum = NULL, + Math::Vector3d *change_of_angular_momentum = NULL, + bool update_kinematics = true +); + +/** \brief Computes the Zero-Moment-Point (ZMP) on a given contact surface. + * + * \param model The model for which we want to compute the ZMP + * \param q The current joint positions + * \param qdot The current joint velocities + * \param qdot The current joint accelerations + * \param normal The normal of the respective contact surface + * \param point A point on the contact surface + * \param zmp (output) location of the Zero-Moment-Point of the model in base coordinates projected on the given contact surface + * \param update_kinematics (optional input) whether the kinematics should be updated (defaults to true) + */ +RBDL_DLLAPI void CalcZeroMomentPoint ( + Model &model, + const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd &qddot, + Math::Vector3d* zmp, + const Math::Vector3d &normal = Math::Vector3d (0., 0., 1.), + const Math::Vector3d &point = Math::Vector3d (0., 0., 0.), + bool update_kinematics = true +); + +/** \brief Computes the potential energy of the full model. */ +RBDL_DLLAPI double CalcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics = true); + +/** \brief Computes the kinetic energy of the full model. */ +RBDL_DLLAPI double CalcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics = true); +} + +} + +/* RBDL_UTILS_H */ +#endif diff --git a/3rdparty/rbdl/python/CMakeLists.txt b/3rdparty/rbdl/python/CMakeLists.txt new file mode 100644 index 0000000..e22fd6a --- /dev/null +++ b/3rdparty/rbdl/python/CMakeLists.txt @@ -0,0 +1,86 @@ +IF (RBDL_USE_PYTHON_2) + FIND_PROGRAM ( PYTHON "python2" ) + SET (Python_ADDITIONAL_VERSIONS 2.7) + INCLUDE ( FindNumPy2 ) + MESSAGE(STATUS ${PYTHON_NUMPY_2_INCLUDE_DIR}) + SET( PYTHON_BIN python2) +ELSE(RBDL_USE_PYTHON_2) + FIND_PROGRAM ( PYTHON "python3" ) + SET (Python_ADDITIONAL_VERSIONS 3.7) + INCLUDE ( FindNumPy3 ) + MESSAGE(STATUS ${PYTHON_NUMPY_3_INCLUDE_DIR}) + SET( PYTHON_BIN python3) +ENDIF (RBDL_USE_PYTHON_2) + +INCLUDE ( UseCython ) + + +FILE( COPY "crbdl.pxd" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}") + +CONFIGURE_FILE ( + ${CMAKE_CURRENT_SOURCE_DIR}/setup.py.cmake + ${CMAKE_CURRENT_BINARY_DIR}/setup.py + ) + +# Process the rbdl-wrapper.pyx to generate rbdl.pyx BEFORE build time +# else cython_add_module will complain on configuration +EXECUTE_PROCESS ( + OUTPUT_VARIABLE WRAPPERGEN_OUT + COMMAND ${PYTHON_BIN} + ${CMAKE_CURRENT_SOURCE_DIR}/wrappergen.py + ${CMAKE_CURRENT_SOURCE_DIR}/rbdl-wrapper.pyx + ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pyx +) +MESSAGE (STATUS ${WRAPPERGEN_OUT}) + + +# Enable C++11 (or C++0x for older compilers) +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +# If the pyx file is a C++ file, we should specify that here. +set_source_files_properties( ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pyx PROPERTIES CYTHON_IS_CXX TRUE ) + +# Multi-file cython modules do not appear to be working at the moment. +# NOTE there is some directory property problem in UseCython when build +# from CMAKE_CURRENT_BINARY_DIR +cython_add_module( rbdl-python ${CMAKE_CURRENT_BINARY_DIR}/rbdl.pyx ) + +#SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES PREFIX "") +SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES OUTPUT_NAME "rbdl") + +IF (RBDL_USE_PYTHON_2) + INCLUDE_DIRECTORIES ( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/python + ${PROJECT_SOURCE_DIR} + ${PYTHON_NUMPY_2_INCLUDE_DIR} + ) +ELSE(RBDL_USE_PYTHON_2) + INCLUDE_DIRECTORIES ( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/python + ${PROJECT_SOURCE_DIR} + ${PYTHON_NUMPY_3_INCLUDE_DIR} + ) +ENDIF (RBDL_USE_PYTHON_2) + +TARGET_LINK_LIBRARIES (rbdl-python rbdl ) + +IF (RBDL_BUILD_ADDON_LUAMODEL) + TARGET_LINK_LIBRARIES (rbdl-python rbdl_luamodel ) +ENDIF() + +IF (RBDL_BUILD_ADDON_URDFREADER) + TARGET_LINK_LIBRARIES (rbdl-python rbdl_urdfreader ) +ENDIF() + +INSTALL ( CODE "EXECUTE_PROCESS(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_BIN} setup.py install)") diff --git a/3rdparty/rbdl/python/README.md b/3rdparty/rbdl/python/README.md new file mode 100644 index 0000000..06c2d39 --- /dev/null +++ b/3rdparty/rbdl/python/README.md @@ -0,0 +1,53 @@ +# Python wrapper for RBDL + +This wrapper uses Cython to wrap RBDL in a Python module. The code requires +C++11 features and must be compiled with the flags ```-std=c++11``` (or on +older compilers: ```-std=c++0x```). It closely follows the C++ API. All +functions are found in the module ```rbdl```, e.g. + + rbdl.ForwardDynamics (model, q, qdot, tau, qddot) + +computes the accelerations qddot of the forward dynamics for given model, +q, qdot, tau. + +Arguments are all passed as reference and where possible, the wrapper +avoids copying values, especially for larger matrices such as Jacobians, +and joint-space inertia matrix. + +All functions have embedded signatures to ease the use from within IPython, +e.g. ```rbdl.ForwardDynamics?``` shows required arguments for the function. + +# Highlights + +Wrappers for the following features are already implemented: + +* supports model creation from Python code +* supports model loading from LuaModel and URDF +* operates directly on raw numpy data for Jacobians and joint-space inertia + matrix - no copying required! +* direct access to almost all values of the Model structure +* all functions of Dynamics.h are wrapped: + - O(n) inverse dynamics via RNEA + - O(n) forward dynamics via ABA + - Coriolis term computation via simplified RNEA + - computation of joint-space inertia matrix via CRBA +* kinematic computations: + - body <-> world transformations + - body point positions, velocities, and accelerations, including their + 6-D counterparts + - point Jacobians (translations) + - 6-D Jacobians (angular and linear movement) + - Spatial 6-D body Jacobians +* model mass, Center of Mass (CoM), CoM velocity, centroidal angular momentum + +# Differences to the C++ API + +The wrapper function ```rbdl.CalcCenterOfMass``` has a scalar return value +which is the mass of the model. Therefore the function does not use the +mass parameter when calling it. + +# ToDo + +* wrapping of constraint sets, and contact dynamics +* inverse kinematics +* documentation diff --git a/3rdparty/rbdl/python/crbdl.pxd b/3rdparty/rbdl/python/crbdl.pxd new file mode 100644 index 0000000..e66fcee --- /dev/null +++ b/3rdparty/rbdl/python/crbdl.pxd @@ -0,0 +1,487 @@ +#cython: boundscheck=False + +from libcpp.string cimport string +from libcpp cimport bool +from libcpp.vector cimport vector + +cdef extern from "" namespace "RigidBodyDynamics::Math": + cdef cppclass VectorNd: + VectorNd () + VectorNd (int dim) + int rows() + int cols() + void resize (int) + double& operator[](int) + double* data() + + cdef cppclass Vector3d: + Vector3d () + int rows() + int cols() + double& operator[](int) + double* data() + + cdef cppclass Quaternion: + Quaternion () + int rows() + int cols() + double& operator[](int) + double* data() + Matrix3d toMatrix() +# Quaternion fromMatrix (Matrix3d &mat) + + cdef cppclass SpatialVector: + SpatialVector () + int rows() + int cols() + double& operator[](int) + double* data() + + cdef cppclass Matrix3d: + Matrix3d () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + + cdef cppclass MatrixNd: + MatrixNd () + MatrixNd (int rows, int cols) + int rows() + int cols() + void resize (int,int) + double& coeff "operator()"(int,int) + double* data() + void setZero() + + cdef cppclass SpatialMatrix: + SpatialMatrix () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + + cdef cppclass Matrix63: + Matrix63 () + int rows() + int cols() + double& coeff "operator()"(int,int) + double* data() + +cdef extern from "" namespace "RigidBodyDynamics::Math::Quaternion": + Quaternion fromMatrix(const Matrix3d &mat) + +cdef extern from "" namespace "RigidBodyDynamics::Math": + cdef cppclass SpatialTransform: + SpatialTransform() + SpatialMatrix toMatrix() + SpatialTransform inverse() + SpatialTransform operator*(const SpatialTransform&) + Matrix3d E + Vector3d r + + cdef cppclass SpatialRigidBodyInertia: + SpatialRigidBodyInertia() + SpatialMatrix toMatrix() + + double m + Vector3d h + double Ixx, Iyx, Iyy, Izx, Izy, Izz + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Body: + Body() + Body(const double mass, const Vector3d &com, const Matrix3d &inertia) + double mMass + Vector3d mCenterOfMass + Matrix3d mInertia + bool mIsVirtual + + cdef cppclass FixedBody: + FixedBody() + double mMass + Vector3d mCenterOfMass + Matrix3d mInertia + unsigned int mMovableParent + SpatialTransform mParentTransform + SpatialTransform mBaseTransform + bool mIsVirtual + + +cdef extern from "" namespace "RigidBodyDynamics": + cdef enum JointType: + JointTypeUndefined = 0 + JointTypeRevolute + JointTypePrismatic + JointTypeRevoluteX + JointTypeRevoluteY + JointTypeRevoluteZ + JointTypeSpherical + JointTypeEulerZYX + JointTypeEulerXYZ + JointTypeEulerYXZ + JointTypeTranslationXYZ + JointTypeFloatingBase + JointTypeFixed + JointType1DoF + JointType2DoF + JointType3DoF + JointType4DoF + JointType5DoF + JointType6DoF + JointTypeCustom + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Joint: + Joint() + Joint(JointType joint_type) + SpatialVector* mJointAxes + JointType mJointType + unsigned int mDoFCount + unsigned int q_index + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass Model: + Model() + unsigned int AddBody (const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + string body_name + ) + unsigned int AppendBody (const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + string body_name + ) + unsigned int GetParentBodyId( + unsigned int body_id) + unsigned int GetBodyId( + const char *body_name) + string GetBodyName ( + unsigned int body_id) + bool IsBodyId ( + unsigned int body_id) + bool IsFixedBodyId ( + unsigned int body_id) + Quaternion GetQuaternion ( + unsigned int body_id, + const VectorNd &q) + void SetQuaternion ( + unsigned int body_id, + const Quaternion &quat, + VectorNd &q) + + vector[unsigned int] _lambda + vector[unsigned int] lambda_q +# vector[vector[unsigned int]] mu + + unsigned int dof_count + unsigned int q_size + unsigned int qdot_size + unsigned int previously_added_body_id + + Vector3d gravity + vector[SpatialVector] v + vector[SpatialVector] a + + vector[Joint] mJoints + vector[SpatialVector] S + vector[SpatialVector] v_J + vector[SpatialVector] c_J + + vector[unsigned int] mJointUpdateOrder + + vector[SpatialTransform] X_T + + vector[unsigned int] mFixedJointCount + + vector[Matrix63] multdof3_S + vector[Matrix63] multdof3_U + vector[Matrix63] multdof3_Dinv + vector[Matrix63] multdof3_u + vector[unsigned int] multdof3_w_index + + vector[SpatialVector] c + vector[SpatialMatrix] IA + vector[SpatialVector] pA + vector[SpatialVector] U + VectorNd d + VectorNd u + vector[SpatialVector] f + vector[SpatialRigidBodyInertia] I + vector[SpatialRigidBodyInertia] Ic + vector[SpatialVector] hc + + vector[SpatialTransform] X_lambda + vector[SpatialTransform] X_base + + vector[FixedBody] mFixedBodies + unsigned int fixed_body_discriminator + + vector[Body] mBodies + +cdef extern from "" namespace "RigidBodyDynamics": + cdef void UpdateKinematics (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot) + + cdef Vector3d CalcBodyToBaseCoordinates (Model& model, + const VectorNd &q, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcBaseToBodyCoordinates (Model& model, + const VectorNd &q, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Matrix3d CalcBodyWorldOrientation (Model& model, + const VectorNd &q, + const unsigned int body_id, + bool update_kinematics) + + cdef Vector3d CalcPointVelocity (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcPointAcceleration (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef SpatialVector CalcPointVelocity6D (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef SpatialVector CalcPointAcceleration6D (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot, + const unsigned int body_id, + const Vector3d &body_point_coordinates, + bool update_kinematics) + + cdef Vector3d CalcAngularVelocityfromMatrix ( + const Matrix3d &RotMat + ) + + cdef cppclass InverseKinematicsConstraintSet: + InverseKinematicsConstraintSet() + unsigned int AddPointConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &target_pos + ) + + unsigned int AddOrientationConstraint ( + unsigned int body_id, + const Matrix3d &target_orientation + ) + + unsigned int AddFullConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &target_pos, + const Matrix3d &target_orientation + ) + + unsigned int ClearConstraints() + + unsigned int num_constraints + double damper # lambda is built-in keyword + unsigned int num_steps + unsigned int max_steps + double step_tol + double constraint_tol + double error_norm + + MatrixNd J # Jacobian of all constraints + VectorNd e # Vector of all constraint residuals + + vector[unsigned int] body_ids + vector[Vector3d] body_points + vector[Vector3d] target_positions + vector[Matrix3d] target_orientations + + cdef bool InverseKinematics ( + Model &model, + const VectorNd &Qinit, + InverseKinematicsConstraintSet &CS, + VectorNd &Qres + ) + +cdef extern from "" namespace "RigidBodyDynamics::Utils": + cdef void CalcCenterOfMass (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd *qdot, + double &mass, + Vector3d &com, + Vector3d *com_velocity, + Vector3d *com_acceleration, + Vector3d *angular_momentum, + Vector3d *change_of_angular_momentum, + bool update_kinematics) + + cdef void CalcZeroMomentPoint (Model& model, + const VectorNd &q, + const VectorNd &qdot, + const VectorNd &qddot, + Vector3d* zmp, + const Vector3d &normal, + const Vector3d &point, + bool update_kinematics) + +cdef extern from "" namespace "RigidBodyDynamics": + cdef cppclass ConstraintSet: + ConstraintSet() + unsigned int AddContactConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &world_normal, + const char* constraint_name, + double normal_acceleration) + + unsigned int AddLoopConstraint ( + unsigned int id_predecessor, + unsigned int id_successor, + const SpatialTransform &X_predecessor, + const SpatialTransform &X_successor, + const SpatialVector &axis, + bool enable_stabilization, + double stabilization_param, + const char *constraint_name) + + ConstraintSet Copy() + # void SetSolver (Math::LinearSolver solver) + bool Bind (const Model &model) + + size_t size() + void clear() + # Math::LinearSolver + bool bound + + vector[string] name + vector[unsigned int] body + vector[Vector3d] point + vector[Vector3d] normal + + VectorNd acceleration + VectorNd force + VectorNd impulse + VectorNd v_plus + + MatrixNd H + VectorNd C + VectorNd gamma + VectorNd G + + MatrixNd A + VectorNd b + VectorNd x + + MatrixNd GT_qr_Q + MatrixNd Y + MatrixNd Z + VectorNd qddot_y + VectorNd qddot_z + + MatrixNd K + VectorNd a + VectorNd QDDot_t + VectorNd QDDot_0 + + vector[SpatialVector] f_t + vector[SpatialVector] f_ext_constraints + vector[Vector3d] point_accel_0 + + vector[SpatialVector] d_pA + vector[SpatialVector] d_a + VectorNd d_u + + vector[SpatialMatrix] d_IA + vector[SpatialVector] d_U + + VectorNd d_d + vector[Vector3d] d_multdof3_u + +cdef extern from "rbdl_ptr_functions.h" namespace "RigidBodyDynamics": + cdef void CalcPointJacobianPtr (Model& model, + const double *q_ptr, + unsigned int body_id, + const Vector3d &point_position, + double *G, + bool update_kinematics) + + cdef void CalcPointJacobian6DPtr (Model &model, + const double *q_ptr, + unsigned int body_id, + const Vector3d &point_position, + double *G, + bool update_kinematics) + + cdef void CalcBodySpatialJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + double *G, + bool update_kinematics) + + cdef void InverseDynamicsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + const double* qddot_ptr, + double* tau_ptr, + vector[SpatialVector] *f_ext + ) + + cdef void NonlinearEffectsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + double* tau_ptr + ) + + cdef void CompositeRigidBodyAlgorithmPtr (Model& model, + const double *q, + double *H, + bool update_kinematics) + + cdef void ForwardDynamicsPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + double* tau_ptr, + const double* qddot_ptr, + vector[SpatialVector] *f_ext + ) + + cdef void ForwardDynamicsConstraintsDirectPtr ( + Model &model, + const double* q_ptr, + const double* qdot_ptr, + const double* tau_ptr, + ConstraintSet &CS, + double* qddot_ptr + ) + +cdef extern from "rbdl_loadmodel.cc": + cdef bool rbdl_loadmodel ( + const char* filename, + Model* model, + bool floating_base, + bool verbose) diff --git a/3rdparty/rbdl/python/rbdl-wrapper.pyx b/3rdparty/rbdl/python/rbdl-wrapper.pyx new file mode 100644 index 0000000..3cf3fbd --- /dev/null +++ b/3rdparty/rbdl/python/rbdl-wrapper.pyx @@ -0,0 +1,1653 @@ +#cython: c_string_type=unicode, c_string_encoding=default, boundscheck=False, embedsignature=True + +import numpy as np +cimport numpy as np +from libc.stdint cimport uintptr_t +from libcpp.string cimport string + +cimport crbdl + +############################## +# +# Linear Algebra Types +# +############################## + +cdef class Vector3d: + cdef crbdl.Vector3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Vector3d() + + if pyvalues is not None: + for i in range (3): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Vector3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Vector3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Vector3d (0, python_values) + +cdef class Matrix3d: + cdef crbdl.Matrix3d *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Matrix3d() + + if pyvalues is not None: + for i in range (3): + for j in range (3): + (&(self.thisptr.coeff(i,j)))[0] = pyvalues[i,j] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Matrix3d [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 3 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Matrix3d (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Matrix3d (0, python_values) + + +cdef class VectorNd: + cdef crbdl.VectorNd *thisptr + cdef free_on_dealloc + + def __cinit__(self, ndim, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.VectorNd(ndim) + + if pyvalues is not None: + for i in range (ndim): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return self.thisptr.rows() + + def toNumpy (self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPythonArray (cls, python_values): + return VectorNd (len(python_values), 0, python_values) + + @classmethod + def fromPointer(cls, uintptr_t address): + cdef crbdl.VectorNd* vector_ptr = address + return VectorNd (vector_ptr.rows(), address) + +cdef class Quaternion: + cdef crbdl.Quaternion *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None, pymatvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.Quaternion() + + if pyvalues is not None: + for i in range (4): + self.thisptr.data()[i] = pyvalues[i] + elif pymatvalues is not None: + mat = Matrix3d() + for i in range (3): + for j in range (3): + (&(mat.thisptr.coeff(i,j)))[0] = pymatvalues[i,j] + self.thisptr[0] = crbdl.fromMatrix (mat.thisptr[0]) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "Quaternion [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], + self.thisptr.data()[2], self.thisptr.data()[3]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 4 + + def toMatrix(self): + cdef crbdl.Matrix3d mat + mat = self.thisptr.toMatrix() + result = np.array ([3,3]) + for i in range (3): + for j in range (3): + result[i,j] = mat.coeff(i,j) + + return result + + def toNumpy(self): + result = np.ndarray (self.thisptr.rows()) + for i in range (0, self.thisptr.rows()): + result[i] = self.thisptr[0][i] + return result + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Quaternion (address) + + @classmethod + def fromPythonArray (cls, python_values): + return Quaternion (0, python_values) + + @classmethod + def fromPythonMatrix (cls, python_matrix_values): + return Quaternion (0, None, python_matrix_values) + +cdef class SpatialVector: + cdef crbdl.SpatialVector *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0, pyvalues=None): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialVector() + + if pyvalues is not None: + for i in range (6): + self.thisptr.data()[i] = pyvalues[i] + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialVector [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))] + else: + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialVector (address) + + @classmethod + def fromPythonArray (cls, python_values): + return SpatialVector (0, python_values) + +cdef class SpatialMatrix: + cdef crbdl.SpatialMatrix *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialMatrix() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialMatrix [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2], + self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5]) + + def __getitem__(self, key): + return self.thisptr.data()[key] + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + self.thisptr.data()[i] = value[src_index] + src_index = src_index + 1 + else: + self.thisptr.data()[key] = value + + def __len__ (self): + return 6 + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialMatrix (address) + +############################## +# +# Conversion Numpy <-> Eigen +# +############################## + +# Vector3d +cdef crbdl.Vector3d NumpyToVector3d (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Vector3d cx = crbdl.Vector3d() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray Vector3dToNumpy (crbdl.Vector3d cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +cdef np.ndarray Matrix3dToNumpy (crbdl.Matrix3d cM): + result = np.ndarray ([3, 3]) + for i in range (3): + for j in range (3): + result[i,j] = cM.coeff(i,j) + + return result + +# VectorNd +cdef crbdl.VectorNd NumpyToVectorNd (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.VectorNd cx = crbdl.VectorNd(x.shape[0]) + for i in range (x.shape[0]): + cx[i] = x[i] + + return cx + +cdef np.ndarray VectorNdToNumpy (crbdl.VectorNd cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +# MatrixNd +cdef crbdl.MatrixNd NumpyToMatrixNd (np.ndarray[double, ndim=2, mode="c"] M): + cdef crbdl.MatrixNd cM = crbdl.MatrixNd(M.shape[0], M.shape[1]) + for i in range (M.shape[0]): + for j in range (M.shape[1]): + (&(cM.coeff(i,j)))[0] = M[i,j] + + return cM + +cdef np.ndarray MatrixNdToNumpy (crbdl.MatrixNd cM): + result = np.ndarray ([cM.rows(), cM.cols()]) + for i in range (cM.rows()): + for j in range (cM.cols()): + result[i,j] = cM.coeff(i,j) + + return result + +# SpatialVector +cdef np.ndarray SpatialVectorToNumpy (crbdl.SpatialVector cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +cdef crbdl.Quaternion NumpyToQuaternion (np.ndarray[double, ndim=1, mode="c"] x): + cdef crbdl.Quaternion cx = crbdl.Quaternion() + for i in range (3): + cx[i] = x[i] + + return cx + +cdef np.ndarray QuaternionToNumpy (crbdl.Quaternion cx): + result = np.ndarray ((cx.rows())) + for i in range (cx.rows()): + result[i] = cx[i] + + return result + +############################## +# +# Spatial Algebra Types +# +############################## + +cdef class SpatialTransform: + cdef crbdl.SpatialTransform *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialTransform() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "SpatialTransform E = [ [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}] ], r = [{:3.4f}, {:3.4f}, {:3.4f}]".format ( + self.thisptr.E.coeff(0,0), self.thisptr.E.coeff(0,1), self.thisptr.E.coeff(0,2), + self.thisptr.E.coeff(1,0), self.thisptr.E.coeff(1,1), self.thisptr.E.coeff(1,2), + self.thisptr.E.coeff(2,0), self.thisptr.E.coeff(2,1), self.thisptr.E.coeff(2,2), + self.thisptr.r[0], self.thisptr.r[1], self.thisptr.r[2]) + + property E: + """ Rotational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.E.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.E.coeff(i,j)))[0] = value[i,j] + + property r: + """ Translational part of the SpatialTransform. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.r[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.r[i]))[0] = value[i] + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialTransform (address) + +cdef class SpatialRigidBodyInertia: + cdef crbdl.SpatialRigidBodyInertia *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.SpatialRigidBodyInertia() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.SpatialRigidBodyInertia (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return SpatialRigidBodyInertia (address) + + property m: + def __get__ (self): + return self.thisptr.m + + def __set__ (self, value): + self.thisptr.m = value + + property h: + """ Translational part of the SpatialRigidBodyInertia. """ + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.h[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.h[i]))[0] = value[i] + + property Ixx: + def __get__ (self): + return self.thisptr.Ixx + + def __set__ (self, value): + self.thisptr.Ixx = value + + property Iyx: + def __get__ (self): + return self.thisptr.Iyx + + def __set__ (self, value): + self.thisptr.Iyx = value + + property Iyy: + def __get__ (self): + return self.thisptr.Iyy + + def __set__ (self, value): + self.thisptr.Iyy = value + + property Izx: + def __get__ (self): + return self.thisptr.Izx + + def __set__ (self, value): + self.thisptr.Izx = value + + property Izy: + def __get__ (self): + return self.thisptr.Izy + + def __set__ (self, value): + self.thisptr.Izy = value + + property Izz: + def __get__ (self): + return self.thisptr.Izz + + def __set__ (self, value): + self.thisptr.Izz = value + +############################## +# +# Rigid Multibody Types +# +############################## + +cdef class Body: + cdef crbdl.Body *thisptr + cdef free_on_dealloc + + def __cinit__(self, **kwargs): + cdef double c_mass + cdef crbdl.Vector3d c_com + cdef crbdl.Matrix3d c_inertia + cdef uintptr_t address=0 + + if "address" in kwargs.keys(): + address=kwargs["address"] + mass = None + if "mass" in kwargs.keys(): + mass=kwargs["mass"] + com = None + if "com" in kwargs.keys(): + com=kwargs["com"] + inertia = None + if "inertia" in kwargs.keys(): + inertia=kwargs["inertia"] + + if address == 0: + self.free_on_dealloc = True + if (mass is not None) and (com is not None) and (inertia is not None): + c_mass = mass + + for i in range (3): + c_com[i] = com[i] + + for i in range (3): + for j in range (3): + (&(c_inertia.coeff(i,j)))[0] = inertia[i,j] + + self.thisptr = new crbdl.Body(c_mass, c_com, c_inertia) + else: + self.thisptr = new crbdl.Body() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.Body (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Body (address=address) + + @classmethod + def fromMassComInertia(cls, double mass, + np.ndarray[double, ndim=1] com, + np.ndarray[double, ndim=2] inertia): + + return Body (address=0, mass=mass, com=com, inertia=inertia) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + + property mIsVirtual: + def __get__ (self): + return self.thisptr.mIsVirtual + + def __set__ (self, value): + self.thisptr.mIsVirtual = value + +cdef class FixedBody: + cdef crbdl.FixedBody *thisptr + cdef free_on_dealloc + + def __cinit__(self, uintptr_t address=0): + if address == 0: + self.free_on_dealloc = True + self.thisptr = new crbdl.FixedBody() + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + return "rbdl.FixedBody (0x{:0x})".format( self.thisptr) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return FixedBody (address) + + # Properties + property mMass: + def __get__ (self): + return self.thisptr.mMass + + def __set__ (self, value): + self.thisptr.mMass = value + + property mCenterOfMass: + def __get__ (self): + result = np.ndarray ((3)) + for i in range (3): + result[i] = self.thisptr.mCenterOfMass[i] + + return result + + def __set__ (self, value): + for i in range (3): + (&(self.thisptr.mCenterOfMass[i]))[0] = value[i] + + property mInertia: + def __get__ (self): + result = np.ndarray ((3,3)) + for i in range (3): + for j in range (3): + result[i,j] = self.thisptr.mInertia.coeff(i,j) + + return result + + def __set__ (self, value): + for i in range (3): + for j in range (3): + (&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j] + +cdef enum JointType: + JointTypeUndefined = 0 + JointTypeRevolute + JointTypePrismatic + JointTypeRevoluteX + JointTypeRevoluteY + JointTypeRevoluteZ + JointTypeSpherical + JointTypeEulerZYX + JointTypeEulerXYZ + JointTypeEulerYXZ + JointTypeTranslationXYZ + JointTypeFloatingBase + JointTypeFixed + JointTypeHelical + JointType1DoF + JointType2DoF + JointType3DoF + JointType4DoF + JointType5DoF + JointType6DoF + JointTypeCustom + +cdef class Joint: + cdef crbdl.Joint *thisptr + cdef free_on_dealloc + + joint_type_map = { + JointTypeUndefined: "JointTypeUndefined", + JointTypeRevolute: "JointTypeRevolute", + JointTypePrismatic: "JointTypePrismatic", + JointTypeRevoluteX: "JointTypeRevoluteX", + JointTypeRevoluteY: "JointTypeRevoluteY", + JointTypeRevoluteZ: "JointTypeRevoluteZ", + JointTypeSpherical: "JointTypeSpherical", + JointTypeEulerZYX: "JointTypeEulerZYX", + JointTypeEulerXYZ: "JointTypeEulerXYZ", + JointTypeEulerYXZ: "JointTypeEulerYXZ", + JointTypeTranslationXYZ: "JointTypeTranslationXYZ", + JointTypeFloatingBase: "JointTypeFloatingBase", + JointTypeFixed: "JointTypeFixed", + JointTypeHelical: "JointTypeHelical", + JointType1DoF: "JointType1DoF", + JointType2DoF: "JointType2DoF", + JointType3DoF: "JointType3DoF", + JointType4DoF: "JointType4DoF", + JointType5DoF: "JointType5DoF", + JointType6DoF: "JointType6DoF", + JointTypeCustom: "JointTypeCustom", + } + + def _joint_type_from_str (self, joint_type_str): + if joint_type_str not in self.joint_type_map.values(): + raise ValueError("Invalid JointType '" + str(joint_type_str) + "'!") + else: + for joint_type, joint_str in self.joint_type_map.iteritems(): + if joint_str == joint_type_str: + return joint_type + + def __cinit__(self, uintptr_t address=0, joint_type=-1): + if address == 0: + self.free_on_dealloc = True + if joint_type == -1: + self.thisptr = new crbdl.Joint() + else: + self.thisptr = new crbdl.Joint(self._joint_type_from_str(joint_type)) + else: + self.free_on_dealloc = False + self.thisptr = address + + def __dealloc__(self): + if self.free_on_dealloc: + del self.thisptr + + def __repr__(self): + joint_type_str = "JointTypeUndefined" + + if self.thisptr.mJointType in self.joint_type_map.keys(): + joint_type_str = self.joint_type_map[self.thisptr.mJointType] + + return "rbdl.Joint (0x{:0x}), JointType: {:s}".format( self.thisptr, joint_type_str) + + # Constructors + @classmethod + def fromPointer(cls, uintptr_t address): + return Joint (address) + + @classmethod + def fromJointType(cls, joint_type): + return Joint (0, joint_type) + + @classmethod + def fromJointAxes(cls, axes): + assert (len(axes) > 0) + assert (len(axes[0]) == 6) + axes_count = len(axes) + joint_type = JointType1DoF + axes_count - 1 + + result = Joint (0, cls.joint_type_map[joint_type]) + + for i in range (axes_count): + result.setJointAxis(i, axes[i]) + + return result + + property mDoFCount: + def __get__ (self): + return self.thisptr.mDoFCount + + def __set__ (self, value): + self.thisptr.mDoFCount = value + + property mJointType: + def __get__ (self): + return self.joint_type_map[self.thisptr.mJointType] + + property q_index: + def __get__ (self): + return self.thisptr.q_index + + def getJointAxis (self, index): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + return SpatialVectorToNumpy (self.thisptr.mJointAxes[index]) + + def setJointAxis (self, index, value): + assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!" + for i in range (6): + (&(self.thisptr.mJointAxes[index][i]))[0] = value[i] + self.thisptr.mJointAxes[index][i] + +cdef class Model + +%VectorWrapperClassDefinitions(PARENT=Model)% + +cdef class Model: + cdef crbdl.Model *thisptr + %VectorWrapperMemberDefinitions (PARENT=Model)% + + def __cinit__(self): + self.thisptr = new crbdl.Model() + %VectorWrapperCInitCode (PARENT=Model)% + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.Model (0x{:0x})".format( self.thisptr) + + def AddBody (self, + parent_id, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = b""): + return self.thisptr.AddBody ( + parent_id, + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def AppendBody (self, + SpatialTransform joint_frame not None, + Joint joint not None, + Body body not None, + string body_name = b""): + return self.thisptr.AppendBody ( + joint_frame.thisptr[0], + joint.thisptr[0], + body.thisptr[0], + body_name + ) + + def SetQuaternion (self, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] quat, + np.ndarray[double, ndim=1, mode="c"] q): + quat_wrap = Quaternion.fromPythonArray (quat) + q_wrap = VectorNd.fromPythonArray (q) + self.thisptr.SetQuaternion (body_id, + (quat_wrap).thisptr[0], + (q_wrap).thisptr[0]) + for i in range(len(q)): + q[i] = q_wrap[i] + + def GetQuaternion (self, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] q): + return QuaternionToNumpy (self.thisptr.GetQuaternion(body_id, NumpyToVectorNd (q))) + + def GetBody (self, index): + return Body (address= &(self.thisptr.mBodies[index])) + + def GetParentBodyId (self, index): + return self.thisptr.GetParentBodyId(index) + + def GetBodyId (self, name): + return self.thisptr.GetBodyId(name) + + def GetBodyName (self, index): + return self.thisptr.GetBodyName(index) + + def IsBodyId (self, index): + return self.thisptr.IsBodyId(index) + + def IsFixedBodyId (self, index): + return self.thisptr.IsFixedBodyId(index) + + property dof_count: + def __get__ (self): + return self.thisptr.dof_count + + property q_size: + def __get__ (self): + return self.thisptr.q_size + + property qdot_size: + def __get__ (self): + return self.thisptr.qdot_size + + property previously_added_body_id: + def __get__ (self): + return self.thisptr.previously_added_body_id + + property gravity: + def __get__ (self): + return np.array ([ + self.thisptr.gravity[0], + self.thisptr.gravity[1], + self.thisptr.gravity[2] + ] + ) + def __set__ (self, values): + for i in range (0,3): + self.thisptr.gravity[i] = values[i] + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=a, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=Joint, MEMBER=mJoints, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=S, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v_J, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c_J, PARENT=Model)% + + property mJointUpdateOrder: + def __get__ (self): + return self.thisptr.mJointUpdateOrder + + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_T, PARENT=Model)% + + property mFixedJointCount: + def __get__ (self): + return self.thisptr.mFixedJointCount + + # TODO + # multdof3_S + # multdof3_U + # multdof3_Dinv + # multdof3_u + + property multdof3_w_index: + def __get__ (self): + return self.thisptr.multdof3_w_index + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialMatrix, MEMBER=IA, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=pA, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=U, PARENT=Model)% + + # TODO + # d + # u + + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=f, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=I, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=Ic, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=hc, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_lambda, PARENT=Model)% + %VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_base, PARENT=Model)% + + %VectorWrapperAddProperty (TYPE=FixedBody, MEMBER=mFixedBodies, PARENT=Model)% + + property fixed_body_discriminator: + def __get__ (self): + return self.thisptr.fixed_body_discriminator + + %VectorWrapperAddProperty (TYPE=Body, MEMBER=mBodies, PARENT=Model)% + +############################## +# +# Constraint Types +# +############################## + +cdef class ConstraintSet + +%VectorWrapperClassDefinitions(PARENT=ConstraintSet)% + +cdef class ConstraintSet: + cdef crbdl.ConstraintSet *thisptr + %VectorWrapperMemberDefinitions (PARENT=ConstraintSet)% + + def __cinit__(self): + self.thisptr = new crbdl.ConstraintSet() + %VectorWrapperCInitCode (PARENT=ConstraintSet)% + + def __dealloc__(self): + del self.thisptr + + def __repr__(self): + return "rbdl.ConstraintSet (0x{:0x})".format( self.thisptr) + + def AddContactConstraint (self, + body_id not None, + body_point not None, + world_normal not None, + constraint_name = None, + normal_acceleration = 0.): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Vector3d c_world_normal + cdef char* constraint_name_ptr + + for i in range (3): + c_body_point[i] = body_point[i] + c_world_normal[i] = world_normal[i] + + if constraint_name is None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddContactConstraint ( + body_id, + c_body_point, + c_world_normal, + constraint_name_ptr, + normal_acceleration + ) + + def AddLoopConstraint (self, + id_predecessor not None, + id_successor not None, + SpatialTransform X_predecessor not None, + SpatialTransform X_successor not None, + SpatialVector axis not None, + enable_stabilization = False, + stabilization_param = 0.1, + constraint_name = None): + cdef char* constraint_name_ptr + + if constraint_name is None: + constraint_name_ptr = NULL + else: + constraint_name_ptr = constraint_name + + return self.thisptr.AddLoopConstraint ( + id_predecessor, + id_successor, + X_predecessor.thisptr[0], + X_successor.thisptr[0], + axis.thisptr[0], + enable_stabilization, + stabilization_param, + constraint_name_ptr) + + def Bind (self, model): + return self.thisptr.Bind ((model).thisptr[0]) + + def size (self): + return self.thisptr.size() + + def clear (self): + self.thisptr.clear() + + property bound: + def __get__ (self): + return self.thisptr.bound + +# %VectorWrapperAddProperty (TYPE=string, MEMBER=name, PARENT=ConstraintSet)% + + %VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=point, PARENT=ConstraintSet)% + %VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=normal, PARENT=ConstraintSet)% + + property force: + def __get__ (self): + return VectorNdToNumpy(self.thisptr.force) + +# property acceleration: +# def __get__(self): +# return VectorNd.fromPointer ( &(self.thisptr.acceleration)).toNumpy() +# def __set__(self, values): +# vec = VectorNd.fromPythonArray (values) +# self.thisptr.acceleration = (vec.thisptr[0]) + +cdef class InverseKinematicsConstraintSet: + cdef crbdl.InverseKinematicsConstraintSet *thisptr + # cdef _ConstraintSet_point_Vector3d_VectorWrapper point + # cdef _ConstraintSet_normal_Vector3d_VectorWrapper normal + + def __cinit__(self): + self.thisptr = new crbdl.InverseKinematicsConstraintSet() + # self.body_points = _ConstraintSet_body_points_Vector3d_VectorWrapper ( self.thisptr) + # self.target_positions = _ConstraintSet_target_positions_Vector3d_VectorWrapper ( self.thisptr) + # self.target_orientations = _ConstraintSet_target_orientations_Vector3d_VectorWrapper ( self.thisptr) + + def __dealloc__(self): + del self.thisptr + + # def __repr__(self): + # return "rbdl.ConstraintSet (0x{:0x})".format( self.thisptr) + + def AddPointConstraint (self, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point, + np.ndarray[double, ndim=1, mode="c"] target_pos + ): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Vector3d c_target_pos + + for i in range (3): + c_body_point[i] = body_point[i] + c_target_pos[i] = target_pos[i] + + return self.thisptr.AddPointConstraint ( + body_id, + c_body_point, + c_target_pos + ) + + def AddOrientationConstraint (self, + unsigned int body_id, + np.ndarray[double, ndim=2, mode="c"] target_orientation + ): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Matrix3d c_target_orientation + + for i in range (3): + for j in range (3): + (&(c_target_orientation.coeff(i,j)))[0] = target_orientation[i, j] + + return self.thisptr.AddOrientationConstraint ( + body_id, + c_target_orientation + ) + + def AddFullConstraint (self, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point, + np.ndarray[double, ndim=1, mode="c"] target_pos, + np.ndarray[double, ndim=2, mode="c"] target_orientation + ): + cdef crbdl.Vector3d c_body_point + cdef crbdl.Vector3d c_target_pos + cdef crbdl.Matrix3d c_target_orientation + + for i in range (3): + c_body_point[i] = body_point[i] + c_target_pos[i] = target_pos[i] + for j in range (3): + (&(c_target_orientation.coeff(i,j)))[0] = target_orientation[i, j] + + return self.thisptr.AddFullConstraint ( + body_id, + c_body_point, + c_target_pos, + c_target_orientation + ) + + property e: + def __get__ (self): + return VectorNd.fromPointer ( &(self.thisptr.e)).toNumpy() + + # property damper: + # def __get__ (self): + # return self.thisptr.damper + + # def __set__ (self, value): + # self.thisptr.damper = value + + + + +def InverseKinematics ( + Model model, + np.ndarray[double, ndim=1, mode="c"] Qinit, + InverseKinematicsConstraintSet CS +): + """Compute solution for inverse kinematics for given constraint set.""" + cdef crbdl.VectorNd Qres + Qres.resize(model.dof_count) + res = np.zeros(model.dof_count) + + # copy over data + # FIXME use memory views + for i in range(model.dof_count): + Qres[i] = 0.0 + + ret = crbdl.InverseKinematics ( + model.thisptr[0], + NumpyToVectorNd (Qinit), + CS.thisptr[0], + Qres + ) + + # copy over data + # FIXME use memory views + for i in range(model.dof_count): + res[i] = Qres[i] + + return res +############################## +# +# Kinematics.h +# +############################## + +def UpdateKinematics( + Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot +): + crbdl.UpdateKinematics( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot) + ) + +def CalcBodyToBaseCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBodyToBaseCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcBaseToBodyCoordinates (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcBaseToBodyCoordinates ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcBodyWorldOrientation (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + update_kinematics=True): + return Matrix3dToNumpy (crbdl.CalcBodyWorldOrientation ( + model.thisptr[0], + NumpyToVectorNd (q), + body_id, + update_kinematics + )) + +def CalcPointVelocity (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointVelocity ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return Vector3dToNumpy (crbdl.CalcPointAcceleration ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointVelocity6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointVelocity6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointAcceleration6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + update_kinematics=True): + return SpatialVectorToNumpy (crbdl.CalcPointAcceleration6D ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + body_id, + NumpyToVector3d (body_point_position), + update_kinematics + )) + +def CalcPointJacobian (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobianPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcPointJacobian6D (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcPointJacobian6DPtr ( + model.thisptr[0], + q.data, + body_id, + NumpyToVector3d (body_point_position), + G.data, + update_kinematics + ) + +def CalcBodySpatialJacobian(Model model, + np.ndarray[double, ndim=1, mode="c"] q, + unsigned int body_id, + np.ndarray[double, ndim=1, mode="c"] body_point_position, + np.ndarray[double, ndim=2, mode="c"] G, + update_kinematics=True): + crbdl.CalcBodySpatialJacobianPtr( + model.thisptr[0], + q.data, + body_id, + G.data, + update_kinematics + ) + +############################## +# +# rbdl_utils.h +# +############################## + +def CalcCenterOfMass (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + np.ndarray[double, ndim=1, mode="c"] com, + np.ndarray[double, ndim=1, mode="c"] com_velocity=None, + np.ndarray[double, ndim=1, mode="c"] com_acceleration=None, + np.ndarray[double, ndim=1, mode="c"] angular_momentum=None, + np.ndarray[double, ndim=1, mode="c"] change_of_angular_momentum=None, + update_kinematics=True): + cdef double cmass + cdef crbdl.Vector3d c_com = crbdl.Vector3d() + cdef crbdl.Vector3d* c_com_vel_ptr # = crbdl.Vector3d() + cdef crbdl.Vector3d* c_com_accel_ptr # = crbdl.Vector3d() + cdef crbdl.Vector3d* c_ang_momentum_ptr # = crbdl.Vector3d() + cdef crbdl.Vector3d* c_change_of_ang_momentum_ptr # = crbdl.Vector3d() + + c_com_vel_ptr = NULL + c_com_accel_ptr = NULL + c_ang_momentum_ptr = NULL + c_change_of_ang_momentum_ptr = NULL + + if com_velocity is not None: + c_com_vel_ptr = new crbdl.Vector3d() + + if com_acceleration is not None: + c_com_accel_ptr = new crbdl.Vector3d() + + if angular_momentum is not None: + c_ang_momentum_ptr = new crbdl.Vector3d() + + if change_of_angular_momentum is not None: + c_change_of_ang_momentum_ptr = new crbdl.Vector3d() + + cdef crbdl.VectorNd qddot_vectornd + if qddot is not None: + qddot_vectornd = NumpyToVectorNd(qddot) + + cmass = 0.0 + crbdl.CalcCenterOfMass ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NULL if qddot is None else &qddot_vectornd, + cmass, + c_com, + c_com_vel_ptr, + c_com_accel_ptr, + c_ang_momentum_ptr, + c_change_of_ang_momentum_ptr, + update_kinematics) + + assert com is not None, "Parameter com for call to CalcCenterOfMass() is not provided (value is 'None')." + + com[0] = c_com[0] + com[1] = c_com[1] + com[2] = c_com[2] + + if com_velocity is not None: + com_velocity[0] = c_com_vel_ptr.data()[0] + com_velocity[1] = c_com_vel_ptr.data()[1] + com_velocity[2] = c_com_vel_ptr.data()[2] + del c_com_vel_ptr + + if com_acceleration is not None: + com_acceleration[0] = c_com_accel_ptr.data()[0] + com_acceleration[1] = c_com_accel_ptr.data()[1] + com_acceleration[2] = c_com_accel_ptr.data()[2] + del c_com_accel_ptr + + if angular_momentum is not None: + angular_momentum[0] = c_ang_momentum_ptr.data()[0] + angular_momentum[1] = c_ang_momentum_ptr.data()[1] + angular_momentum[2] = c_ang_momentum_ptr.data()[2] + del c_ang_momentum_ptr + + if change_of_angular_momentum is not None: + change_of_angular_momentum[0] = c_change_of_ang_momentum_ptr.data()[0] + change_of_angular_momentum[1] = c_change_of_ang_momentum_ptr.data()[1] + change_of_angular_momentum[2] = c_change_of_ang_momentum_ptr.data()[2] + del c_change_of_ang_momentum_ptr + + return cmass + +def CalcZeroMomentPoint (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + np.ndarray[double, ndim=1, mode="c"] zmp, + np.ndarray[double, ndim=1, mode="c"] normal=None, + np.ndarray[double, ndim=1, mode="c"] point=None, + update_kinematics=True): + + cdef crbdl.Vector3d c_normal = crbdl.Vector3d() + cdef crbdl.Vector3d c_point = crbdl.Vector3d() + + cdef crbdl.Vector3d* c_zmp_ptr# = crbdl.Vector3d() + c_zmp_ptr = new crbdl.Vector3d() + + if normal is not None: + c_normal[0] = normal[0] + c_normal[1] = normal[1] + c_normal[2] = normal[2] + else: + c_normal[0] = 0 + c_normal[1] = 0 + c_normal[2] = 1 + + if point is not None: + c_point[0] = point[0] + c_point[1] = point[1] + c_point[2] = point[2] + + crbdl.CalcZeroMomentPoint ( + model.thisptr[0], + NumpyToVectorNd (q), + NumpyToVectorNd (qdot), + NumpyToVectorNd (qddot), + c_zmp_ptr, + c_normal, + c_point, + update_kinematics) + + zmp[0] = c_zmp_ptr.data()[0] + zmp[1] = c_zmp_ptr.data()[1] + zmp[2] = c_zmp_ptr.data()[2] + + return zmp +############################## +# +# Dynamics.h +# +############################## + +def InverseDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] qddot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.InverseDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + qddot.data, + tau.data, + NULL + ) + +def NonlinearEffects (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau): + crbdl.NonlinearEffectsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data + ) + +def CompositeRigidBodyAlgorithm (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=2, mode="c"] H, + update_kinematics=True): + crbdl.CompositeRigidBodyAlgorithmPtr (model.thisptr[0], + q.data, + H.data, + update_kinematics); + +def ForwardDynamics (Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau, + np.ndarray[double, ndim=1, mode="c"] qddot): + crbdl.ForwardDynamicsPtr (model.thisptr[0], + q.data, + qdot.data, + tau.data, + qddot.data, + NULL + ) +############################## +# +# Constraints.h +# +############################## + +def ForwardDynamicsConstraintsDirect ( + Model model, + np.ndarray[double, ndim=1, mode="c"] q, + np.ndarray[double, ndim=1, mode="c"] qdot, + np.ndarray[double, ndim=1, mode="c"] tau, + ConstraintSet CS, + np.ndarray[double, ndim=1, mode="c"] qddot): + crbdl.ForwardDynamicsConstraintsDirectPtr ( + model.thisptr[0], + q.data, + qdot.data, + tau.data, + CS.thisptr[0], + qddot.data + ) + +############################## +# +# Utilities +# +############################## + +def loadModel ( + filename, + **kwargs + ): + verbose = False + if "verbose" in kwargs.keys(): + verbose=kwargs["verbose"] + + floating_base = False + if "floating_base" in kwargs.keys(): + floating_base=kwargs["floating_base"] + + result = Model() + if crbdl.rbdl_loadmodel (filename, result.thisptr, floating_base, verbose): + return result + + print ("Error loading model {}!".format (filename)) + return None diff --git a/3rdparty/rbdl/python/rbdl_loadmodel.cc b/3rdparty/rbdl/python/rbdl_loadmodel.cc new file mode 100644 index 0000000..04f0941 --- /dev/null +++ b/3rdparty/rbdl/python/rbdl_loadmodel.cc @@ -0,0 +1,42 @@ +#include + +#ifdef RBDL_BUILD_ADDON_LUAMODEL +#include +#endif + +#ifdef RBDL_BUILD_ADDON_URDFREADER +#include +#endif + +#include +#include + +using namespace RigidBodyDynamics; +using namespace std; + +bool rbdl_loadmodel (const char* filename, Model* model, bool floating_base=false, bool verbose=false) { + string fname (filename); + + for (size_t i = 0; i < fname.size(); i++) { + fname[i] = tolower(fname[i]); + } + + bool result = false; + if (fname.substr (fname.size() - 4, 4) == ".lua") { +#ifdef RBDL_BUILD_ADDON_LUAMODEL + result = Addons::LuaModelReadFromFile (filename, model, verbose); +#else + cerr << "Error: RBDL Addon LuaModel not enabled!" << endl; +#endif + } else if (fname.substr (fname.size() - 5, 5) == ".urdf") { +#ifdef RBDL_BUILD_ADDON_URDFREADER + result = Addons::URDFReadFromFile (filename, model, floating_base, verbose); +#else + cerr << "Error: RBDL Addon URDFReader not enabled!" << endl; +#endif + } else { + cerr << "Error: Cannot identify model type from filename '" << filename << "'!" << endl; + } + + return result; +} diff --git a/3rdparty/rbdl/python/rbdl_ptr_functions.h b/3rdparty/rbdl/python/rbdl_ptr_functions.h new file mode 100644 index 0000000..ec833bd --- /dev/null +++ b/3rdparty/rbdl/python/rbdl_ptr_functions.h @@ -0,0 +1,674 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2015 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + * + * This file defines functions that allows calling of the RBDL algorithms + * by providing input and output as raw double arrays. It eliminates the + * need of copying from Numpy values into temporary RBDL (C++) vectors and + * matrices. However it requires C++11 and must be compiled with -std=c++11 + * (or -std=c++0x on older compilers). + */ + +#include +#include + +namespace RigidBodyDynamics { + +namespace Math { + +// PTR_DATA_ROW_MAJOR : +// Specifies whether the data that is provided via raw double pointers is +// stored as row major. Eigen uses column major by default and therefore +// this has to be properly mapped. +#define PTR_DATA_ROW_MAJOR 1 + +#ifdef RBDL_USE_SIMPLE_MATH + typedef VectorNd VectorNdRef; + typedef MatrixNd MatrixNdRef; +#else + typedef Eigen::Ref VectorNdRef; + +#ifdef PTR_DATA_ROW_MAJOR + typedef Eigen::Matrix MatrixNdRowMaj; + typedef Eigen::Ref MatrixNdRef; +#else + typedef Eigen::Ref MatrixNdRef; +#endif + +#endif + +RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) { +#ifdef RBDL_USE_SIMPLE_MATH + return SimpleMath::Map (ptr, n, 1); +#elif defined EIGEN_CORE_H + return Eigen::Map (ptr, n, 1); +#else + std::cerr << __func__ << " not defined for used math library!" << std::endl; + abort(); + return VectorNd::Constant (1,1./0.); +#endif +} + +RBDL_DLLAPI inline MatrixNdRef MatrixFromPtr (double *ptr, unsigned int rows, unsigned int cols, bool row_major = true) { +#ifdef RBDL_USE_SIMPLE_MATH + return SimpleMath::Map (ptr, rows, cols); +#elif defined EIGEN_CORE_H +#ifdef PTR_DATA_ROW_MAJOR + return Eigen::Map (ptr, rows, cols); +#else + return Eigen::Map (ptr, rows, cols); +#endif +#else + std::cerr << __func__ << " not defined for used math library!" << std::endl; + abort(); + return MatrixNd::Constant (1,1, 1./0.); +#endif +} + +} + +RBDL_DLLAPI +void UpdateKinematicsCustomPtr (Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *qddot_ptr + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + unsigned int i; + + if (q_ptr) { + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + VectorNd QDot_zero (VectorNd::Zero (model.qdot_size)); + + jcalc (model, i, (Q), QDot_zero); + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + } + } + + if (qdot_ptr) { + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.qdot_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) { + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } else { + model.v[i] = model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } + // LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl; + } + } + + if (qddot_ptr) { + VectorNdRef QDDot = VectorFromPtr(const_cast(qddot_ptr), model.qdot_size); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + } else { + model.a[i] = model.c[i]; + } + + if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp ((QDDot)[q_index], (QDDot)[q_index + 1], (QDDot)[q_index + 2]); + model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp; + } else { + model.a[i] = model.a[i] + model.S[i] * (QDDot)[q_index]; + } + } + } +} + +RBDL_DLLAPI +void CalcPointJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + const Math::Vector3d &point_position, + double * G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 3, model.qdot_size); + + SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false)); + + assert (G.rows() == 3 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + // e[j] is set to 1 if joint j contributes to the jacobian that we are + // computing. For all other joints the column will be zero. + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 3, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(3,0,3,3); + } else { + G.block(0,q_index, 3, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(3,0,3,1); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void CalcPointJacobian6DPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + const Math::Vector3d &point_position, + double *G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 6, model.qdot_size); + + SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false)); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 6, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(0,0,6,3); + } else { + G.block(0,q_index, 6, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(0,0,6,1); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void CalcBodySpatialJacobianPtr ( + Model &model, + const double *q_ptr, + unsigned int body_id, + double *G_ptr, + bool update_kinematics + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL); + } + + MatrixNdRef G = MatrixFromPtr(const_cast(G_ptr), 6, model.qdot_size); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + SpatialTransform base_to_body; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + base_to_body = model.mFixedBodies[fbody_id].mParentTransform * model.X_base[reference_body_id]; + } else { + base_to_body = model.X_base[reference_body_id]; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + G.block(0,q_index,6,3) = (base_to_body * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]; + } else { + G.block(0,q_index,6,1) = base_to_body.apply(model.X_base[j].inverse().apply(model.S[j])); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI +void InverseDynamicsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *qddot_ptr, + const double *tau_ptr, + std::vector *f_ext + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.qdot_size); + VectorNdRef QDDot = VectorFromPtr(const_cast(qddot_ptr), model.qdot_size); + VectorNdRef Tau = VectorFromPtr(const_cast(tau_ptr), model.qdot_size); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + + if (model.mJoints[i].mDoFCount == 3) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.multdof3_S[i] * Vector3d (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]); + } else { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.S[i] * QDDot[q_index]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) + model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i]; + } else { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI +void NonlinearEffectsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *tau_ptr + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef QDot = VectorFromPtr(const_cast(qdot_ptr), model.qdot_size); + VectorNdRef Tau = VectorFromPtr(const_cast(tau_ptr), model.qdot_size); + + SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0] = spatial_gravity; + + for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) { + jcalc (model, model.mJointUpdateOrder[i], Q, QDot); + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.lambda[i] == 0) { + model.v[i] = model.v_J[i]; + model.a[i] = model.X_lambda[i].apply(spatial_gravity); + } else { + model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i]; + } else { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI +inline void CompositeRigidBodyAlgorithmPtr ( + Model& model, + const double *q_ptr, + double *H_ptr, + bool update_kinematics = true + ) { + using namespace RigidBodyDynamics::Math; + + VectorNdRef&& Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + MatrixNdRef&& H = MatrixFromPtr(H_ptr, model.qdot_size, model.qdot_size); + + assert (H.rows() == model.dof_count && H.cols() == model.dof_count); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (update_kinematics) { + jcalc_X_lambda_S (model, i, Q); + } + model.Ic[i] = model.I[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.lambda[i] != 0) { + model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]); + } + + unsigned int dof_index_i = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3) { + Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i]; + H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]); + + H.block<3,3>(dof_index_i,dof_index_j) = H_temp2; + H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } else { + Vector3d H_temp2 = F_63.transpose() * (model.S[j]); + + H.block<3,1>(dof_index_i,dof_index_j) = H_temp2; + H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } + } + } else { + SpatialVector F = model.Ic[i] * model.S[i]; + H(dof_index_i, dof_index_i) = model.S[i].dot(F); + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F = model.X_lambda[j].applyTranspose(F); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if (model.mJoints[j].mDoFCount == 3) { + Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose(); + + LOG << F.transpose() << std::endl << model.multdof3_S[j] << std::endl; + LOG << H_temp2.transpose() << std::endl; + + H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose(); + H.block<3,1>(dof_index_j,dof_index_i) = H_temp2; + } else { + H(dof_index_i,dof_index_j) = F.dot(model.S[j]); + H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j); + } + } + } + } +} + +RBDL_DLLAPI +void ForwardDynamicsPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *tau_ptr, + const double *qddot_ptr, + std::vector *f_ext + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef&& Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef&& QDot = VectorFromPtr(const_cast(qdot_ptr), model.qdot_size); + VectorNdRef&& QDDot = VectorFromPtr(const_cast(qddot_ptr), model.qdot_size); + VectorNdRef&& Tau = VectorFromPtr(const_cast(tau_ptr), model.qdot_size); + + SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]); + + unsigned int i = 0; + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "QDot = " << QDot.transpose() << std::endl; + LOG << "Tau = " << Tau.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + else + model.X_base[i] = model.X_lambda[i]; + + model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i]; + + /* + LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl; + LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl; + LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl; + LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl; + LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl; + */ + + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.I[i].setSpatialMatrix (model.IA[i]); + + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + +// ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3) { + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); +#endif + Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]); + + model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i]; + +// LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; + } + } else { + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); +// LOG << "u[" << i << "] = " << model.u[i] << std::endl; + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; + } + } + } + +// ClearLogOutput(); + + model.a[0] = spatial_gravity * -1.; + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 3) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]); + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsDirectPtr ( + Model &model, + const double *q_ptr, + const double *qdot_ptr, + const double *tau_ptr, + ConstraintSet &CS, + double *qddot_ptr +) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + using namespace RigidBodyDynamics::Math; + + VectorNdRef&& Q = VectorFromPtr(const_cast(q_ptr), model.q_size); + VectorNdRef&& QDot = VectorFromPtr(const_cast(qdot_ptr), model.qdot_size); + VectorNdRef&& QDDot = VectorFromPtr(const_cast(qddot_ptr), model.qdot_size); + VectorNdRef&& Tau = VectorFromPtr(const_cast(tau_ptr), model.qdot_size); + + // create copy of non-const accelerations + VectorNd QDDot_dummy = QDDot; + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "QDot = " << QDot.transpose() << std::endl; + LOG << "Tau = " << Tau.transpose() << std::endl; + + LOG << "QDDot = " << QDDot.transpose() << std::endl; + LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl; + + // calling non-pointer version + ForwardDynamicsConstraintsDirect ( + model, Q, QDot, Tau, CS, QDDot_dummy + ); + + for (int i = 0; i < model.qdot_size; ++i) { + QDDot[i] = QDDot_dummy[i]; + } + LOG << "QDDot = " << QDDot.transpose() << std::endl; + LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl; + LOG << "---" << std::endl; +} +} diff --git a/3rdparty/rbdl/python/setup.py.cmake b/3rdparty/rbdl/python/setup.py.cmake new file mode 100755 index 0000000..2f2e505 --- /dev/null +++ b/3rdparty/rbdl/python/setup.py.cmake @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +from distutils.core import setup +from distutils.extension import Extension +from distutils.sysconfig import get_python_lib +from Cython.Distutils import build_ext +from Cython.Build import cythonize + +import os +import sys +import numpy as np + +if not os.path.exists('rbdl.so'): + print("""The setup.py script should be executed from the build directory.""") + sys.exit(1) + +lib_path = get_python_lib()[5:] + +setup( + name='rbdl', + author='Martin Felis', + author_email='martin@fysx.org', + description='Python wrapper for RBDL - the Rigid Body Dynamics Library', + license='zlib', + version='${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH}', + url='https://rbdl.github.io/', + cmdclass={'build_ext': build_ext}, + ext_modules=cythonize(ext_modules), + data_files = [(lib_path, ["rbdl.so"])], +) diff --git a/3rdparty/rbdl/python/test_wrapper.py b/3rdparty/rbdl/python/test_wrapper.py new file mode 100755 index 0000000..913fefc --- /dev/null +++ b/3rdparty/rbdl/python/test_wrapper.py @@ -0,0 +1,329 @@ +#!/usr/bin/python +# +# RBDL - Rigid Body Dynamics Library +# Copyright (c) 2011-2015 Martin Felis +# +# Licensed under the zlib license. See LICENSE for more details. + +import unittest + +import math +import numpy as np +from numpy.testing import * +import rbdl + +class JointTests (unittest.TestCase): + def test_JointConstructorAxesSimple(self): + axis = np.asarray([[1., 0., 0., 0., 0., 0.]]) + joint_rot_x = rbdl.Joint.fromJointAxes (axis) + + assert_equal (joint_rot_x.getJointAxis(0), axis[0]) + + def test_JointConstructorAxes6DoF(self): + axis = np.asarray([ + [1., 0., 0., 0., 0., 0.], + [0., 1., 0., 0., 0., 0.], + [0., 0., 1., 0., 0., 0.], + [0., 0., 0., 1., 0., 0.], + [0., 0., 0., 0., 1., 0.], + [0., 0., 0., 0., 0., 1.], + ]) + joint = rbdl.Joint.fromJointAxes (axis) + + for i in range (axis.shape[0]): + assert_equal (joint.getJointAxis(i), axis[i]) + +class SampleModel3R (unittest.TestCase): + """ Example planar triple pendulum + + - All joints along the positive Z axis in rest position + - All joints revolute y joints + - all "links" are 1 unit length + """ + def setUp(self): + self.model = rbdl.Model() + joint_rot_y = rbdl.Joint.fromJointType ("JointTypeRevoluteY") + self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.5]), np.eye(3) * + 0.05) + self.xtrans = rbdl.SpatialTransform() + self.xtrans.r = np.array([0., 0., 1.]) + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body) + self.body_2 = self.model.AppendBody (self.xtrans, joint_rot_y, self.body) + self.body_3 = self.model.AppendBody (self.xtrans, joint_rot_y, self.body) + + self.q = np.zeros (self.model.q_size) + self.qdot = np.zeros (self.model.qdot_size) + self.qddot = np.zeros (self.model.qdot_size) + self.tau = np.zeros (self.model.qdot_size) + + def test_CoordinateTransformBodyBase (self): + """ + Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates + give the right results. + """ + q = np.random.rand(self.model.q_size) + point_local = np.array ([1., 2., 3.]) + point_base = rbdl.CalcBodyToBaseCoordinates ( + self.model, + q, + self.body_3, + point_local) + point_local_2 = rbdl.CalcBaseToBodyCoordinates ( + self.model, + q, + self.body_3, + point_base) + + assert_almost_equal (point_local, point_local_2) + + def test_CalcPointVelocity (self): + """ + Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates + give the right results. + """ + q = np.zeros(self.model.q_size) + qdot = np.zeros(self.model.q_size) + qdot[0] = 1. + point_local = np.array ([0., 0., 0.]) + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_3, + point_local + ) + + assert_almost_equal (np.array([2., 0., 0.]), point_vel) + + def test_CalcCenterOfMass (self): + """ Tests calculation of center of mass + TODO: add checks for angular momentum + """ + + com = np.array ([-1., -1., -1.]) + com_vel = np.array([-2., -2., -2.]) + ang_mom = np.array([-3., -3., -3.]) + self.qdot[0] = 1. + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + None, + com, + None, + None + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + assert_almost_equal (np.array([0., 0., 1.5]), com) + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + None, + com, + com_vel, + None + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + assert_almost_equal (np.array([1.5, 0., 0.0]), com_vel) + + mass = rbdl.CalcCenterOfMass ( + self.model, + self.q, + self.qdot, + None, + com, + com_vel, + ang_mom + ) + self.assertEqual (3, mass) + assert_almost_equal (np.array([0., 0., 1.5]), com) + + def test_DynamicsConsistency (self): + """ Checks whether forward and inverse dynamics are consistent """ + q = np.random.rand (self.model.q_size) + qdot = np.random.rand (self.model.q_size) + qddot = np.random.rand (self.model.q_size) + + tau = np.random.rand (self.model.q_size) + + rbdl.ForwardDynamics ( + self.model, + q, + qdot, + tau, + qddot) + + tau_id = np.zeros ((self.model.q_size)) + rbdl.InverseDynamics ( + self.model, + q, + qdot, + qddot, + tau_id + ) + + assert_almost_equal (tau, tau_id) + + def test_NonlinearEffectsConsistency (self): + """ Checks whether NonlinearEffects is consistent with InverseDynamics """ + q = np.random.rand (self.model.q_size) + qdot = np.random.rand (self.model.q_size) + + nle_id = np.random.rand (self.model.q_size) + + rbdl.InverseDynamics( + self.model, + q, + qdot, + np.zeros (self.model.qdot_size), + nle_id) + + nle = np.zeros ((self.model.q_size)) + rbdl.NonlinearEffects ( + self.model, + q, + qdot, + nle + ) + + assert_almost_equal (nle_id, nle) + + def test_CalcPointJacobian (self): + """ Computes point Jacobian and checks whether G * qdot is consistent + with CalcPointVelocity. """ + q = np.zeros (self.model.q_size) + G = np.zeros ([3, self.model.q_size]) + point_coords = np.array ([0., 0., 1.]) + + rbdl.CalcPointJacobian ( + self.model, + q, + self.body_3, + point_coords, + G + ) + + qdot = np.ones(self.model.qdot_size) + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_3, + point_coords + ) + + jac_point_vel = np.dot (G, qdot) + assert_almost_equal (jac_point_vel, point_vel) + + def test_CalcPointJacobianNonSquare (self): + """ Computes point Jacobian and checks whether G * qdot is consistent + with CalcPointVelocity. """ + + self.model = rbdl.Model() + joint_trans_xyz = rbdl.Joint.fromJointType ("JointTypeTranslationXYZ") + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), + joint_trans_xyz, self.body) + + self.body_4 = self.model.AppendBody (rbdl.SpatialTransform(), + joint_trans_xyz, self.body) + + point_coords = np.array ([0., 0., 1.]) + q = np.zeros (self.model.q_size) + G = np.zeros ([3, self.model.q_size]) + + rbdl.CalcPointJacobian ( + self.model, + q, + self.body_4, + point_coords, + G + ) + + qdot = np.ones(self.model.qdot_size) + jac_point_vel = np.dot (G, qdot) + + point_vel = rbdl.CalcPointVelocity ( + self.model, + q, + qdot, + self.body_4, + point_coords + ) + + assert_almost_equal (jac_point_vel, point_vel) + +class FloatingBaseModel (unittest.TestCase): + """ Model with a floating base + """ + def setUp(self): + self.model = rbdl.Model() + joint_rot_y = rbdl.Joint.fromJointType ("JointTypeFloatingBase") + self.body = rbdl.Body.fromMassComInertia (1., np.array([0., 0.0, 0.5]), np.eye(3) * + 0.05) + self.xtrans = rbdl.SpatialTransform() + self.xtrans.r = np.array([0., 0., 0.]) + + self.body_1 = self.model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, self.body) + + self.q = np.zeros (self.model.q_size) + self.qdot = np.zeros (self.model.qdot_size) + self.qddot = np.zeros (self.model.qdot_size) + self.tau = np.zeros (self.model.qdot_size) + + def test_Dimensions (self): + """ + Checks whether the dimensions of q and qdot are correct + """ + + q = np.random.rand(self.model.q_size) + self.assertEqual (7, self.model.q_size) + self.assertEqual (6, self.model.qdot_size) + + def test_SetQuaternion (self): + mat = np.asarray ([[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]) + rbdl_quat = rbdl.Quaternion.fromPythonMatrix (mat) + ref_q = self.q.copy() + + self.model.SetQuaternion (2, rbdl_quat.toNumpy(), self.q) + + ref_q[3:6] = rbdl_quat[0:3] + ref_q[-1] = rbdl_quat[3] + + assert_array_equal (ref_q, self.q) + + def test_GetQuaternion (self): + mat = np.asarray ([[0., 1., 0.], [-1., 0., 0.], [0., 0., 1.]]) + rbdl_quat = rbdl.Quaternion.fromPythonMatrix (mat) + + self.assertEqual (4, len(rbdl_quat)) + self.q[5] = math.sqrt(2.) * 0.5 + self.q[6] = math.sqrt(2.) * 0.5 + + ref_quat = [0., 0., math.sqrt(2.) * 0.5, math.sqrt(2.) * 0.5] + quat = self.model.GetQuaternion (2, self.q) + + assert_array_equal (np.asarray(ref_quat), quat) + +class ConstraintSetTests (unittest.TestCase): + def test_Simple (self): + # only tests whether the API seems to work. No functional + # tests yet. + + cs = rbdl.ConstraintSet() + idx = cs.AddContactConstraint (1, [1., 2., 3.], [4., 5., 6.]) + assert_equal (0, idx) + + X = rbdl.SpatialTransform() + sv = rbdl.SpatialVector.fromPythonArray ([1., 2., 3., 4., 5., 6.]) + idx2 = cs.AddLoopConstraint (1, 2, X, X, sv, 1.) + assert_equal (1, idx2) + +if __name__ == '__main__': + unittest.main() diff --git a/3rdparty/rbdl/python/wrappergen.py b/3rdparty/rbdl/python/wrappergen.py new file mode 100755 index 0000000..6eb597c --- /dev/null +++ b/3rdparty/rbdl/python/wrappergen.py @@ -0,0 +1,137 @@ +#!/usr/bin/python + +import sys, re, os + +def usage(arg0): + print ("Usage: {} ".format(arg0)) + sys.exit(-1) + +wrapper_command_strings = { + "ClassDefinitions" : """cdef class _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper: + cdef crbdl.%PARENT% *parent + + def __cinit__ (self, uintptr_t ptr): + self.parent = ptr + + def __getitem__(self, key): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + return [%TYPE%.fromPointer ( &(self.parent.%MEMBER%[i])) for i in xrange (*key.indices(len(self)))] + else: + return %TYPE%.fromPointer ( &(self.parent.%MEMBER%[key])) + + def __setitem__(self, key, value): + if isinstance( key, slice ) : + #Get the start, stop, and step from the slice + src_index = 0 + for i in xrange (*key.indices(len(self))): + assert isinstance (value[src_index], %TYPE%), "Invalid type! Expected %TYPE%, but got " + str(type(value[src_index])) + "." + self.parent.%MEMBER%[i] = (<%TYPE%> value[src_index]).thisptr[0] + src_index = src_index + 1 + else: + assert isinstance (value, %TYPE%), "Invalid type! Expected %TYPE%, but got " + str(type(value)) + "." + self.parent.%MEMBER%[key] = (<%TYPE%> value).thisptr[0] + + def __len__(self): + return self.parent.%MEMBER%.size() +""", + + "MemberDefinitions" : """ cdef _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper %MEMBER%""", + + "CInitCode" : """ self.%MEMBER% = _%PARENT%_%MEMBER%_%TYPE%_VectorWrapper ( self.thisptr)""", + + "AddProperty" : """ property %MEMBER%: + def __get__ (self): + return self.%MEMBER% +""" +} + +def parse_line (line_str): + command = "" + args = {} + + # remove comments + line_str = line_str.split("#")[0] + + wrapper_line_str_match = re.search ("%VectorWrapper(\S*)\s*\((.*)\).*%", line_str) + if (wrapper_line_str_match): + command = wrapper_line_str_match.group(1) + arg_str = wrapper_line_str_match.group(2) + arg_match = re.findall("(\s*(\S*)\s*=\s*(\w*)\s*,?)", arg_str) + if len(arg_match) > 0: + for arg in arg_match: + if len(arg) != 3: + print ("Invalid command args at line_str {}".format + (line_number)) + sys.exit(-1) + + args[arg[1]] = arg[2] + + return command, args + + return False, None + +if __name__ == "__main__": + if len(sys.argv) != 3: + usage (sys.argv[0]) + + infilename = sys.argv[1] + outfilename = sys.argv[2] + + print ("Processing {} to generate {}".format (infilename, outfilename)) + infile = open(infilename) + outfile = open(outfilename, "w") + outfile.write ("""# WARNING! +# +# This file was automatically created from {} using {}. +# Do not modify this file directly. Edit original source instead!! + +""".format (os.path.basename(infilename), os.path.basename(sys.argv[0]))) + + template = infile.read() + template_lines = template.split ("\n") + + # find the classes that will contain generated code + generated_parent_classes = [] + generated_parent_members = {} + for line_number, line_str in enumerate (template_lines): + command, args = parse_line (line_str) + if command: + if args["PARENT"] not in generated_parent_classes: + generated_parent_classes.append (args["PARENT"]) + generated_parent_members[args["PARENT"]] = [] + + if command=="AddProperty": + generated_parent_members[args["PARENT"]].append ({ + "TYPE": args["TYPE"], + "MEMBER": args["MEMBER"] + }) + + # generate code + for line_number, line_str in enumerate (template_lines): + command, args = parse_line (line_str) + if not command: + outfile.write (line_str + "\n") + else: +# print (command, wrapper_command_strings.keys()) + if command in wrapper_command_strings.keys(): + parent = args["PARENT"] + if command == "AddProperty": + content_type = args["TYPE"] + member_name = args["MEMBER"] + command_code = wrapper_command_strings[command][:] + command_code = command_code.replace ( + "%PARENT%", parent).replace ( + "%MEMBER%", member_name).replace ( + "%TYPE%", content_type) + outfile.write (command_code + "\n") + else: + for member in generated_parent_members[parent]: + content_type = member["TYPE"] + member_name = member["MEMBER"] + command_code = wrapper_command_strings[command][:] + command_code = command_code.replace ( + "%PARENT%", parent).replace ( + "%MEMBER%", member_name).replace ( + "%TYPE%", content_type) + outfile.write (command_code + "\n") diff --git a/3rdparty/rbdl/rbdl.pc.cmake b/3rdparty/rbdl/rbdl.pc.cmake new file mode 100644 index 0000000..6646b02 --- /dev/null +++ b/3rdparty/rbdl/rbdl.pc.cmake @@ -0,0 +1,13 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=@CMAKE_INSTALL_FULL_LIBDIR@ +includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@ + +Name: RBDL +Description: Rigid Body Dynamics Library +URL: https://rbdl.github.io/ +Version: @RBDL_VERSION@ +Requires: eigen3 +Conflicts: +Libs: -L${libdir} -lrbdl -Wl,-rpath ${libdir} +Libs.private: +Cflags: -I${includedir} diff --git a/3rdparty/rbdl/src/Constraints.cc b/3rdparty/rbdl/src/Constraints.cc new file mode 100644 index 0000000..32d2237 --- /dev/null +++ b/3rdparty/rbdl/src/Constraints.cc @@ -0,0 +1,1651 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +void SolveLinearSystem ( + const MatrixNd& A, + const VectorNd& b, + VectorNd& x, + LinearSolver ls +); + +unsigned int GetMovableBodyId (Model& model, unsigned int id); + +unsigned int ConstraintSet::AddContactConstraint ( + unsigned int body_id, + const Vector3d &body_point, + const Vector3d &world_normal, + const char *constraint_name, + double normal_acceleration + ) { + assert (bound == false); + + unsigned int n_constr = size() + 1; + + std::string name_str; + if (constraint_name != NULL) { + name_str = constraint_name; + } + + constraintType.push_back (ContactConstraint); + name.push_back (name_str); + mContactConstraintIndices.push_back(size()); + + // These variables will be used for this type of constraint. + body.push_back (body_id); + point.push_back (body_point); + normal.push_back (world_normal); + + // These variables will not be used. + body_p.push_back (0); + body_s.push_back (0); + X_p.push_back (SpatialTransform()); + X_s.push_back (SpatialTransform()); + constraintAxis.push_back (SpatialVector::Zero()); + baumgarteParameters.push_back(Vector2d(0.0, 0.0)); + err.conservativeResize(n_constr); + err[n_constr - 1] = 0.; + errd.conservativeResize(n_constr); + errd[n_constr - 1] = 0.; + + acceleration.conservativeResize (n_constr); + acceleration[n_constr - 1] = normal_acceleration; + + force.conservativeResize (n_constr); + force[n_constr - 1] = 0.; + + impulse.conservativeResize (n_constr); + impulse[n_constr - 1] = 0.; + + v_plus.conservativeResize (n_constr); + v_plus[n_constr - 1] = 0.; + + d_multdof3_u = std::vector(n_constr, Math::Vector3d::Zero()); + + return n_constr - 1; +} + +unsigned int ConstraintSet::AddLoopConstraint ( + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + const Math::SpatialVector &axis, + bool enable_stabilization, + const double stabilization_param, + const char *constraint_name + ) { + assert (bound == false); + + unsigned int n_constr = size() + 1; + + std::string name_str; + if (constraint_name != NULL) { + name_str = constraint_name; + } + + constraintType.push_back(LoopConstraint); + name.push_back (name_str); + mLoopConstraintIndices.push_back(size()); + + // These variables will be used for this kind of constraint. + body_p.push_back (id_predecessor); + body_s.push_back (id_successor); + X_p.push_back (X_predecessor); + X_s.push_back (X_successor); + constraintAxis.push_back (axis); + + // Set up constraint stabilization + double baumgarte_coefficient = 0.0; + if (enable_stabilization) { + if (stabilization_param == 0.0) { + std::cerr << "Error: Baumgarte stabilization is enabled but the stabilization parameter is 0.0" << std::endl; + abort(); + } + baumgarte_coefficient = 1.0 / stabilization_param; + } + baumgarteParameters.push_back( + Vector2d(baumgarte_coefficient, baumgarte_coefficient)); + + err.conservativeResize(n_constr); + err[n_constr - 1] = 0.; + errd.conservativeResize(n_constr); + errd[n_constr - 1] = 0.; + + // These variables will not be used by loop constraints but are necessary + // for point constraints. + body.push_back (0); + point.push_back (Vector3d::Zero()); + normal.push_back (Vector3d::Zero()); + + acceleration.conservativeResize (n_constr); + acceleration[n_constr - 1] = 0.; + + force.conservativeResize (n_constr); + force[n_constr - 1] = 0.; + + impulse.conservativeResize (n_constr); + impulse[n_constr - 1] = 0.; + + v_plus.conservativeResize (n_constr); + v_plus[n_constr - 1] = 0.; + + d_multdof3_u = std::vector(n_constr, Math::Vector3d::Zero()); + + return n_constr - 1; +} + +unsigned int ConstraintSet::AddCustomConstraint( + CustomConstraint *customConstraint, + unsigned int id_predecessor, + unsigned int id_successor, + const Math::SpatialTransform &X_predecessor, + const Math::SpatialTransform &X_successor, + bool enable_stabilization, + const double stabilization_param, + const char *constraint_name) { + assert (bound == false); + + unsigned int n_constr_start_idx = size(); + unsigned int n_constr_size = size() + customConstraint->mConstraintCount; + + std::string name_str; + if (constraint_name != NULL) { + name_str = constraint_name; + } + + SpatialVector axis = SpatialVector::Zero(); + std::stringstream nameConstraintIndex; + + for(unsigned int i = 0; i < customConstraint->mConstraintCount; ++i ){ + constraintType.push_back( ConstraintTypeCustom ); + nameConstraintIndex << name_str << "_" << i; + name.push_back (nameConstraintIndex.str()); + nameConstraintIndex.str(std::string()); + if(i==0){ + mCustomConstraintIndices.push_back(n_constr_start_idx); + } + // These variables will be used for each CustomConstraint + body_p.push_back (id_predecessor); + body_s.push_back (id_successor); + X_p.push_back (X_predecessor); + X_s.push_back (X_successor); + constraintAxis.push_back (axis); + + // Set up constraint stabilization + double baumgarte_coefficient = 0.0; + if (enable_stabilization) { + if (stabilization_param == 0.0) { + std::cerr << "Error: Baumgarte stabilization is enabled but the stabilization parameter is 0.0" << std::endl; + abort(); + } + baumgarte_coefficient = 1.0 / stabilization_param; + } + baumgarteParameters.push_back( + Vector2d(baumgarte_coefficient, baumgarte_coefficient)); + + // These variables will not be used in CustomConstraints but are kept + // so that the indexing across all of the ConstraintSet variables + // remains preserved. + body.push_back (0); + point.push_back (Vector3d::Zero()); + normal.push_back (Vector3d::Zero()); + } + + err.conservativeResize( n_constr_size); + errd.conservativeResize(n_constr_size); + + acceleration.conservativeResize ( n_constr_size); + force.conservativeResize ( n_constr_size); + impulse.conservativeResize ( n_constr_size); + v_plus.conservativeResize ( n_constr_size); + d_multdof3_u = std::vector( + n_constr_size, Math::Vector3d::Zero()); + + for(unsigned int i = n_constr_start_idx; i < n_constr_size; i++){ + err[i] = 0.; + errd[i] = 0.; + acceleration[i] = 0.; + force[i] = 0.; + impulse[i] = 0.; + v_plus[i] = 0.; + } + + mCustomConstraints.push_back(customConstraint); + + return n_constr_size - 1; +} + +bool ConstraintSet::Bind (const Model &model) { + assert (bound == false); + + if (bound) { + std::cerr << "Error: binding an already bound constraint set!" << std::endl; + abort(); + } + unsigned int n_constr = size(); + + H.conservativeResize (model.dof_count, model.dof_count); + H.setZero(); + C.conservativeResize (model.dof_count); + C.setZero(); + gamma.conservativeResize (n_constr); + gamma.setZero(); + G.conservativeResize (n_constr, model.dof_count); + G.setZero(); + A.conservativeResize (model.dof_count + n_constr, model.dof_count + n_constr); + A.setZero(); + b.conservativeResize (model.dof_count + n_constr); + b.setZero(); + x.conservativeResize (model.dof_count + n_constr); + x.setZero(); + + Gi.conservativeResize (3, model.qdot_size); + GSpi.conservativeResize (6, model.qdot_size); + GSsi.conservativeResize (6, model.qdot_size); + GSJ.conservativeResize (6, model.qdot_size); + + // HouseHolderQR crashes if matrix G has more rows than columns. + GT_qr.compute(G.transpose()); + GT_qr_Q = MatrixNd::Zero (model.dof_count, model.dof_count); + Y = MatrixNd::Zero (model.dof_count, G.rows()); + Z = MatrixNd::Zero (model.dof_count, model.dof_count - G.rows()); + qddot_y = VectorNd::Zero (model.dof_count); + qddot_z = VectorNd::Zero (model.dof_count); + + K.conservativeResize (n_constr, n_constr); + K.setZero(); + a.conservativeResize (n_constr); + a.setZero(); + QDDot_t.conservativeResize (model.dof_count); + QDDot_t.setZero(); + QDDot_0.conservativeResize (model.dof_count); + QDDot_0.setZero(); + f_t.resize (n_constr, SpatialVector::Zero()); + f_ext_constraints.resize (model.mBodies.size(), SpatialVector::Zero()); + point_accel_0.resize (n_constr, Vector3d::Zero()); + + d_pA =std::vector (model.mBodies.size(),SpatialVector::Zero()); + d_a = std::vector (model.mBodies.size(),SpatialVector::Zero()); + d_u = VectorNd::Zero (model.mBodies.size()); + + d_IA = std::vector (model.mBodies.size() + , SpatialMatrix::Identity()); + d_U = std::vector (model.mBodies.size(),SpatialVector::Zero()); + d_d = VectorNd::Zero (model.mBodies.size()); + + d_multdof3_u = std::vector (model.mBodies.size() + , Math::Vector3d::Zero()); + + bound = true; + + return bound; +} + +void ConstraintSet::clear() { + acceleration.setZero(); + force.setZero(); + impulse.setZero(); + + H.setZero(); + C.setZero(); + gamma.setZero(); + G.setZero(); + A.setZero(); + b.setZero(); + x.setZero(); + + K.setZero(); + a.setZero(); + QDDot_t.setZero(); + QDDot_0.setZero(); + + unsigned int i; + for (i = 0; i < f_t.size(); i++) + f_t[i].setZero(); + + for (i = 0; i < f_ext_constraints.size(); i++) + f_ext_constraints[i].setZero(); + + for (i = 0; i < point_accel_0.size(); i++) + point_accel_0[i].setZero(); + + for (i = 0; i < d_pA.size(); i++) + d_pA[i].setZero(); + + for (i = 0; i < d_a.size(); i++) + d_a[i].setZero(); + + d_u.setZero(); +} + +RBDL_DLLAPI +void SolveConstrainedSystemDirect ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &UNUSED(qddot), + Math::VectorNd &UNUSED(lambda), + Math::MatrixNd &A, + Math::VectorNd &b, + Math::VectorNd &x, + Math::LinearSolver &linear_solver + ) { + // Build the system: Copy H + A.block(0, 0, c.rows(), c.rows()) = H; + + // Copy G and G^T + A.block(0, c.rows(), c.rows(), gamma.rows()) = G.transpose(); + A.block(c.rows(), 0, gamma.rows(), c.rows()) = G; + + // Build the system: Copy -C + \tau + b.block(0, 0, c.rows(), 1) = c; + b.block(c.rows(), 0, gamma.rows(), 1) = gamma; + + LOG << "A = " << std::endl << A << std::endl; + LOG << "b = " << std::endl << b << std::endl; + + switch (linear_solver) { + case (LinearSolverPartialPivLU) : + x = A.partialPivLu().solve(b); + break; + case (LinearSolverColPivHouseholderQR) : + x = A.colPivHouseholderQr().solve(b); + break; + case (LinearSolverHouseholderQR) : + x = A.householderQr().solve(b); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } + + LOG << "x = " << std::endl << x << std::endl; +} + +RBDL_DLLAPI +void SolveConstrainedSystemRangeSpaceSparse ( + Model &model, + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &K, + Math::VectorNd &a, + Math::LinearSolver UNUSED(linear_solver) + ) { + SparseFactorizeLTL (model, H); + + MatrixNd Y (G.transpose()); + + for (unsigned int i = 0; i < Y.cols(); i++) { + VectorNd Y_col = Y.block(0,i,Y.rows(),1); + SparseSolveLTx (model, H, Y_col); + Y.block(0,i,Y.rows(),1) = Y_col; + } + + VectorNd z (c); + SparseSolveLTx (model, H, z); + + K = Y.transpose() * Y; + + a = gamma - Y.transpose() * z; + + lambda = K.llt().solve(a); + + qddot = c + G.transpose() * lambda; + SparseSolveLTx (model, H, qddot); + SparseSolveLx (model, H, qddot); +} + +RBDL_DLLAPI +void SolveConstrainedSystemNullSpace ( + Math::MatrixNd &H, + const Math::MatrixNd &G, + const Math::VectorNd &c, + const Math::VectorNd &gamma, + Math::VectorNd &qddot, + Math::VectorNd &lambda, + Math::MatrixNd &Y, + Math::MatrixNd &Z, + Math::VectorNd &qddot_y, + Math::VectorNd &qddot_z, + Math::LinearSolver &linear_solver + ) { + switch (linear_solver) { + case (LinearSolverPartialPivLU) : + qddot_y = (G * Y).partialPivLu().solve (gamma); + break; + case (LinearSolverColPivHouseholderQR) : + qddot_y = (G * Y).colPivHouseholderQr().solve (gamma); + break; + case (LinearSolverHouseholderQR) : + qddot_y = (G * Y).householderQr().solve (gamma); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } + + qddot_z = (Z.transpose() * H * Z).llt().solve(Z.transpose() * (c - H * Y * qddot_y)); + + qddot = Y * qddot_y + Z * qddot_z; + + switch (linear_solver) { + case (LinearSolverPartialPivLU) : + lambda = (G * Y).partialPivLu().solve (Y.transpose() * (H * qddot - c)); + break; + case (LinearSolverColPivHouseholderQR) : + lambda = (G * Y).colPivHouseholderQr().solve (Y.transpose() * (H * qddot - c)); + break; + case (LinearSolverHouseholderQR) : + lambda = (G * Y).householderQr().solve (Y.transpose() * (H * qddot - c)); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } +} + +RBDL_DLLAPI +void CalcConstraintsPositionError ( + Model& model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics + ) { + assert(err.size() == CS.size()); + + if(update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) { + const unsigned int c = CS.mContactConstraintIndices[i]; + err[c] = 0.; + } + + for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) { + const unsigned int lci = CS.mLoopConstraintIndices[i]; + + // Variables used for computations. + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + SpatialVector d; + + // Constraints computed in the predecessor body frame. + + // Compute the orientation of the two constraint frames. + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[lci], false).transpose() + * CS.X_p[lci].E; + rot_s = CalcBodyWorldOrientation (model, Q, CS.body_s[lci], false).transpose() + * CS.X_s[lci].E; + + // Compute the orientation from the predecessor to the successor frame. + rot_ps = rot_p.transpose() * rot_s; + + // Compute the position of the two contact points. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[lci], CS.X_p[lci].r + , false); + pos_s = CalcBodyToBaseCoordinates (model, Q, CS.body_s[lci], CS.X_s[lci].r + , false); + + // The first three elements represent the rotation error. + // This formulation is equivalent to u * sin(theta), where u and theta are + // the angle-axis of rotation from the predecessor to the successor frame. + // These quantities are expressed in the predecessor frame. + d[0] = -0.5 * (rot_ps(1,2) - rot_ps(2,1)); + d[1] = -0.5 * (rot_ps(2,0) - rot_ps(0,2)); + d[2] = -0.5 * (rot_ps(0,1) - rot_ps(1,0)); + + // The last three elements represent the position error. + // It is equivalent to the difference in the position of the two + // constraint points. + // The distance is projected on the predecessor frame to be consistent + // with the rotation. + d.block<3,1>(3,0) = rot_p.transpose() * (pos_s - pos_p); + + // Project the error on the constraint axis to find the actual error. + err[lci] = CS.constraintAxis[lci].transpose() * d; + } + + for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) { + const unsigned int cci = CS.mCustomConstraintIndices[i]; + CS.mCustomConstraints[i]->CalcPositionError(model,cci,Q,CS,err, cci); + } +} + +RBDL_DLLAPI +void CalcConstraintsJacobian ( + Model &model, + const Math::VectorNd &Q, + ConstraintSet &CS, + Math::MatrixNd &G, + bool update_kinematics + ) { + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + // variables to check whether we need to recompute G. + unsigned int prev_body_id_1 = 0; + unsigned int prev_body_id_2 = 0; + SpatialTransform prev_body_X_1; + SpatialTransform prev_body_X_2; + + for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) { + const unsigned int c = CS.mContactConstraintIndices[i]; + + // only compute the matrix Gi if actually needed + if (prev_body_id_1 != CS.body[c] + || prev_body_X_1.r != CS.point[c]) { + + // Compute the jacobian for the point. + CS.Gi.setZero(); + CalcPointJacobian (model, Q, CS.body[c], CS.point[c], CS.Gi, false); + + // Update variables for optimization check. + prev_body_id_1 = CS.body[c]; + prev_body_X_1 = Xtrans(CS.point[c]); + } + + for(unsigned int j = 0; j < model.dof_count; j++) { + Vector3d gaxis (CS.Gi(0,j), CS.Gi(1,j), CS.Gi(2,j)); + G(c,j) = gaxis.transpose() * CS.normal[c]; + } + } + + // Variables used for computations. + Vector3d normal; + SpatialVector axis; + Vector3d pos_p; + Matrix3d rot_p; + SpatialTransform X_0p; + + for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) { + const unsigned int c = CS.mLoopConstraintIndices[i]; + + // Only recompute variables if necessary. + if( prev_body_id_1 != CS.body_p[c] + || prev_body_id_2 != CS.body_s[c] + || prev_body_X_1.r != CS.X_p[c].r + || prev_body_X_2.r != CS.X_s[c].r + || prev_body_X_1.E != CS.X_p[c].E + || prev_body_X_2.E != CS.X_s[c].E) { + + // Compute the 6D jacobians of the two contact points. + CS.GSpi.setZero(); + CS.GSsi.setZero(); + CalcPointJacobian6D(model, Q, CS.body_p[c], CS.X_p[c].r, CS.GSpi, false); + CalcPointJacobian6D(model, Q, CS.body_s[c], CS.X_s[c].r, CS.GSsi, false); + CS.GSJ = CS.GSsi - CS.GSpi; + + // Compute position and rotation matrix from predecessor body to base. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c], CS.X_p[c].r + , false); + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c] + , false).transpose()* CS.X_p[c].E; + X_0p = SpatialTransform (rot_p, pos_p); + + // Update variables for optimization check. + prev_body_id_1 = CS.body_p[c]; + prev_body_id_2 = CS.body_s[c]; + prev_body_X_1 = CS.X_p[c]; + prev_body_X_2 = CS.X_s[c]; + } + + // Express the constraint axis in the base frame. + axis = X_0p.apply(CS.constraintAxis[c]); + + // Compute the constraint Jacobian row. + G.block(c, 0, 1, model.dof_count) = axis.transpose() * CS.GSJ; + } + + // Go and get the CustomConstraint Jacobians + for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) { + const unsigned int cci = CS.mCustomConstraintIndices[i]; + // const unsigned int rows= CS.mCustomConstraints[i]->mConstraintCount; + // const unsigned int cols= CS.G.cols(); + CS.mCustomConstraints[i]->CalcConstraintsJacobianAndConstraintAxis( + model,cci,Q,CS,G, cci,0); + } +} + +RBDL_DLLAPI +void CalcConstraintsVelocityError ( + Model& model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + ConstraintSet &CS, + Math::VectorNd& err, + bool update_kinematics + ) { + + //This works for the contact and loop constraints because they are + //time invariant. But this does not necessarily work for the CustomConstraints + //which can be time-varying. And thus the parts of err associated with the + //custom constraints must be updated. + //MatrixNd G(MatrixNd::Zero(CS.size(), model.dof_count)); + CalcConstraintsJacobian (model, Q, CS, CS.G, update_kinematics); + err = CS.G * QDot; + + unsigned int cci, rows, cols; + for (unsigned int i = 0; i < CS.mCustomConstraintIndices.size(); i++) { + cci = CS.mCustomConstraintIndices[i]; + rows= CS.mCustomConstraints[i]->mConstraintCount; + cols= CS.G.cols(); + CS.mCustomConstraints[i]->CalcVelocityError(model,cci,Q,QDot,CS, + CS.G.block(cci,0,rows,cols),err, cci); + } + + +} + +RBDL_DLLAPI +void CalcConstrainedSystemVariables ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &UNUSED(Tau), + ConstraintSet &CS, + std::vector *f_ext + ) { + // Compute C + NonlinearEffects(model, Q, QDot, CS.C, f_ext); + assert(CS.H.cols() == model.dof_count && CS.H.rows() == model.dof_count); + + // Compute H + CS.H.setZero(); + CompositeRigidBodyAlgorithm(model, Q, CS.H, false); + + // Compute G + // We have to update model.X_base as they are not automatically computed + // by NonlinearEffects() + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + model.X_base[i] = model.X_lambda[i] * model.X_base[model.lambda[i]]; + } + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + // Compute position error for Baumgarte Stabilization. + CalcConstraintsPositionError (model, Q, CS, CS.err, false); + + // Compute velocity error for Baugarte stabilization. + CalcConstraintsVelocityError (model, Q, QDot, CS, CS.errd, false); + //CS.errd = CS.G * QDot; + + // Compute gamma + unsigned int prev_body_id = 0; + Vector3d prev_body_point = Vector3d::Zero(); + Vector3d gamma_i = Vector3d::Zero(); + + CS.QDDot_0.setZero(); + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0); + + for (unsigned int i = 0; i < CS.mContactConstraintIndices.size(); i++) { + const unsigned int c = CS.mContactConstraintIndices[i]; + + // only compute point accelerations when necessary + if (prev_body_id != CS.body[c] || prev_body_point != CS.point[c]) { + gamma_i = CalcPointAcceleration (model, Q, QDot, CS.QDDot_0, CS.body[c] + , CS.point[c], false); + prev_body_id = CS.body[c]; + prev_body_point = CS.point[c]; + } + + // we also substract ContactData[c].acceleration such that the contact + // point will have the desired acceleration + CS.gamma[c] = CS.acceleration[c] - CS.normal[c].dot(gamma_i); + } + + + + for (unsigned int i = 0; i < CS.mLoopConstraintIndices.size(); i++) { + const unsigned int c = CS.mLoopConstraintIndices[i]; + + // Variables used for computations. + Vector3d pos_p; + Matrix3d rot_p; + SpatialVector vel_p; + SpatialVector vel_s; + SpatialVector axis; + // unsigned int id_p; + // unsigned int id_s; + + + // Express the constraint axis in the base frame. + pos_p = CalcBodyToBaseCoordinates (model, Q, CS.body_p[c], + CS.X_p[c].r,false); + rot_p = CalcBodyWorldOrientation (model, Q, CS.body_p[c], false + ).transpose() * CS.X_p[c].E; + axis = SpatialTransform (rot_p, pos_p).apply(CS.constraintAxis[c]); + + // Compute the spatial velocities of the two constrained bodies. + vel_p = CalcPointVelocity6D (model, Q, QDot, CS.body_p[c], + CS.X_p[c].r, false); + vel_s = CalcPointVelocity6D (model, Q, QDot, CS.body_s[c], + CS.X_s[c].r, false); + + // Compute the derivative of the axis wrt the base frame. + SpatialVector axis_dot = crossm(vel_p, axis); + + // Compute the velocity product accelerations. These correspond to the + // accelerations that the bodies would have if q ddot were 0. + SpatialVector acc_p = CalcPointAcceleration6D (model, Q, QDot + , VectorNd::Zero(model.dof_count), CS.body_p[c], CS.X_p[c].r, false); + SpatialVector acc_s = CalcPointAcceleration6D (model, Q, QDot + , VectorNd::Zero(model.dof_count), CS.body_s[c], CS.X_s[c].r, false); + + // Problem here if one of the bodies is fixed... + // Compute the value of gamma. + CS.gamma[c] + // Right hand side term. + = - axis.dot(acc_s - acc_p) - axis_dot.dot(vel_s - vel_p) + // Baumgarte stabilization term. + - 2. * CS.baumgarteParameters[c][0] * CS.errd[c] + - CS.baumgarteParameters[c][1] * CS.baumgarteParameters[c][1] * CS.err[c]; + } + + unsigned int ccid,rows,cols,z; + for(unsigned int i=0; i< CS.mCustomConstraintIndices.size(); i++){ + ccid = CS.mCustomConstraintIndices[i]; + rows = CS.mCustomConstraints[i]->mConstraintCount; + cols = CS.G.cols(); + CS.mCustomConstraints[i]->CalcGamma(model,ccid,Q,QDot,CS, + CS.G.block(ccid,0,rows,cols), + CS.gamma,ccid); + for(unsigned int j=0; jmConstraintCount;j++){ + z = ccid+j; + CS.gamma[z] += (- 2. * CS.baumgarteParameters[z][0] * CS.errd[z] + - CS.baumgarteParameters[z][1] + * CS.baumgarteParameters[z][1] * CS.err[z]); + } + + } + +} + +RBDL_DLLAPI +bool CalcAssemblyQ ( + Model &model, + Math::VectorNd QInit, // Note: passed by value intentionally + ConstraintSet &cs, + Math::VectorNd &Q, + const Math::VectorNd &weights, + double tolerance, + unsigned int max_iter + ) { + + if(Q.size() != model.q_size) { + std::cerr << "Incorrect Q vector size." << std::endl; + assert(false); + abort(); + } + if(QInit.size() != model.q_size) { + std::cerr << "Incorrect QInit vector size." << std::endl; + assert(false); + abort(); + } + if(weights.size() != model.dof_count) { + std::cerr << "Incorrect weights vector size." << std::endl; + assert(false); + abort(); + } + + // Initialize variables. + MatrixNd constraintJac (cs.size(), model.dof_count); + MatrixNd A = MatrixNd::Zero (cs.size() + model.dof_count, cs.size() + + model.dof_count); + VectorNd b = VectorNd::Zero (cs.size() + model.dof_count); + VectorNd x = VectorNd::Zero (cs.size() + model.dof_count); + VectorNd d = VectorNd::Zero (model.dof_count); + VectorNd e = VectorNd::Zero (cs.size()); + + // The top-left block is the weight matrix and is constant. + for(unsigned int i = 0; i < weights.size(); ++i) { + A(i,i) = weights[i]; + } + + // Check if the error is small enough already. If so, just return the initial + // guess as the solution. + CalcConstraintsPositionError (model, QInit, cs, e); + if (e.norm() < tolerance) { + Q = QInit; + return true; + } + + // We solve the linearized problem iteratively. + // Iterations are stopped if the maximum is reached. + for(unsigned int it = 0; it < max_iter; ++it) { + // Compute the constraint jacobian and build A and b. + constraintJac.setZero(); + CalcConstraintsJacobian (model, QInit, cs, constraintJac); + A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac; + A.block (0, model.dof_count, model.dof_count, cs.size()) + = constraintJac.transpose(); + b.block (model.dof_count, 0, cs.size(), 1) = -e; + + // Solve the sistem A*x = b. + SolveLinearSystem (A, b, x, cs.linear_solver); + + // Extract the d = (Q - QInit) vector from x. + d = x.block (0, 0, model.dof_count, 1); + + // Update solution. + for (size_t i = 0; i < model.mJoints.size(); ++i) { + // If the joint is spherical, translate the corresponding components + // of d into a modification in the joint quaternion. + if (model.mJoints[i].mJointType == JointTypeSpherical) { + Quaternion quat = model.GetQuaternion(i, QInit); + Vector3d omega = d.block<3,1>(model.mJoints[i].q_index,0); + // Convert the 3d representation of the displacement to 4d and sum it + // to the components of the quaternion. + quat += quat.omegaToQDot(omega); + // The quaternion needs to be normalized after the previous sum. + quat /= quat.norm(); + model.SetQuaternion(i, quat, QInit); + } + // If the current joint is not spherical, simply add the corresponding + // components of d to QInit. + else { + unsigned int qIdx = model.mJoints[i].q_index; + for(size_t j = 0; j < model.mJoints[i].mDoFCount; ++j) { + QInit[qIdx + j] += d[qIdx + j]; + } + // QInit.block(qIdx, 0, model.mJoints[i].mDoFCount, 1) + // += d.block(model.mJoints[i].q_index, 0, model.mJoints[i].mDoFCount, 1); + } + } + + // Update the errors. + CalcConstraintsPositionError (model, QInit, cs, e); + + // Check if the error and the step are small enough to end. + if (e.norm() < tolerance && d.norm() < tolerance){ + Q = QInit; + return true; + } + } + + // Return false if maximum number of iterations is exceeded. + Q = QInit; + return false; +} + +RBDL_DLLAPI +void CalcAssemblyQDot ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotInit, + ConstraintSet &cs, + Math::VectorNd &QDot, + const Math::VectorNd &weights + ) { + if(QDot.size() != model.dof_count) { + std::cerr << "Incorrect QDot vector size." << std::endl; + assert(false); + abort(); + } + if(Q.size() != model.q_size) { + std::cerr << "Incorrect Q vector size." << std::endl; + assert(false); + abort(); + } + if(QDotInit.size() != QDot.size()) { + std::cerr << "Incorrect QDotInit vector size." << std::endl; + assert(false); + abort(); + } + if(weights.size() != QDot.size()) { + std::cerr << "Incorrect weight vector size." << std::endl; + assert(false); + abort(); + } + + // Initialize variables. + MatrixNd constraintJac = MatrixNd::Zero(cs.size(), model.dof_count); + MatrixNd A = MatrixNd::Zero(cs.size() + model.dof_count, cs.size() + + model.dof_count); + VectorNd b = VectorNd::Zero(cs.size() + model.dof_count); + VectorNd x = VectorNd::Zero(cs.size() + model.dof_count); + + // The top-left block is the weight matrix and is constant. + for(unsigned int i = 0; i < weights.size(); ++i) { + A(i,i) = weights[i]; + b[i] = weights[i] * QDotInit[i]; + } + CalcConstraintsJacobian (model, Q, cs, constraintJac); + A.block (model.dof_count, 0, cs.size(), model.dof_count) = constraintJac; + A.block (0, model.dof_count, model.dof_count, cs.size()) + = constraintJac.transpose(); + + // Solve the sistem A*x = b. + SolveLinearSystem (A, b, x, cs.linear_solver); + + // Copy the result to the output variable. + QDot = x.block (0, 0, model.dof_count, 1); +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsDirect ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot, + std::vector *f_ext + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext); + + SolveConstrainedSystemDirect (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot + , CS.force, CS.A, CS.b, CS.x, CS.linear_solver); + + // Copy back QDDot + for (unsigned int i = 0; i < model.dof_count; i++) + QDDot[i] = CS.x[i]; + + // Copy back contact forces + for (unsigned int i = 0; i < CS.size(); i++) { + CS.force[i] = -CS.x[model.dof_count + i]; + } +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + const Math::VectorNd &Tau, + ConstraintSet &CS, + Math::VectorNd &QDDot, + std::vector *f_ext) { + + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext); + + SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, Tau - CS.C + , CS.gamma, QDDot, CS.force, CS.K, CS.a, CS.linear_solver); +} + +RBDL_DLLAPI +void ForwardDynamicsConstraintsNullSpace ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot, + std::vector *f_ext + ) { + + LOG << "-------- " << __func__ << " --------" << std::endl; + + CalcConstrainedSystemVariables (model, Q, QDot, Tau, CS, f_ext); + + CS.GT_qr.compute (CS.G.transpose()); + CS.GT_qr.householderQ().evalTo (CS.GT_qr_Q); + + CS.Y = CS.GT_qr_Q.block (0,0,QDot.rows(), CS.G.rows()); + CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDot.rows(), QDot.rows() - CS.G.rows()); + + SolveConstrainedSystemNullSpace (CS.H, CS.G, Tau - CS.C, CS.gamma, QDDot + , CS.force, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z, CS.linear_solver); + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesDirect ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + SolveConstrainedSystemDirect (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus + , QDotPlus, CS.impulse, CS.A, CS.b, CS.x, CS.linear_solver); + + // Copy back QDotPlus + for (unsigned int i = 0; i < model.dof_count; i++) + QDotPlus[i] = CS.x[i]; + + // Copy back constraint impulses + for (unsigned int i = 0; i < CS.size(); i++) { + CS.impulse[i] = CS.x[model.dof_count + i]; + } + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesRangeSpaceSparse ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + SolveConstrainedSystemRangeSpaceSparse (model, CS.H, CS.G, CS.H * QDotMinus + , CS.v_plus, QDotPlus, CS.impulse, CS.K, CS.a, CS.linear_solver); + +} + +RBDL_DLLAPI +void ComputeConstraintImpulsesNullSpace ( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDotMinus, + ConstraintSet &CS, + Math::VectorNd &QDotPlus + ) { + + // Compute H + UpdateKinematicsCustom (model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (model, Q, CS.H, false); + + // Compute G + CalcConstraintsJacobian (model, Q, CS, CS.G, false); + + CS.GT_qr.compute(CS.G.transpose()); + CS.GT_qr_Q = CS.GT_qr.householderQ(); + + CS.Y = CS.GT_qr_Q.block (0,0,QDotMinus.rows(), CS.G.rows()); + CS.Z = CS.GT_qr_Q.block (0,CS.G.rows(),QDotMinus.rows(), QDotMinus.rows() + - CS.G.rows()); + + SolveConstrainedSystemNullSpace (CS.H, CS.G, CS.H * QDotMinus, CS.v_plus + , QDotPlus, CS.impulse, CS.Y, CS.Z, CS.qddot_y, CS.qddot_z + , CS.linear_solver); +} + +/** \brief Compute only the effects of external forces on the generalized accelerations + * + * This function is a reduced version of ForwardDynamics() which only + * computes the effects of the external forces on the generalized + * accelerations. + * + */ +RBDL_DLLAPI +void ForwardDynamicsApplyConstraintForces ( + Model &model, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (QDDot.size() == model.dof_count); + + unsigned int i = 0; + + for (i = 1; i < model.mBodies.size(); i++) { + model.IA[i] = model.I[i].toMatrix();; + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (CS.f_ext_constraints[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * CS.f_ext_constraints[i]; + } + } + + // ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + unsigned int lambda = model.lambda[i]; + model.multdof3_u[i] = Vector3d (Tau[q_index], + Tau[q_index + 1], + Tau[q_index + 2]) + - model.multdof3_S[i].transpose() * model.pA[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose()); + + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() + << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.U[i] * (model.U[i] / model.d[i]).transpose(); + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + model.U[i] * model.u[i] / model.d[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix()); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if(model.mJoints[i].mJointType == JointTypeCustom) { + + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + unsigned int lambda = model.lambda[i]; + VectorNd tau_temp = VectorNd::Zero(dofI); + + for(unsigned int z = 0 ; z < dofI; ++z){ + tau_temp[z] = Tau[q_index+z]; + } + + model.mCustomJoints[kI]->u = tau_temp + - (model.mCustomJoints[kI]->S.transpose() + * model.pA[i]); + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); + + SpatialVector pa = model.pA[i] + Ia * model.c[i] + + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() + << std::endl; + } + } + } + + model.a[0] = SpatialVector (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * + (model.multdof3_u[i] + - model.multdof3_U[i].transpose() * model.a[i]); + + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd qdd_temp = VectorNd::Zero(dofI); + + qdd_temp = model.mCustomJoints[kI]->Dinv + * (model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() + * model.a[i]); + + for(unsigned int z = 0; z < dofI; ++z){ + QDDot[q_index+z] = qdd_temp[z]; + } + + model.a[i] = model.a[i] + (model.mCustomJoints[kI]->S * qdd_temp); + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +/** \brief Computes the effect of external forces on the generalized accelerations. + * + * This function is essentially similar to ForwardDynamics() except that it + * tries to only perform computations of variables that change due to + * external forces defined in f_t. + */ +RBDL_DLLAPI +void ForwardDynamicsAccelerationDeltas ( + Model &model, + ConstraintSet &CS, + VectorNd &QDDot_t, + const unsigned int body_id, + const std::vector &f_t + ) { + LOG << "-------- " << __func__ << " ------" << std::endl; + + assert (CS.d_pA.size() == model.mBodies.size()); + assert (CS.d_a.size() == model.mBodies.size()); + assert (CS.d_u.size() == model.mBodies.size()); + + // TODO reset all values (debug) + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + CS.d_pA[i].setZero(); + CS.d_a[i].setZero(); + CS.d_u[i] = 0.; + CS.d_multdof3_u[i].setZero(); + } + for(unsigned int i=0; id_u.setZero(); + } + + for (unsigned int i = body_id; i > 0; i--) { + if (i == body_id) { + CS.d_pA[i] = -model.X_base[i].applyAdjoint(f_t[i]); + } + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + CS.d_multdof3_u[i] = - model.multdof3_S[i].transpose() * (CS.d_pA[i]); + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + CS.d_pA[lambda] = CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + (model.multdof3_U[i] + * model.multdof3_Dinv[i] + * CS.d_multdof3_u[i])); + } + } else if(model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + CS.d_u[i] = - model.S[i].dot(CS.d_pA[i]); + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + CS.d_pA[lambda] = CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]); + } + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + + unsigned int kI = model.mJoints[i].custom_joint_index; + // unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + //CS. + model.mCustomJoints[kI]->d_u = + - model.mCustomJoints[kI]->S.transpose() * (CS.d_pA[i]); + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + CS.d_pA[lambda] = + CS.d_pA[lambda] + + model.X_lambda[i].applyTranspose ( + CS.d_pA[i] + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->d_u) + ); + } + } + } + + for (unsigned int i = 0; i < f_t.size(); i++) { + LOG << "f_t[" << i << "] = " << f_t[i].transpose() << std::endl; + } + + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + LOG << "i = " << i << ": d_pA[i] " << CS.d_pA[i].transpose() << std::endl; + } + for (unsigned int i = 0; i < model.mBodies.size(); i++) { + LOG << "i = " << i << ": d_u[i] = " << CS.d_u[i] << std::endl; + } + + QDDot_t[0] = 0.; + CS.d_a[0] = model.a[0]; + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + SpatialVector Xa = model.X_lambda[i].apply(CS.d_a[lambda]); + + if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] + * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa); + + QDDot_t[q_index] = qdd_temp[0]; + QDDot_t[q_index + 1] = qdd_temp[1]; + QDDot_t[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + CS.d_a[i] = Xa + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom){ + + QDDot_t[q_index] = (CS.d_u[i] - model.U[i].dot(Xa) ) / model.d[i]; + CS.d_a[i] = Xa + model.S[i] * QDDot_t[q_index]; + } else if (model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd qdd_temp = VectorNd::Zero(dofI); + + qdd_temp = model.mCustomJoints[kI]->Dinv + * (model.mCustomJoints[kI]->d_u + - model.mCustomJoints[kI]->U.transpose() * Xa); + + for(unsigned int z = 0; z < dofI; ++z){ + QDDot_t[q_index+z] = qdd_temp[z]; + } + + model.a[i] = model.a[i] + model.mCustomJoints[kI]->S * qdd_temp; + CS.d_a[i] = Xa + model.mCustomJoints[kI]->S * qdd_temp; + } + + LOG << "QDDot_t[" << i - 1 << "] = " << QDDot_t[i - 1] << std::endl; + LOG << "d_a[i] = " << CS.d_a[i].transpose() << std::endl; + } +} + +inline void set_zero (std::vector &spatial_values) { + for (unsigned int i = 0; i < spatial_values.size(); i++) + spatial_values[i].setZero(); +} + +RBDL_DLLAPI +void ForwardDynamicsContactsKokkevis ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + ConstraintSet &CS, + VectorNd &QDDot + ) { + LOG << "-------- " << __func__ << " ------" << std::endl; + + assert (CS.f_ext_constraints.size() == model.mBodies.size()); + assert (CS.QDDot_0.size() == model.dof_count); + assert (CS.QDDot_t.size() == model.dof_count); + assert (CS.f_t.size() == CS.size()); + assert (CS.point_accel_0.size() == CS.size()); + assert (CS.K.rows() == CS.size()); + assert (CS.K.cols() == CS.size()); + assert (CS.force.size() == CS.size()); + assert (CS.a.size() == CS.size()); + + Vector3d point_accel_t; + + unsigned int ci = 0; + + // The default acceleration only needs to be computed once + { + SUPPRESS_LOGGING; + ForwardDynamics(model, Q, QDot, Tau, CS.QDDot_0); + } + + LOG << "=== Initial Loop Start ===" << std::endl; + // we have to compute the standard accelerations first as we use them to + // compute the effects of each test force + for(ci = 0; ci < CS.size(); ci++) { + + { + SUPPRESS_LOGGING; + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_0); + } + + if(CS.constraintType[ci] == ConstraintSet::ContactConstraint) + { + LOG << "body_id = " << CS.body[ci] << std::endl; + LOG << "point = " << CS.point[ci] << std::endl; + LOG << "normal = " << CS.normal[ci] << std::endl; + LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl; + { + SUPPRESS_LOGGING; + CS.point_accel_0[ci] = CalcPointAcceleration (model, Q, QDot + , CS.QDDot_0, CS.body[ci], CS.point[ci], false); + CS.a[ci] = - CS.acceleration[ci] + + CS.normal[ci].dot(CS.point_accel_0[ci]); + } + LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose(); + } + else + { + std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \ + type." << std::endl; + assert(false); + abort(); + } + } + + // Now we can compute and apply the test forces and use their net effect + // to compute the inverse articlated inertia to fill K. + for (ci = 0; ci < CS.size(); ci++) { + + LOG << "=== Testforce Loop Start ===" << std::endl; + + unsigned int movable_body_id = 0; + Vector3d point_global; + + switch (CS.constraintType[ci]) { + + case ConstraintSet::ContactConstraint: + + movable_body_id = GetMovableBodyId(model, CS.body[ci]); + + // assemble the test force + LOG << "normal = " << CS.normal[ci].transpose() << std::endl; + + point_global = CalcBodyToBaseCoordinates(model, Q, CS.body[ci] + , CS.point[ci], false); + + LOG << "point_global = " << point_global.transpose() << std::endl; + + CS.f_t[ci] = SpatialTransform(Matrix3d::Identity(), -point_global) + .applyAdjoint(SpatialVector (0., 0., 0. + , -CS.normal[ci][0], -CS.normal[ci][1], -CS.normal[ci][2])); + CS.f_ext_constraints[movable_body_id] = CS.f_t[ci]; + + LOG << "f_t[" << movable_body_id << "] = " << CS.f_t[ci].transpose() + << std::endl; + + { + ForwardDynamicsAccelerationDeltas(model, CS, CS.QDDot_t + , movable_body_id, CS.f_ext_constraints); + + LOG << "QDDot_0 = " << CS.QDDot_0.transpose() << std::endl; + LOG << "QDDot_t = " << (CS.QDDot_t + CS.QDDot_0).transpose() + << std::endl; + LOG << "QDDot_t - QDDot_0 = " << (CS.QDDot_t).transpose() << std::endl; + } + + CS.f_ext_constraints[movable_body_id].setZero(); + + CS.QDDot_t += CS.QDDot_0; + + // compute the resulting acceleration + { + SUPPRESS_LOGGING; + UpdateKinematicsCustom(model, NULL, NULL, &CS.QDDot_t); + } + + for(unsigned int cj = 0; cj < CS.size(); cj++) { + { + SUPPRESS_LOGGING; + + point_accel_t = CalcPointAcceleration(model, Q, QDot, CS.QDDot_t + , CS.body[cj], CS.point[cj], false); + } + + LOG << "point_accel_0 = " << CS.point_accel_0[ci].transpose() + << std::endl; + LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl; + + CS.K(ci,cj) = CS.normal[cj].dot(point_accel_t - CS.point_accel_0[cj]); + + } + + break; + + default: + + std::cerr << "Forward Dynamic Contact Kokkevis: unsupported constraint \ + type." << std::endl; + assert(false); + abort(); + + break; + + } + + } + + LOG << "K = " << std::endl << CS.K << std::endl; + LOG << "a = " << std::endl << CS.a << std::endl; + + switch (CS.linear_solver) { + case (LinearSolverPartialPivLU) : + CS.force = CS.K.partialPivLu().solve(CS.a); + break; + case (LinearSolverColPivHouseholderQR) : + CS.force = CS.K.colPivHouseholderQr().solve(CS.a); + break; + case (LinearSolverHouseholderQR) : + CS.force = CS.K.householderQr().solve(CS.a); + break; + default: + LOG << "Error: Invalid linear solver: " << CS.linear_solver << std::endl; + assert (0); + break; + } + + LOG << "f = " << CS.force.transpose() << std::endl; + + for (ci = 0; ci < CS.size(); ci++) { + unsigned int body_id = CS.body[ci]; + unsigned int movable_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + movable_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + CS.f_ext_constraints[movable_body_id] -= CS.f_t[ci] * CS.force[ci]; + LOG << "f_ext[" << movable_body_id << "] = " << CS.f_ext_constraints[movable_body_id].transpose() << std::endl; + } + + { + SUPPRESS_LOGGING; + ForwardDynamicsApplyConstraintForces (model, Tau, CS, QDDot); + } + + LOG << "QDDot after applying f_ext: " << QDDot.transpose() << std::endl; +} + +void SolveLinearSystem ( + const MatrixNd& A, + const VectorNd& b, + VectorNd& x, + LinearSolver ls + ) { + if(A.rows() != b.size() || A.cols() != x.size()) { + std::cerr << "Mismatching sizes." << std::endl; + assert(false); + abort(); + } + + // Solve the sistem A*x = b. + switch (ls) { + case (LinearSolverPartialPivLU) : + x = A.partialPivLu().solve(b); + break; + case (LinearSolverColPivHouseholderQR) : + x = A.colPivHouseholderQr().solve(b); + break; + case (LinearSolverHouseholderQR) : + x = A.householderQr().solve(b); + break; + default: + std::cerr << "Error: Invalid linear solver: " << ls << std::endl; + assert(false); + abort(); + break; + } +} + +unsigned int GetMovableBodyId (Model& model, unsigned int id) { + if(model.IsFixedBodyId(id)) { + unsigned int fbody_id = id - model.fixed_body_discriminator; + return model.mFixedBodies[fbody_id].mMovableParent; + } + else { + return id; + } +} + +} /* namespace RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/Dynamics.cc b/3rdparty/rbdl/src/Dynamics.cc new file mode 100644 index 0000000..53cf4c4 --- /dev/null +++ b/3rdparty/rbdl/src/Dynamics.cc @@ -0,0 +1,862 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" +#include "rbdl/Body.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void InverseDynamics ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + VectorNd &Tau, + std::vector *f_ext) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.multdof3_S[i] * Vector3d (QDDot[q_index], + QDDot[q_index + 1], + QDDot[q_index + 2]); + } + }else if(model.mJoints[i].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[i].custom_joint_index; + VectorNd customJointQDDot(model.mCustomJoints[k]->mDoFCount); + for(unsigned z = 0; z < model.mCustomJoints[k]->mDoFCount; ++z){ + customJointQDDot[z] = QDDot[q_index+z]; + } + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + + model.c[i] + + model.mCustomJoints[k]->S * customJointQDDot; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + } else { + model.f[i].setZero(); + } + } + + if (f_ext != NULL) { + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]); + } else if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) + = model.multdof3_S[i].transpose() * model.f[i]; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[i].custom_joint_index; + Tau.block(model.mJoints[i].q_index,0, + model.mCustomJoints[k]->mDoFCount, 1) + = model.mCustomJoints[k]->S.transpose() * model.f[i]; + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI void NonlinearEffects ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + VectorNd &Tau, + std::vector *f_ext) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]); + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0] = spatial_gravity; + + for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) { + jcalc (model, model.mJointUpdateOrder[i], Q, QDot); + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.lambda[i] == 0) { + model.v[i] = model.v_J[i]; + model.a[i] = model.X_lambda[i].apply(spatial_gravity); + } else { + model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i]; + } + + if (!model.mBodies[i].mIsVirtual) { + model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]); + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { + model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } else { + model.f[i].setZero(); + } + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + Tau[model.mJoints[i].q_index] + = model.S[i].dot(model.f[i]); + } else if (model.mJoints[i].mDoFCount == 3) { + Tau.block<3,1>(model.mJoints[i].q_index, 0) + = model.multdof3_S[i].transpose() * model.f[i]; + } + } else if(model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[i].custom_joint_index; + Tau.block(model.mJoints[i].q_index,0, + model.mCustomJoints[k]->mDoFCount, 1) + = model.mCustomJoints[k]->S.transpose() * model.f[i]; + } + + if (model.lambda[i] != 0) { + model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]); + } + } +} + +RBDL_DLLAPI void CompositeRigidBodyAlgorithm ( + Model& model, + const VectorNd &Q, + MatrixNd &H, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + assert (H.rows() == model.dof_count && H.cols() == model.dof_count); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (update_kinematics) { + jcalc_X_lambda_S (model, i, Q); + } + model.Ic[i] = model.I[i]; + } + + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + if (model.lambda[i] != 0) { + model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]); + } + + unsigned int dof_index_i = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + SpatialVector F = model.Ic[i] * model.S[i]; + H(dof_index_i, dof_index_i) = model.S[i].dot(F); + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F = model.X_lambda[j].applyTranspose(F); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom) { + if (model.mJoints[j].mDoFCount == 1) { + H(dof_index_i,dof_index_j) = F.dot(model.S[j]); + H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j); + } else if (model.mJoints[j].mDoFCount == 3) { + Vector3d H_temp2 = + (F.transpose() * model.multdof3_S[j]).transpose(); + LOG << F.transpose() << std::endl + << model.multdof3_S[j] << std::endl; + LOG << H_temp2.transpose() << std::endl; + + H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose(); + H.block<3,1>(dof_index_j,dof_index_i) = H_temp2; + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + VectorNd H_temp2 = + (F.transpose() * model.mCustomJoints[k]->S).transpose(); + + LOG << F.transpose() + << std::endl + << model.mCustomJoints[j]->S << std::endl; + + LOG << H_temp2.transpose() << std::endl; + + H.block(dof_index_i,dof_index_j,1,dof) = H_temp2.transpose(); + H.block(dof_index_j,dof_index_i,dof,1) = H_temp2; + } + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i]; + H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + Vector3d H_temp2 = F_63.transpose() * (model.S[j]); + + H.block<3,1>(dof_index_i,dof_index_j) = H_temp2; + H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } else if (model.mJoints[j].mDoFCount == 3) { + Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]); + + H.block<3,3>(dof_index_i,dof_index_j) = H_temp2; + H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose(); + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + + MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S); + + H.block(dof_index_i,dof_index_j,3,dof) = H_temp2; + H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose(); + } + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + + MatrixNd F_Nd = model.Ic[i].toMatrix() + * model.mCustomJoints[kI]->S; + + H.block(dof_index_i, dof_index_i,dofI,dofI) + = model.mCustomJoints[kI]->S.transpose() * F_Nd; + + unsigned int j = i; + unsigned int dof_index_j = dof_index_i; + + while (model.lambda[j] != 0) { + F_Nd = model.X_lambda[j].toMatrixTranspose() * (F_Nd); + j = model.lambda[j]; + dof_index_j = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]); + H.block( dof_index_i, dof_index_j, + H_temp2.rows(),H_temp2.cols()) = H_temp2; + H.block(dof_index_j,dof_index_i, + H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose(); + } else if (model.mJoints[j].mDoFCount == 3) { + MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]); + H.block(dof_index_i, dof_index_j, + H_temp2.rows(),H_temp2.cols()) = H_temp2; + H.block(dof_index_j, dof_index_i, + H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose(); + } + } else if (model.mJoints[j].mJointType == JointTypeCustom){ + unsigned int k = model.mJoints[j].custom_joint_index; + unsigned int dof = model.mCustomJoints[k]->mDoFCount; + + MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S); + + H.block(dof_index_i,dof_index_j,3,dof) = H_temp2; + H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose(); + } + } + } + } +} + +RBDL_DLLAPI void ForwardDynamics ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + VectorNd &QDDot, + std::vector *f_ext) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]); + + unsigned int i = 0; + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "QDot = " << QDot.transpose() << std::endl; + LOG << "Tau = " << Tau.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + else + model.X_base[i] = model.X_lambda[i]; + + model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i]; + + /* + LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl; + LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl; + LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl; + LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl; + LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl; + LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl; + */ + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.I[i].setSpatialMatrix (model.IA[i]); + + model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]); + + if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) { + LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl; + model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i]; + } + } + + // ClearLogOutput(); + + LOG << "--- first loop ---" << std::endl; + + for (i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.U[i] + * (model.U[i] / model.d[i]).transpose(); + + SpatialVector pa = model.pA[i] + + Ia * model.c[i] + + model.U[i] * model.u[i] / model.d[i]; + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() + += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() + += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() + * model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() + * model.multdof3_U[i]).inverse(); +#endif + Vector3d tau_temp(Tau.block(q_index,0,3,1)); + model.multdof3_u[i] = tau_temp + - model.multdof3_S[i].transpose() * model.pA[i]; + + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose(); + SpatialVector pa = model.pA[i] + + Ia + * model.c[i] + + model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_u[i]; +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() + += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + + model.pA[lambda].noalias() + += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] + += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() + << std::endl; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + model.mCustomJoints[kI]->U = + model.IA[i] * model.mCustomJoints[kI]->S; + +#ifdef EIGEN_CORE_H + model.mCustomJoints[kI]->Dinv + = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U).inverse().eval(); +#else + model.mCustomJoints[kI]->Dinv + = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U).inverse(); +#endif + VectorNd tau_temp(Tau.block(q_index,0,dofI,1)); + model.mCustomJoints[kI]->u = tau_temp + - model.mCustomJoints[kI]->S.transpose() * model.pA[i]; + + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - (model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); + SpatialVector pa = model.pA[i] + + Ia * model.c[i] + + (model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); + +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() + << std::endl; + } + } + } + + // ClearLogOutput(); + + model.a[0] = spatial_gravity * -1.; + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]); + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI=model.mCustomJoints[kI]->mDoFCount; + + VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv + * ( model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() + * model.a[i]); + + for(unsigned int z=0; z < dofI; ++z){ + QDDot[q_index+z] = qdd_temp[z]; + } + + model.a[i] = model.a[i] + + model.mCustomJoints[kI]->S * qdd_temp; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +RBDL_DLLAPI void ForwardDynamicsLagrangian ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &Tau, + VectorNd &QDDot, + Math::LinearSolver linear_solver, + std::vector *f_ext, + Math::MatrixNd *H, + Math::VectorNd *C) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + bool free_H = false; + bool free_C = false; + + if (H == NULL) { + H = new MatrixNd (MatrixNd::Zero(model.dof_count, model.dof_count)); + free_H = true; + } + + if (C == NULL) { + C = new VectorNd (VectorNd::Zero(model.dof_count)); + free_C = true; + } + + // we set QDDot to zero to compute C properly with the InverseDynamics + // method. + QDDot.setZero(); + + InverseDynamics (model, Q, QDot, QDDot, (*C), f_ext); + CompositeRigidBodyAlgorithm (model, Q, *H, false); + + LOG << "A = " << std::endl << *H << std::endl; + LOG << "b = " << std::endl << *C * -1. + Tau << std::endl; + + switch (linear_solver) { + case (LinearSolverPartialPivLU) : + QDDot = H->partialPivLu().solve (*C * -1. + Tau); + break; + case (LinearSolverColPivHouseholderQR) : + QDDot = H->colPivHouseholderQr().solve (*C * -1. + Tau); + break; + case (LinearSolverHouseholderQR) : + QDDot = H->householderQr().solve (*C * -1. + Tau); + break; + case (LinearSolverLLT) : + QDDot = H->llt().solve (*C * -1. + Tau); + break; + default: + LOG << "Error: Invalid linear solver: " << linear_solver << std::endl; + assert (0); + break; + } + + if (free_C) { + delete C; + } + + if (free_H) { + delete H; + } + + LOG << "x = " << QDDot << std::endl; +} + +RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, + const VectorNd &Q, + const VectorNd &Tau, + VectorNd &QDDot, + bool update_kinematics) { + + LOG << "Q = " << Q.transpose() << std::endl; + LOG << "---" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) { + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + jcalc_X_lambda_S (model, model.mJointUpdateOrder[i], Q); + + model.v_J[i].setZero(); + model.v[i].setZero(); + model.c[i].setZero(); + model.pA[i].setZero(); + model.I[i].setSpatialMatrix (model.IA[i]); + } + } + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + model.pA[i].setZero(); + } + + // ClearLogOutput(); + + if (update_kinematics) { + // Compute Articulate Body Inertias + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + // unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + model.U[i] = model.IA[i] * model.S[i]; + model.d[i] = model.S[i].dot(model.U[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] - + model.U[i] * (model.U[i] / model.d[i]).transpose(); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#endif + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; + +#ifdef EIGEN_CORE_H + model.multdof3_Dinv[i] = + (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval(); +#else + model.multdof3_Dinv[i] = + (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); +#endif + // LOG << "mCustomJoints[kI]->u[" << i << "] = " + //<< model.mCustomJoints[kI]->u[i].transpose() << std::endl; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_U[i].transpose()); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += + model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += + model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#endif + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + // unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; + +#ifdef EIGEN_CORE_H + model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U + ).inverse().eval(); +#else + model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose() + * model.mCustomJoints[kI]->U + ).inverse(); +#endif + // LOG << "mCustomJoints[kI]->u[" << i << "] = " + //<< model.mCustomJoints[kI]->u.transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialMatrix Ia = model.IA[i] + - ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->U.transpose()); +#ifdef EIGEN_CORE_H + model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() + * Ia + * model.X_lambda[i].toMatrix(); +#else + model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() + * Ia * model.X_lambda[i].toMatrix(); +#endif + } + } + } + } + + // compute articulated bias forces + for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int q_index = model.mJoints[i].q_index; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + + model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]); + // LOG << "u[" << i << "] = " << model.u[i] << std::endl; + unsigned int lambda = model.lambda[i]; + if (lambda != 0) { + SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i]; + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + + Vector3d tau_temp ( Tau[q_index], + Tau[q_index + 1], + Tau[q_index + 2]); + model.multdof3_u[i] = tau_temp + - model.multdof3_S[i].transpose()*model.pA[i]; + // LOG << "multdof3_u[" << i << "] = " + // << model.multdof3_u[i].transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialVector pa = model.pA[i] + + model.multdof3_U[i] + * model.multdof3_Dinv[i] + * model.multdof3_u[i]; + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += + model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + VectorNd tau_temp(Tau.block(q_index,0,dofI,1)); + + model.mCustomJoints[kI]->u = + tau_temp - ( model.mCustomJoints[kI]->S.transpose()* model.pA[i]); + // LOG << "mCustomJoints[kI]->u" + // << model.mCustomJoints[kI]->u.transpose() << std::endl; + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + SpatialVector pa = model.pA[i] + + ( model.mCustomJoints[kI]->U + * model.mCustomJoints[kI]->Dinv + * model.mCustomJoints[kI]->u); + +#ifdef EIGEN_CORE_H + model.pA[lambda].noalias() += + model.X_lambda[i].applyTranspose(pa); +#else + model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); +#endif + LOG << "pA[" << lambda << "] = " + << model.pA[lambda].transpose() << std::endl; + } + } + } + + // ClearLogOutput(); + + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + SpatialTransform X_lambda = model.X_lambda[i]; + + model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i]; + LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl; + + if (model.mJoints[i].mDoFCount == 1 + && model.mJoints[i].mJointType != JointTypeCustom) { + QDDot[q_index] = (1./model.d[i])*(model.u[i]-model.U[i].dot(model.a[i])); + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3 + && model.mJoints[i].mJointType != JointTypeCustom) { + Vector3d qdd_temp = + model.multdof3_Dinv[i] * (model.multdof3_u[i] + - model.multdof3_U[i].transpose()*model.a[i]); + + QDDot[q_index] = qdd_temp[0]; + QDDot[q_index + 1] = qdd_temp[1]; + QDDot[q_index + 2] = qdd_temp[2]; + model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp; + } else if (model.mJoints[i].mJointType == JointTypeCustom) { + unsigned int kI = model.mJoints[i].custom_joint_index; + unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; + + VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv + * ( model.mCustomJoints[kI]->u + - model.mCustomJoints[kI]->U.transpose() * model.a[i]); + + for(unsigned z = 0; z < dofI; ++z){ + QDDot[q_index+z] = qdd_temp[z]; + } + + model.a[i] = model.a[i] + + model.mCustomJoints[kI]->S * qdd_temp; + } + } + + LOG << "QDDot = " << QDDot.transpose() << std::endl; +} + +} /* namespace RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/Joint.cc b/3rdparty/rbdl/src/Joint.cc new file mode 100644 index 0000000..894d155 --- /dev/null +++ b/3rdparty/rbdl/src/Joint.cc @@ -0,0 +1,602 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Joint.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void jcalc ( + Model &model, + unsigned int joint_id, + const VectorNd &q, + const VectorNd &qdot + ) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + model.X_T[joint_id].E(0, 0), + model.X_T[joint_id].E(0, 1), + model.X_T[joint_id].E(0, 2), + + c * model.X_T[joint_id].E(1, 0) + s * model.X_T[joint_id].E(2, 0), + c * model.X_T[joint_id].E(1, 1) + s * model.X_T[joint_id].E(2, 1), + c * model.X_T[joint_id].E(1, 2) + s * model.X_T[joint_id].E(2, 2), + + -s * model.X_T[joint_id].E(1, 0) + c * model.X_T[joint_id].E(2, 0), + -s * model.X_T[joint_id].E(1, 1) + c * model.X_T[joint_id].E(2, 1), + -s * model.X_T[joint_id].E(1, 2) + c * model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + c * model.X_T[joint_id].E(0, 0) + -s * model.X_T[joint_id].E(2, 0), + c * model.X_T[joint_id].E(0, 1) + -s * model.X_T[joint_id].E(2, 1), + c * model.X_T[joint_id].E(0, 2) + -s * model.X_T[joint_id].E(2, 2), + + model.X_T[joint_id].E(1, 0), + model.X_T[joint_id].E(1, 1), + model.X_T[joint_id].E(1, 2), + + s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(2, 0), + s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(2, 1), + s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.v_J[joint_id][1] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + c * model.X_T[joint_id].E(0, 0) + s * model.X_T[joint_id].E(1, 0), + c * model.X_T[joint_id].E(0, 1) + s * model.X_T[joint_id].E(1, 1), + c * model.X_T[joint_id].E(0, 2) + s * model.X_T[joint_id].E(1, 2), + + -s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(1, 0), + -s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(1, 1), + -s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(1, 2), + + model.X_T[joint_id].E(2, 0), + model.X_T[joint_id].E(2, 1), + model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.v_J[joint_id][2] = qdot[model.mJoints[joint_id].q_index]; + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) { + SpatialTransform X_J = jcalc_XJ (model, joint_id, q); + jcalc_X_lambda_S(model, joint_id, q); + double Jqd = qdot[model.mJoints[joint_id].q_index]; + model.v_J[joint_id] = model.S[joint_id] * Jqd; + + Vector3d St = model.S[joint_id].block(0,0,3,1); + Vector3d c = X_J.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + c = St.cross(c); + c *= -Jqd * Jqd; + model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mDoFCount == 1 && + model.mJoints[joint_id].mJointType != JointTypeCustom) { + SpatialTransform X_J = jcalc_XJ (model, joint_id, q); + model.v_J[joint_id] = + model.S[joint_id] * qdot[model.mJoints[joint_id].q_index]; + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { + SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(), + Vector3d (0., 0., 0.)); + + model.multdof3_S[joint_id](0,0) = 1.; + model.multdof3_S[joint_id](1,1) = 1.; + model.multdof3_S[joint_id](2,2) = 1.; + + Vector3d omega (qdot[model.mJoints[joint_id].q_index], + qdot[model.mJoints[joint_id].q_index+1], + qdot[model.mJoints[joint_id].q_index+2]); + + model.v_J[joint_id] = SpatialVector ( + omega[0], omega[1], omega[2], + 0., 0., 0.); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + SpatialTransform X_J (Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d::Zero()); + + model.multdof3_S[joint_id](0,0) = -s1; + model.multdof3_S[joint_id](0,2) = 1.; + + model.multdof3_S[joint_id](1,0) = c1 * s2; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0.,0., 0.); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + SpatialTransform X_J (Matrix3d( + c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0, + -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0, + s1, -c1 * s0, c1 * c0 + ), + Vector3d::Zero()); + + model.multdof3_S[joint_id](0,0) = c2 * c1; + model.multdof3_S[joint_id](0,1) = s2; + + model.multdof3_S[joint_id](1,0) = -s2 * c1; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = s1; + model.multdof3_S[joint_id](2,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 + c2*qdot2*qdot1, + -c2*c1*qdot2*qdot0 + s2*s1*qdot1*qdot0 - s2*qdot2*qdot1, + c1*qdot1*qdot0, + 0., 0., 0. + ); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + SpatialTransform X_J (Matrix3d( + c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0, + -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0, + c1 * s0, - s1, c1 * c0), + Vector3d::Zero()); + + model.multdof3_S[joint_id](0,0) = s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = c2 * c1; + model.multdof3_S[joint_id](1,1) = -s2; + + model.multdof3_S[joint_id](2,0) = -s1; + model.multdof3_S[joint_id](2,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + c2*c1*qdot2*qdot0 - s2*s1*qdot1*qdot0 - s2*qdot2*qdot1, + -s2*c1*qdot2*qdot0 - c2*s1*qdot1*qdot0 - c2*qdot2*qdot1, + -c1*qdot1*qdot0, + 0., 0., 0. + ); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZXY) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + -s0 * s1 * s2 + c0 * c2, s0 * c2 + s1 * s2 * c0, -s2 * c1, + -s0 * c1, c0 * c1, s1, + s0 * s1 * c2 + s2 * c0, s0 * s2 - s1 * c0 * c2, c1 * c2 + ), + Vector3d::Zero()) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = -s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = s1; + model.multdof3_S[joint_id](1,2) = 1; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + (-c1 * c2 * qdot2 + s1 * s2 * qdot1) * qdot0 - s2 * qdot1 * qdot2, + c1 * qdot1 * qdot0, + (-s1 * c2 * qdot1 - c1 * s2 * qdot2) * qdot0 + c2 * qdot2 * qdot1, + 0., 0., 0. + ); + } else if(model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ){ + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + model.multdof3_S[joint_id](3,0) = 1.; + model.multdof3_S[joint_id](4,1) = 1.; + model.multdof3_S[joint_id](5,2) = 1.; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = + model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.); + model.X_lambda[joint_id].E = model.X_T[joint_id].E; + model.X_lambda[joint_id].r = model.X_T[joint_id].r + model.X_T[joint_id].E.transpose() * Vector3d (q0, q1, q2); + } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { + const Joint &joint = model.mJoints[joint_id]; + CustomJoint *custom_joint = + model.mCustomJoints[joint.custom_joint_index]; + custom_joint->jcalc (model, joint_id, q, qdot); + } else { + std::cerr << "Error: invalid joint type " << model.mJoints[joint_id].mJointType << " at id " << joint_id << std::endl; + abort(); + } +} + +RBDL_DLLAPI Math::SpatialTransform jcalc_XJ ( + Model &model, + unsigned int joint_id, + const Math::VectorNd &q) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mDoFCount == 1 + && model.mJoints[joint_id].mJointType != JointTypeCustom) { + if (model.mJoints[joint_id].mJointType == JointTypeRevolute) { + return Xrot (q[model.mJoints[joint_id].q_index], Vector3d ( + model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2] + )); + } else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) { + return Xtrans ( Vector3d ( + model.mJoints[joint_id].mJointAxes[0][3] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][4] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][5] + * q[model.mJoints[joint_id].q_index] + ) + ); + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical) { + SpatialTransform rot = Xrot( + q[model.mJoints[joint_id].q_index], Vector3d ( + model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2] + )); + SpatialTransform trans = Xtrans ( Vector3d ( + model.mJoints[joint_id].mJointAxes[0][3] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][4] + * q[model.mJoints[joint_id].q_index], + model.mJoints[joint_id].mJointAxes[0][5] + * q[model.mJoints[joint_id].q_index] + ) + ); + return rot * trans; + } + } + std::cerr << "Error: invalid joint type: " << model.mJoints[joint_id].mJointType << std::endl; + abort(); + return SpatialTransform(); +} + +RBDL_DLLAPI void jcalc_X_lambda_S ( + Model &model, + unsigned int joint_id, + const VectorNd &q + ) { + // exception if we calculate it for the root body + assert (joint_id > 0); + + if (model.mJoints[joint_id].mJointType == JointTypeRevoluteX) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + model.X_T[joint_id].E(0, 0), + model.X_T[joint_id].E(0, 1), + model.X_T[joint_id].E(0, 2), + + c * model.X_T[joint_id].E(1, 0) + s * model.X_T[joint_id].E(2, 0), + c * model.X_T[joint_id].E(1, 1) + s * model.X_T[joint_id].E(2, 1), + c * model.X_T[joint_id].E(1, 2) + s * model.X_T[joint_id].E(2, 2), + + -s * model.X_T[joint_id].E(1, 0) + c * model.X_T[joint_id].E(2, 0), + -s * model.X_T[joint_id].E(1, 1) + c * model.X_T[joint_id].E(2, 1), + -s * model.X_T[joint_id].E(1, 2) + c * model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.S[joint_id][0] = 1.0; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteY) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + c * model.X_T[joint_id].E(0, 0) + -s * model.X_T[joint_id].E(2, 0), + c * model.X_T[joint_id].E(0, 1) + -s * model.X_T[joint_id].E(2, 1), + c * model.X_T[joint_id].E(0, 2) + -s * model.X_T[joint_id].E(2, 2), + + model.X_T[joint_id].E(1, 0), + model.X_T[joint_id].E(1, 1), + model.X_T[joint_id].E(1, 2), + + s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(2, 0), + s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(2, 1), + s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.S[joint_id][1] = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeRevoluteZ) { + double s, c; + sincos (q[model.mJoints[joint_id].q_index], &s, &c); + + model.X_lambda[joint_id].E = Matrix3d ( + c * model.X_T[joint_id].E(0, 0) + s * model.X_T[joint_id].E(1, 0), + c * model.X_T[joint_id].E(0, 1) + s * model.X_T[joint_id].E(1, 1), + c * model.X_T[joint_id].E(0, 2) + s * model.X_T[joint_id].E(1, 2), + + -s * model.X_T[joint_id].E(0, 0) + c * model.X_T[joint_id].E(1, 0), + -s * model.X_T[joint_id].E(0, 1) + c * model.X_T[joint_id].E(1, 1), + -s * model.X_T[joint_id].E(0, 2) + c * model.X_T[joint_id].E(1, 2), + + model.X_T[joint_id].E(2, 0), + model.X_T[joint_id].E(2, 1), + model.X_T[joint_id].E(2, 2)); + + model.X_lambda[joint_id].r = model.X_T[joint_id].r; + + model.S[joint_id][2] = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeHelical){ + SpatialTransform XJ = jcalc_XJ (model, joint_id, q); + model.X_lambda[joint_id] = XJ * model.X_T[joint_id]; + // Set the joint axis + Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); + + model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0], + model.mJoints[joint_id].mJointAxes[0][1], + model.mJoints[joint_id].mJointAxes[0][2], + trans[0], trans[1], trans[2]); + } else if (model.mJoints[joint_id].mDoFCount == 1 + && model.mJoints[joint_id].mJointType != JointTypeCustom){ + model.X_lambda[joint_id] = + jcalc_XJ (model, joint_id, q) * model.X_T[joint_id]; + model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; + } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { + model.X_lambda[joint_id] = SpatialTransform ( + model.GetQuaternion (joint_id, q).toMatrix(), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = 1.; + model.multdof3_S[joint_id](1,1) = 1.; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZYX) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = -s1; + model.multdof3_S[joint_id](0,2) = 1.; + + model.multdof3_S[joint_id](1,0) = c1 * s2; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = - s2; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c2 * c1, s2 * c0 + c2 * s1 * s0, s2 * s0 - c2 * s1 * c0, + -s2 * c1, c2 * c0 - s2 * s1 * s0, c2 * s0 + s2 * s1 * c0, + s1, -c1 * s0, c1 * c0 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = c2 * c1; + model.multdof3_S[joint_id](0,1) = s2; + + model.multdof3_S[joint_id](1,0) = -s2 * c1; + model.multdof3_S[joint_id](1,1) = c2; + + model.multdof3_S[joint_id](2,0) = s1; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerYXZ ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c2 * c0 + s2 * s1 * s0, s2 * c1, -c2 * s0 + s2 * s1 * c0, + -s2 * c0 + c2 * s1 * s0, c2 * c1, s2 * s0 + c2 * s1 * c0, + c1 * s0, - s1, c1 * c0 + ), + Vector3d (0., 0., 0.)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = c2 * c1; + model.multdof3_S[joint_id](1,1) = -s2; + + model.multdof3_S[joint_id](2,0) = -s1; + model.multdof3_S[joint_id](2,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeEulerZXY ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + -s0 * s1 * s2 + c0 * c2, s0 * c2 + s1 * s2 * c0, -s2 * c1, + -s0 * c1, c0 * c1, s1, + s0 * s1 * c2 + s2 * c0, s0 * s2 - s1 * c0 * c2, c1 * c2 + ), + Vector3d::Zero()) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](0,0) = -s2 * c1; + model.multdof3_S[joint_id](0,1) = c2; + + model.multdof3_S[joint_id](1,0) = s1; + model.multdof3_S[joint_id](1,2) = 1; + + model.multdof3_S[joint_id](2,0) = c1 * c2; + model.multdof3_S[joint_id](2,1) = s2; + } else if (model.mJoints[joint_id].mJointType == JointTypeTranslationXYZ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d::Identity (3,3), + Vector3d (q0, q1, q2)) + * model.X_T[joint_id]; + + model.multdof3_S[joint_id](3,0) = 1.; + model.multdof3_S[joint_id](4,1) = 1.; + model.multdof3_S[joint_id](5,2) = 1.; + } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { + const Joint &joint = model.mJoints[joint_id]; + CustomJoint *custom_joint + = model.mCustomJoints[joint.custom_joint_index]; + + custom_joint->jcalc_X_lambda_S (model, joint_id, q); + } else { + std::cerr << "Error: invalid joint type!" << std::endl; + abort(); + } +} +} diff --git a/3rdparty/rbdl/src/Kinematics.cc b/3rdparty/rbdl/src/Kinematics.cc new file mode 100644 index 0000000..7b53db5 --- /dev/null +++ b/3rdparty/rbdl/src/Kinematics.cc @@ -0,0 +1,902 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +namespace RigidBodyDynamics { + +using namespace Math; + +RBDL_DLLAPI void UpdateKinematics( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + unsigned int i; + + model.a[0].setZero(); + + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, Q, QDot); + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + } else { + model.X_base[i] = model.X_lambda[i]; + model.v[i] = model.v_J[i]; + } + + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + + if(model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.a[i] + model.S[i] * QDDot[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp (QDDot[q_index], + QDDot[q_index + 1], + QDDot[q_index + 2]); + model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp; + } + } else { + unsigned int custom_index = model.mJoints[i].custom_joint_index; + const CustomJoint* custom_joint = model.mCustomJoints[custom_index]; + unsigned int joint_dof_count = custom_joint->mDoFCount; + + model.a[i] = model.a[i] + + ( model.mCustomJoints[custom_index]->S + * QDDot.block(q_index, 0, joint_dof_count, 1)); + } + } + + for (i = 1; i < model.mBodies.size(); i++) { + LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl; + } +} + +RBDL_DLLAPI void UpdateKinematicsCustom( + Model &model, + const VectorNd *Q, + const VectorNd *QDot, + const VectorNd *QDDot) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + unsigned int i; + + if (Q) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + VectorNd QDot_zero (VectorNd::Zero (model.q_size)); + + jcalc (model, i, (*Q), QDot_zero); + + if (lambda != 0) { + model.X_base[i] = model.X_lambda[i] * model.X_base[lambda]; + } else { + model.X_base[i] = model.X_lambda[i]; + } + } + } + + if (QDot) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int lambda = model.lambda[i]; + + jcalc (model, i, *Q, *QDot); + + if (lambda != 0) { + model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } else { + model.v[i] = model.v_J[i]; + model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]); + } + // LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl; + } + } + + // FIXME?: Changing QDot can alter body accelerations via c[] - update to QDot but not QDDot can result in incorrect a[] + if (QDDot) { + for (i = 1; i < model.mBodies.size(); i++) { + unsigned int q_index = model.mJoints[i].q_index; + + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i]; + } else { + model.a[i] = model.c[i]; + } + + if( model.mJoints[i].mJointType != JointTypeCustom){ + if (model.mJoints[i].mDoFCount == 1) { + model.a[i] = model.a[i] + model.S[i] * (*QDDot)[q_index]; + } else if (model.mJoints[i].mDoFCount == 3) { + Vector3d omegadot_temp ((*QDDot)[q_index], + (*QDDot)[q_index + 1], + (*QDDot)[q_index + 2]); + model.a[i] = model.a[i] + + model.multdof3_S[i] * omegadot_temp; + } + } else { + unsigned int k = model.mJoints[i].custom_joint_index; + + const CustomJoint* custom_joint = model.mCustomJoints[k]; + unsigned int joint_dof_count = custom_joint->mDoFCount; + + model.a[i] = model.a[i] + + ( (model.mCustomJoints[k]->S) + *(QDDot->block(q_index, 0, joint_dof_count, 1))); + } + } + } +} + +RBDL_DLLAPI Vector3d CalcBodyToBaseCoordinates ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_body_coordinates, + bool update_kinematics) { + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent; + + Matrix3d fixed_rotation = + model.mFixedBodies[fbody_id].mParentTransform.E.transpose(); + Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r; + + Matrix3d parent_body_rotation = model.X_base[parent_id].E.transpose(); + Vector3d parent_body_position = model.X_base[parent_id].r; + + return (parent_body_position + + (parent_body_rotation + * (fixed_position + fixed_rotation * (point_body_coordinates))) ); + } + + Matrix3d body_rotation = model.X_base[body_id].E.transpose(); + Vector3d body_position = model.X_base[body_id].r; + + return body_position + body_rotation * point_body_coordinates; +} + +RBDL_DLLAPI Vector3d CalcBaseToBodyCoordinates ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_base_coordinates, + bool update_kinematics) { + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + unsigned int parent_id = model.mFixedBodies[fbody_id].mMovableParent; + + Matrix3d fixed_rotation = model.mFixedBodies[fbody_id].mParentTransform.E; + Vector3d fixed_position = model.mFixedBodies[fbody_id].mParentTransform.r; + + Matrix3d parent_body_rotation = model.X_base[parent_id].E; + Vector3d parent_body_position = model.X_base[parent_id].r; + + return (fixed_rotation + * ( - fixed_position + - parent_body_rotation + * (parent_body_position - point_base_coordinates))); + } + + Matrix3d body_rotation = model.X_base[body_id].E; + Vector3d body_position = model.X_base[body_id].r; + + return body_rotation * (point_base_coordinates - body_position); +} + +RBDL_DLLAPI Matrix3d CalcBodyWorldOrientation( + Model &model, + const VectorNd &Q, + const unsigned int body_id, + bool update_kinematics) { + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + if (body_id >= model.fixed_body_discriminator) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + model.mFixedBodies[fbody_id].mBaseTransform = + model.mFixedBodies[fbody_id].mParentTransform + * model.X_base[model.mFixedBodies[fbody_id].mMovableParent]; + + return model.mFixedBodies[fbody_id].mBaseTransform.E; + } + + return model.X_base[body_id].E; +} + +RBDL_DLLAPI void CalcPointJacobian ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_position, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + SpatialTransform point_trans = + SpatialTransform (Matrix3d::Identity(), + CalcBodyToBaseCoordinates ( model, + Q, + body_id, + point_position, + false)); + + assert (G.rows() == 3 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + // e[j] is set to 1 if joint j contributes to the jacobian that we are + // computing. For all other joints the column will be zero. + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index, 3, 1) = + point_trans.apply( + model.X_base[j].inverse().apply( + model.S[j])).block(3,0,3,1); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 3, 3) = + ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.multdof3_S[j]).block(3,0,3,3); + } + } else { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0, q_index, 3, model.mCustomJoints[k]->mDoFCount) = + ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.mCustomJoints[k]->S).block( + 3,0,3,model.mCustomJoints[k]->mDoFCount); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI void CalcPointJacobian6D ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + const Vector3d &point_position, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + SpatialTransform point_trans = + SpatialTransform (Matrix3d::Identity(), + CalcBodyToBaseCoordinates (model, + Q, + body_id, + point_position, + false)); + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index, 6, 1) + = point_trans.apply( + model.X_base[j].inverse().apply( + model.S[j])).block(0,0,6,1); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0, q_index, 6, 3) + = ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.multdof3_S[j]).block(0,0,6,3); + } + } else { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0, q_index, 6, model.mCustomJoints[k]->mDoFCount) + = ((point_trans + * model.X_base[j].inverse()).toMatrix() + * model.mCustomJoints[k]->S).block( + 0,0,6,model.mCustomJoints[k]->mDoFCount); + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI void CalcBodySpatialJacobian ( + Model &model, + const VectorNd &Q, + unsigned int body_id, + MatrixNd &G, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // update the Kinematics if necessary + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, NULL, NULL); + } + + assert (G.rows() == 6 && G.cols() == model.qdot_size ); + + unsigned int reference_body_id = body_id; + + SpatialTransform base_to_body; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id + - model.fixed_body_discriminator; + + reference_body_id = model + .mFixedBodies[fbody_id] + .mMovableParent; + + base_to_body = model.mFixedBodies[fbody_id] + .mParentTransform + * model.X_base[reference_body_id]; + } else { + base_to_body = model.X_base[reference_body_id]; + } + + unsigned int j = reference_body_id; + + while (j != 0) { + unsigned int q_index = model.mJoints[j].q_index; + + if(model.mJoints[j].mJointType != JointTypeCustom){ + if (model.mJoints[j].mDoFCount == 1) { + G.block(0,q_index,6,1) = + base_to_body.apply( + model.X_base[j] + .inverse() + .apply(model.S[j]) + ); + } else if (model.mJoints[j].mDoFCount == 3) { + G.block(0,q_index,6,3) = + (base_to_body * model.X_base[j].inverse() + ).toMatrix() * model.multdof3_S[j]; + } + }else if(model.mJoints[j].mJointType == JointTypeCustom) { + unsigned int k = model.mJoints[j].custom_joint_index; + + G.block(0,q_index,6,model.mCustomJoints[k]->mDoFCount ) = + (base_to_body * model.X_base[j].inverse() + ).toMatrix() * model.mCustomJoints[k]->S; + } + + j = model.lambda[j]; + } +} + +RBDL_DLLAPI Vector3d CalcPointVelocity ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (model.IsBodyId(body_id) || body_id == 0); + assert (model.q_size == Q.size()); + assert (model.qdot_size == QDot.size()); + + // Reset the velocity of the root body + model.v[0].setZero(); + + // update the Kinematics with zero acceleration + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, &QDot, NULL); + } + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false); + } + + SpatialVector point_spatial_velocity = + SpatialTransform ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point).apply(model.v[reference_body_id]); + + return Vector3d ( + point_spatial_velocity[3], + point_spatial_velocity[4], + point_spatial_velocity[5] + ); +} + +RBDL_DLLAPI Math::SpatialVector CalcPointVelocity6D( + Model &model, + const Math::VectorNd &Q, + const Math::VectorNd &QDot, + unsigned int body_id, + const Math::Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + assert (model.IsBodyId(body_id) || body_id == 0); + assert (model.q_size == Q.size()); + assert (model.qdot_size == QDot.size()); + + // Reset the velocity of the root body + model.v[0].setZero(); + + // update the Kinematics with zero acceleration + if (update_kinematics) { + UpdateKinematicsCustom (model, &Q, &QDot, NULL); + } + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates(model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates(model, Q, reference_body_id, base_coords,false); + } + + return SpatialTransform ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point).apply(model.v[reference_body_id]); +} + +RBDL_DLLAPI Vector3d CalcPointAcceleration ( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) + UpdateKinematics (model, Q, QDot, QDDot); + + LOG << std::endl; + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false); + } + + SpatialTransform p_X_i ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point); + + SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]); + Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2] + ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5])); + SpatialVector p_a_i = p_X_i.apply(model.a[reference_body_id]); + + return Vector3d ( + p_a_i[3] + a_dash[0], + p_a_i[4] + a_dash[1], + p_a_i[5] + a_dash[2] + ); +} + +RBDL_DLLAPI SpatialVector CalcPointAcceleration6D( + Model &model, + const VectorNd &Q, + const VectorNd &QDot, + const VectorNd &QDDot, + unsigned int body_id, + const Vector3d &point_position, + bool update_kinematics) { + LOG << "-------- " << __func__ << " --------" << std::endl; + + // Reset the velocity of the root body + model.v[0].setZero(); + model.a[0].setZero(); + + if (update_kinematics) + UpdateKinematics (model, Q, QDot, QDDot); + + LOG << std::endl; + + unsigned int reference_body_id = body_id; + Vector3d reference_point = point_position; + + if (model.IsFixedBodyId(body_id)) { + unsigned int fbody_id = body_id - model.fixed_body_discriminator; + reference_body_id = model.mFixedBodies[fbody_id].mMovableParent; + Vector3d base_coords = + CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false); + reference_point = + CalcBaseToBodyCoordinates (model, Q, reference_body_id,base_coords,false); + } + + SpatialTransform p_X_i ( + CalcBodyWorldOrientation (model, Q, reference_body_id, false).transpose(), + reference_point); + + SpatialVector p_v_i = p_X_i.apply(model.v[reference_body_id]); + Vector3d a_dash = Vector3d (p_v_i[0], p_v_i[1], p_v_i[2] + ).cross(Vector3d (p_v_i[3], p_v_i[4], p_v_i[5])); + return (p_X_i.apply(model.a[reference_body_id]) + + SpatialVector (0, 0, 0, a_dash[0], a_dash[1], a_dash[2])); +} + +RBDL_DLLAPI bool InverseKinematics ( + Model &model, + const VectorNd &Qinit, + const std::vector& body_id, + const std::vector& body_point, + const std::vector& target_pos, + VectorNd &Qres, + double step_tol, + double lambda, + unsigned int max_iter) { + assert (Qinit.size() == model.q_size); + assert (body_id.size() == body_point.size()); + assert (body_id.size() == target_pos.size()); + + MatrixNd J = MatrixNd::Zero(3 * body_id.size(), model.qdot_size); + VectorNd e = VectorNd::Zero(3 * body_id.size()); + + Qres = Qinit; + + for (unsigned int ik_iter = 0; ik_iter < max_iter; ik_iter++) { + UpdateKinematicsCustom (model, &Qres, NULL, NULL); + + for (unsigned int k = 0; k < body_id.size(); k++) { + MatrixNd G (MatrixNd::Zero(3, model.qdot_size)); + CalcPointJacobian (model, Qres, body_id[k], body_point[k], G, false); + Vector3d point_base = + CalcBodyToBaseCoordinates (model, Qres, body_id[k], body_point[k], false); + LOG << "current_pos = " << point_base.transpose() << std::endl; + + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < model.qdot_size; j++) { + unsigned int row = k * 3 + i; + LOG << "i = " << i << " j = " << j << " k = " << k << " row = " + << row << " col = " << j << std::endl; + J(row, j) = G (i,j); + } + + e[k * 3 + i] = target_pos[k][i] - point_base[i]; + } + } + + LOG << "J = " << J << std::endl; + LOG << "e = " << e.transpose() << std::endl; + + // abort if we are getting "close" + if (e.norm() < step_tol) { + LOG << "Reached target close enough after " << ik_iter << " steps" << std::endl; + return true; + } + + MatrixNd JJTe_lambda2_I = + J * J.transpose() + + lambda*lambda * MatrixNd::Identity(e.size(), e.size()); + + VectorNd z (body_id.size() * 3); + z = JJTe_lambda2_I.colPivHouseholderQr().solve (e); + + LOG << "z = " << z << std::endl; + + VectorNd delta_theta = J.transpose() * z; + LOG << "change = " << delta_theta << std::endl; + + Qres = Qres + delta_theta; + LOG << "Qres = " << Qres.transpose() << std::endl; + + if (delta_theta.norm() < step_tol) { + LOG << "reached convergence after " << ik_iter << " steps" << std::endl; + return true; + } + + VectorNd test_1 (z.size()); + VectorNd test_res (z.size()); + + test_1.setZero(); + + for (unsigned int i = 0; i < z.size(); i++) { + test_1[i] = 1.; + + VectorNd test_delta = J.transpose() * test_1; + + test_res[i] = test_delta.squaredNorm(); + + test_1[i] = 0.; + } + + LOG << "test_res = " << test_res.transpose() << std::endl; + } + + return false; +} + +RBDL_DLLAPI +Vector3d CalcAngularVelocityfromMatrix ( + const Matrix3d &RotMat + ) { + double tol = 1e-12; + + Vector3d l = Vector3d (RotMat(2,1) - RotMat(1,2), RotMat(0,2) - RotMat(2,0), RotMat(1,0) - RotMat(0,1)); + if(l.norm() > tol){ + double preFactor = atan2(l.norm(),(RotMat.trace() - 1.0))/l.norm(); + return preFactor*l; + } + else if((RotMat(0,0)>0 && RotMat(1,1)>0 && RotMat(2,2) > 0) || l.norm() < tol){ + return Vector3dZero; + } + else{ + double PI = atan(1)*4.0; + return Vector3d (PI/2*(RotMat(0,0) + 1.0),PI/2*(RotMat(1,1) + 1.0),PI/2*(RotMat(2,2) + 1.0)); + } +} + +RBDL_DLLAPI +InverseKinematicsConstraintSet::InverseKinematicsConstraintSet() { + lambda = 1e-9; + num_steps = 0; + max_steps = 300; + step_tol = 1e-12; + constraint_tol = 1e-12; + num_constraints = 0; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddPointConstraint( + unsigned int body_id, + const Vector3d& body_point, + const Vector3d& target_pos + ) { + constraint_type.push_back (ConstraintTypePosition); + body_ids.push_back(body_id); + body_points.push_back(body_point); + target_positions.push_back(target_pos); + target_orientations.push_back(Matrix3d::Zero(3,3)); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 3; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddOrientationConstraint( + unsigned int body_id, + const Matrix3d& target_orientation + ) { + constraint_type.push_back (ConstraintTypeOrientation); + body_ids.push_back(body_id); + body_points.push_back(Vector3d::Zero()); + target_positions.push_back(Vector3d::Zero()); + target_orientations.push_back(target_orientation); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 3; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::AddFullConstraint( + unsigned int body_id, + const Vector3d& body_point, + const Vector3d& target_pos, + const Matrix3d& target_orientation + ) { + constraint_type.push_back (ConstraintTypeFull); + body_ids.push_back(body_id); + body_points.push_back(body_point); + target_positions.push_back(target_pos); + target_orientations.push_back(target_orientation); + constraint_row_index.push_back(num_constraints); + num_constraints = num_constraints + 6; + return constraint_type.size() - 1; +} + +RBDL_DLLAPI +unsigned int InverseKinematicsConstraintSet::ClearConstraints() +{ + for (unsigned int i =0; i < constraint_type.size(); i++){ + constraint_type.pop_back(); + body_ids.pop_back(); + body_points.pop_back(); + target_positions.pop_back(); + target_orientations.pop_back(); + num_constraints = 0; + } + return constraint_type.size(); +} + + +RBDL_DLLAPI +bool InverseKinematics ( + Model &model, + const Math::VectorNd &Qinit, + InverseKinematicsConstraintSet &CS, + Math::VectorNd &Qres + ) { + assert (Qinit.size() == model.q_size); + assert (Qres.size() == Qinit.size()); + + CS.J = MatrixNd::Zero(CS.num_constraints, model.qdot_size); + CS.e = VectorNd::Zero(CS.num_constraints); + + Qres = Qinit; + + for (CS.num_steps = 0; CS.num_steps < CS.max_steps; CS.num_steps++) { + UpdateKinematicsCustom (model, &Qres, NULL, NULL); + + for (unsigned int k = 0; k < CS.body_ids.size(); k++) { + CS.G = MatrixNd::Zero(6, model.qdot_size); + CalcPointJacobian6D (model, Qres, CS.body_ids[k], CS.body_points[k], CS.G, false); + Vector3d point_base = CalcBodyToBaseCoordinates (model, Qres, CS.body_ids[k], CS.body_points[k], false); + Matrix3d R = CalcBodyWorldOrientation(model, Qres, CS.body_ids[k], false); + Vector3d angular_velocity = R.transpose()*CalcAngularVelocityfromMatrix(R*CS.target_orientations[k].transpose()); + + //assign offsets and Jacobians + if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypeFull){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row + 3] = CS.target_positions[k][i] - point_base[i]; + CS.e[row] = angular_velocity[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row + 3, j) = CS.G (i + 3,j); + CS.J(row, j) = CS.G (i,j); + } + } + } + else if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypeOrientation){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row] = angular_velocity[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row, j) = CS.G (i,j); + } + } + } + else if (CS.constraint_type[k] == InverseKinematicsConstraintSet::ConstraintTypePosition){ + for (unsigned int i = 0; i < 3; i++){ + unsigned int row = CS.constraint_row_index[k] + i; + CS.e[row] = CS.target_positions[k][i] - point_base[i]; + for (unsigned int j = 0; j < model.qdot_size; j++) { + CS.J(row, j) = CS.G (i + 3,j); + } + } + } + else { + assert (false && !"Invalid inverse kinematics constraint"); + } + } + + LOG << "J = " << CS.J << std::endl; + LOG << "e = " << CS.e.transpose() << std::endl; + CS.error_norm = CS.e.norm(); + + // abort if we are getting "close" + if (CS.error_norm < CS.step_tol) { + LOG << "Reached target close enough after " << CS.num_steps << " steps" << std::endl; + return true; + } + + // // "task space" from puppeteer + // MatrixNd Ek = MatrixNd::Zero (CS.e.size(), CS.e.size()); + // + // for (size_t ei = 0; ei < CS.e.size(); ei ++) { + // // Ek(ei,ei) = CS.error_norm * CS.error_norm * 0.5 + CS.lambda; + // Ek(ei,ei) = CS.e[ei]*CS.e[ei] * 0.5 + CS.lambda; + // } + // + // MatrixNd JJT_Ek_wnI = CS.J * CS.J.transpose() + Ek; + // + // VectorNd delta_theta = CS.J.transpose() * JJT_Ek_wnI.colPivHouseholderQr().solve (CS.e); + // + // LOG << "change = " << delta_theta << std::endl; + + + // "joint space" from puppeteer + + double Ek = 0.; + + for (unsigned int ei = 0; ei < CS.e.size(); ei ++) { + Ek += CS.e[ei] * CS.e[ei] * 0.5; + } + + VectorNd ek = CS.J.transpose() * CS.e; + MatrixNd Wn = MatrixNd::Zero (Qres.size(), Qres.size()); + + assert (ek.size() == Qres.size()); + + for (unsigned int wi = 0; wi < Qres.size(); wi++) { + Wn(wi, wi) = ek[wi] * ek[wi] * 0.5 + CS.lambda; + // Wn(wi, wi) = Ek + 1.0e-3; + } + + MatrixNd A = CS.J.transpose() * CS.J + Wn; + VectorNd delta_theta = A.colPivHouseholderQr().solve(CS.J.transpose() * CS.e); + + Qres = Qres + delta_theta; + if (delta_theta.norm() < CS.step_tol) { + LOG << "reached convergence after " << CS.num_steps << " steps" << std::endl; + return true; + } + } + + return false; +} + +} diff --git a/3rdparty/rbdl/src/Logging.cc b/3rdparty/rbdl/src/Logging.cc new file mode 100644 index 0000000..9427980 --- /dev/null +++ b/3rdparty/rbdl/src/Logging.cc @@ -0,0 +1,14 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "rbdl/Logging.h" + +RBDL_DLLAPI std::ostringstream LogOutput; + +RBDL_DLLAPI void ClearLogOutput() { + LogOutput.str(""); +} diff --git a/3rdparty/rbdl/src/Model.cc b/3rdparty/rbdl/src/Model.cc new file mode 100644 index 0000000..8c0e7f9 --- /dev/null +++ b/3rdparty/rbdl/src/Model.cc @@ -0,0 +1,495 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include +#include + +#include "rbdl/rbdl_mathutils.h" + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Body.h" +#include "rbdl/Joint.h" + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +Model::Model() { + Body root_body; + Joint root_joint; + + Vector3d zero_position (0., 0., 0.); + SpatialVector zero_spatial (0., 0., 0., 0., 0., 0.); + + // structural information + lambda.push_back(0); + lambda_q.push_back(0); + mu.push_back(std::vector()); + dof_count = 0; + q_size = 0; + qdot_size = 0; + previously_added_body_id = 0; + + gravity = Vector3d (0., -9.81, 0.); + + // state information + v.push_back(zero_spatial); + a.push_back(zero_spatial); + + // Joints + mJoints.push_back(root_joint); + S.push_back (zero_spatial); + X_T.push_back(SpatialTransform()); + + v_J.push_back (zero_spatial); + c_J.push_back (zero_spatial); + + // Spherical joints + multdof3_S.push_back (Matrix63::Zero()); + multdof3_U.push_back (Matrix63::Zero()); + multdof3_Dinv.push_back (Matrix3d::Zero()); + multdof3_u.push_back (Vector3d::Zero()); + multdof3_w_index.push_back (0); + + // Dynamic variables + c.push_back(zero_spatial); + IA.push_back(SpatialMatrix::Identity()); + pA.push_back(zero_spatial); + U.push_back(zero_spatial); + + u = VectorNd::Zero(1); + d = VectorNd::Zero(1); + + f.push_back (zero_spatial); + SpatialRigidBodyInertia rbi(0., + Vector3d (0., 0., 0.), + Matrix3d::Zero(3,3)); + Ic.push_back (rbi); + I.push_back(rbi); + hc.push_back (zero_spatial); + hdotc.push_back (zero_spatial); + + // Bodies + X_lambda.push_back(SpatialTransform()); + X_base.push_back(SpatialTransform()); + + mBodies.push_back(root_body); + mBodyNameMap["ROOT"] = 0; + + fixed_body_discriminator = std::numeric_limits::max() / 2; +} + +unsigned int AddBodyFixedJoint ( + Model &model, + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &UNUSED(joint), + const Body &body, + std::string body_name) { + FixedBody fbody = FixedBody::CreateFromBody (body); + fbody.mMovableParent = parent_id; + fbody.mParentTransform = joint_frame; + + if (model.IsFixedBodyId(parent_id)) { + FixedBody fixed_parent = + model.mFixedBodies[parent_id - model.fixed_body_discriminator]; + + fbody.mMovableParent = fixed_parent.mMovableParent; + fbody.mParentTransform = joint_frame * fixed_parent.mParentTransform; + } + + // merge the two bodies + Body parent_body = model.mBodies[fbody.mMovableParent]; + parent_body.Join (fbody.mParentTransform, body); + model.mBodies[fbody.mMovableParent] = parent_body; + model.I[fbody.mMovableParent] = + SpatialRigidBodyInertia::createFromMassComInertiaC ( + parent_body.mMass, + parent_body.mCenterOfMass, + parent_body.mInertia); + + model.mFixedBodies.push_back (fbody); + + if (model.mFixedBodies.size() > + std::numeric_limits::max() - model.fixed_body_discriminator) { + std::cerr << "Error: cannot add more than " + << std::numeric_limits::max() + - model.mFixedBodies.size() + << " fixed bodies. You need to modify " + << "Model::fixed_body_discriminator for this." + << std::endl; + assert (0); + abort(); + } + + if (body_name.size() != 0) { + if (model.mBodyNameMap.find(body_name) != model.mBodyNameMap.end()) { + std::cerr << "Error: Body with name '" + << body_name + << "' already exists!" + << std::endl; + assert (0); + abort(); + } + model.mBodyNameMap[body_name] = model.mFixedBodies.size() + + model.fixed_body_discriminator - 1; + } + + return model.mFixedBodies.size() + model.fixed_body_discriminator - 1; +} + +unsigned int AddBodyMultiDofJoint ( + Model &model, + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + // Here we emulate multi DoF joints by simply adding nullbodies. This + // allows us to use fixed size elements for S,v,a, etc. which is very + // fast in Eigen. + unsigned int joint_count = 0; + if (joint.mJointType == JointType1DoF) + joint_count = 1; + else if (joint.mJointType == JointType2DoF) + joint_count = 2; + else if (joint.mJointType == JointType3DoF) + joint_count = 3; + else if (joint.mJointType == JointType4DoF) + joint_count = 4; + else if (joint.mJointType == JointType5DoF) + joint_count = 5; + else if (joint.mJointType == JointType6DoF) + joint_count = 6; + else if (joint.mJointType == JointTypeFloatingBase) + // no action required + {} + else { + std::cerr << "Error: Invalid joint type: " + << joint.mJointType + << std::endl; + + assert (0 && !"Invalid joint type!"); + } + + Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + null_body.mIsVirtual = true; + + unsigned int null_parent = parent_id; + SpatialTransform joint_frame_transform; + + if (joint.mJointType == JointTypeFloatingBase) { + null_parent = model.AddBody (parent_id, + joint_frame, + JointTypeTranslationXYZ, + null_body); + + return model.AddBody (null_parent, + SpatialTransform(), + JointTypeSpherical, + body, + body_name); + } + + Joint single_dof_joint; + unsigned int j; + + // Here we add multiple virtual bodies that have no mass or inertia for + // which each is attached to the model with a single degree of freedom + // joint. + for (j = 0; j < joint_count; j++) { + single_dof_joint = Joint (joint.mJointAxes[j]); + + if (single_dof_joint.mJointType == JointType1DoF) { + Vector3d rotation ( + joint.mJointAxes[j][0], + joint.mJointAxes[j][1], + joint.mJointAxes[j][2]); + Vector3d translation ( + joint.mJointAxes[j][3], + joint.mJointAxes[j][4], + joint.mJointAxes[j][5]); + + if (rotation == Vector3d (0., 0., 0.)) { + single_dof_joint = Joint (JointTypePrismatic, translation); + } else if (translation == Vector3d (0., 0., 0.)) { + single_dof_joint = Joint (JointTypeRevolute, rotation); + } + } + + // the first joint has to be transformed by joint_frame, all the + // others must have a null transformation + if (j == 0) + joint_frame_transform = joint_frame; + else + joint_frame_transform = SpatialTransform(); + + if (j == joint_count - 1) + // if we are at the last we must add the real body + break; + else { + // otherwise we just add an intermediate body + null_parent = model.AddBody (null_parent, + joint_frame_transform, + single_dof_joint, + null_body); + } + } + + return model.AddBody (null_parent, + joint_frame_transform, + single_dof_joint, + body, + body_name); +} + +unsigned int Model::AddBody( + const unsigned int parent_id, + const SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + assert (lambda.size() > 0); + assert (joint.mJointType != JointTypeUndefined); + + if (joint.mJointType == JointTypeFixed) { + previously_added_body_id = AddBodyFixedJoint (*this, + parent_id, + joint_frame, + joint, + body, + body_name); + + return previously_added_body_id; + } else if ( (joint.mJointType == JointTypeSpherical) + || (joint.mJointType == JointTypeEulerZYX) + || (joint.mJointType == JointTypeEulerXYZ) + || (joint.mJointType == JointTypeEulerYXZ) + || (joint.mJointType == JointTypeEulerZXY) + || (joint.mJointType == JointTypeTranslationXYZ) + || (joint.mJointType == JointTypeCustom) + ) { + // no action required + } else if (joint.mJointType != JointTypePrismatic + && joint.mJointType != JointTypeRevolute + && joint.mJointType != JointTypeRevoluteX + && joint.mJointType != JointTypeRevoluteY + && joint.mJointType != JointTypeRevoluteZ + && joint.mJointType != JointTypeHelical + ) { + previously_added_body_id = AddBodyMultiDofJoint (*this, + parent_id, + joint_frame, + joint, + body, + body_name); + return previously_added_body_id; + } + + // If we add the body to a fixed body we have to make sure that we + // actually add it to its movable parent. + unsigned int movable_parent_id = parent_id; + SpatialTransform movable_parent_transform; + + if (IsFixedBodyId(parent_id)) { + unsigned int fbody_id = parent_id - fixed_body_discriminator; + movable_parent_id = mFixedBodies[fbody_id].mMovableParent; + movable_parent_transform = mFixedBodies[fbody_id].mParentTransform; + } + + // structural information + lambda.push_back(movable_parent_id); + unsigned int lambda_q_last = mJoints[mJoints.size() - 1].q_index; + + if (mJoints[mJoints.size() - 1].mDoFCount > 0 + && mJoints[mJoints.size() - 1].mJointType != JointTypeCustom) { + lambda_q_last = lambda_q_last + mJoints[mJoints.size() - 1].mDoFCount; + } else if (mJoints[mJoints.size() - 1].mJointType == JointTypeCustom) { + // unsigned int custom_index = mJoints[mJoints.size() - 1].custom_joint_index; + lambda_q_last = lambda_q_last + + mCustomJoints[mCustomJoints.size() - 1]->mDoFCount; + } + + for (unsigned int i = 0; i < joint.mDoFCount; i++) { + lambda_q.push_back(lambda_q_last + i); + } + mu.push_back(std::vector()); + mu.at(movable_parent_id).push_back(mBodies.size()); + + // Bodies + X_lambda.push_back(SpatialTransform()); + X_base.push_back(SpatialTransform()); + mBodies.push_back(body); + + if (body_name.size() != 0) { + if (mBodyNameMap.find(body_name) != mBodyNameMap.end()) { + std::cerr << "Error: Body with name '" + << body_name + << "' already exists!" + << std::endl; + assert (0); + abort(); + } + mBodyNameMap[body_name] = mBodies.size() - 1; + } + + // state information + v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + + // Joints + unsigned int prev_joint_index = mJoints.size() - 1; + mJoints.push_back(joint); + + if (mJoints[prev_joint_index].mJointType != JointTypeCustom) { + mJoints[mJoints.size() - 1].q_index = + mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; + } else { + mJoints[mJoints.size() - 1].q_index = + mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; + } + + S.push_back (joint.mJointAxes[0]); + + // Joint state variables + v_J.push_back (joint.mJointAxes[0]); + c_J.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); + + // workspace for joints with 3 dof + multdof3_S.push_back (Matrix63::Zero(6,3)); + multdof3_U.push_back (Matrix63::Zero()); + multdof3_Dinv.push_back (Matrix3d::Zero()); + multdof3_u.push_back (Vector3d::Zero()); + multdof3_w_index.push_back (0); + + dof_count = dof_count + joint.mDoFCount; + + // update the w components of the Quaternions. They are stored at the end + // of the q vector + int multdof3_joint_counter = 0; + // int mCustomJoint_joint_counter = 0; + for (unsigned int i = 1; i < mJoints.size(); i++) { + if (mJoints[i].mJointType == JointTypeSpherical) { + multdof3_w_index[i] = dof_count + multdof3_joint_counter; + multdof3_joint_counter++; + } + } + + q_size = dof_count + + multdof3_joint_counter; + + qdot_size = qdot_size + joint.mDoFCount; + + // we have to invert the transformation as it is later always used from the + // child bodies perspective. + X_T.push_back(joint_frame * movable_parent_transform); + + // Dynamic variables + c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + IA.push_back(SpatialMatrix::Zero(6,6)); + pA.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.)); + + d = VectorNd::Zero (mBodies.size()); + u = VectorNd::Zero (mBodies.size()); + + f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.)); + + SpatialRigidBodyInertia rbi = + SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, + body.mCenterOfMass, + body.mInertia); + + Ic.push_back (rbi); + I.push_back (rbi); + hc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); + hdotc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); + + if (mBodies.size() == fixed_body_discriminator) { + std::cerr << "Error: cannot add more than " + << fixed_body_discriminator + << " movable bodies. You need to modify " + "Model::fixed_body_discriminator for this." + << std::endl; + assert (0); + abort(); + } + + previously_added_body_id = mBodies.size() - 1; + + mJointUpdateOrder.clear(); + + // update the joint order computation + std::vector > joint_types; + for (unsigned int i = 0; i < mJoints.size(); i++) { + joint_types.push_back( + std::pair (mJoints[i].mJointType,i)); + mJointUpdateOrder.push_back (mJoints[i].mJointType); + } + + mJointUpdateOrder.clear(); + JointType current_joint_type = JointTypeUndefined; + while (joint_types.size() != 0) { + current_joint_type = joint_types[0].first; + + std::vector >::iterator type_iter = + joint_types.begin(); + + while (type_iter != joint_types.end()) { + if (type_iter->first == current_joint_type) { + mJointUpdateOrder.push_back (type_iter->second); + type_iter = joint_types.erase (type_iter); + } else { + ++type_iter; + } + } + } + + // for (unsigned int i = 0; i < mJointUpdateOrder.size(); i++) { + // std::cout << "i = " << i << ": joint_id = " << mJointUpdateOrder[i] + // << " joint_type = " << mJoints[mJointUpdateOrder[i]].mJointType << std::endl; + // } + + return previously_added_body_id; +} + +unsigned int Model::AppendBody ( + const Math::SpatialTransform &joint_frame, + const Joint &joint, + const Body &body, + std::string body_name) { + return Model::AddBody (previously_added_body_id, + joint_frame, + joint, + body, + body_name); +} + +unsigned int Model::AddBodyCustomJoint ( + const unsigned int parent_id, + const Math::SpatialTransform &joint_frame, + CustomJoint *custom_joint, + const Body &body, + std::string body_name) { + Joint proxy_joint (JointTypeCustom, custom_joint->mDoFCount); + proxy_joint.custom_joint_index = mCustomJoints.size(); + //proxy_joint.mDoFCount = custom_joint->mDoFCount; //MM added. Otherwise + //model.q_size = 0, which is not good. + + mCustomJoints.push_back (custom_joint); + + unsigned int body_id = AddBody (parent_id, + joint_frame, + proxy_joint, + body, + body_name); + + return body_id; +} + diff --git a/3rdparty/rbdl/src/rbdl_mathutils.cc b/3rdparty/rbdl/src/rbdl_mathutils.cc new file mode 100644 index 0000000..7565c4b --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_mathutils.cc @@ -0,0 +1,319 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include +#include + +#include +#include + +#include +#include + +#include "rbdl/Logging.h" + +namespace RigidBodyDynamics { +namespace Math { + +RBDL_DLLAPI Vector3d Vector3dZero (0., 0., 0.); +RBDL_DLLAPI Matrix3d Matrix3dIdentity ( + 1., 0., 0., + 0., 1., 0., + 0., 0., 1 + ); +RBDL_DLLAPI Matrix3d Matrix3dZero ( + 0., 0., 0., + 0., 0., 0., + 0., 0., 0. + ); + +RBDL_DLLAPI SpatialVector SpatialVectorZero ( 0., 0., 0., 0., 0., 0.); + +RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x) { + x = VectorNd::Zero(x.size()); + + // We can only solve quadratic systems + assert (A.rows() == A.cols()); + + unsigned int n = A.rows(); + unsigned int pi; + + // the pivots + size_t *pivot = new size_t[n]; + + // temporary result vector which contains the pivoted result + VectorNd px(x); + + unsigned int i,j,k; + + for (i = 0; i < n; i++) + pivot[i] = i; + + for (j = 0; j < n; j++) { + pi = j; + double pv = fabs (A(j,pivot[j])); + + // LOG << "j = " << j << " pv = " << pv << std::endl; + // find the pivot + for (k = j; k < n; k++) { + double pt = fabs (A(j,pivot[k])); + if (pt > pv) { + pv = pt; + pi = k; + unsigned int p_swap = pivot[j]; + pivot[j] = pivot[pi]; + pivot[pi] = p_swap; + // LOG << "swap " << j << " with " << pi << std::endl; + // LOG << "j = " << j << " pv = " << pv << std::endl; + } + } + + for (i = j + 1; i < n; i++) { + if (fabs(A(j,pivot[j])) <= std::numeric_limits::epsilon()) { + std::cerr << "Error: pivoting failed for matrix A = " << std::endl; + std::cerr << "A = " << std::endl << A << std::endl; + std::cerr << "b = " << b << std::endl; + } + // assert (fabs(A(j,pivot[j])) > std::numeric_limits::epsilon()); + double d = A(i,pivot[j])/A(j,pivot[j]); + + b[i] -= b[j] * d; + + for (k = j; k < n; k++) { + A(i,pivot[k]) -= A(j,pivot[k]) * d; + } + } + } + + // warning: i is an unsigned int, therefore a for loop of the + // form "for (i = n - 1; i >= 0; i--)" might end up in getting an invalid + // value for i! + i = n; + do { + i--; + + for (j = i + 1; j < n; j++) { + px[i] += A(i,pivot[j]) * px[j]; + } + px[i] = (b[i] - px[i]) / A(i,pivot[i]); + + } while (i > 0); + + // Unswapping + for (i = 0; i < n; i++) { + x[pivot[i]] = px[i]; + } + + /* + LOG << "A = " << std::endl << A << std::endl; + LOG << "b = " << b << std::endl; + LOG << "x = " << x << std::endl; + LOG << "pivot = " << pivot[0] << " " << pivot[1] << " " << pivot[2] << std::endl; + std::cout << LogOutput.str() << std::endl; + */ + + delete[] pivot; + + return true; +} + +RBDL_DLLAPI void SpatialMatrixSetSubmatrix( + SpatialMatrix &dest, + unsigned int row, + unsigned int col, + const Matrix3d &matrix) { + assert (row < 2 && col < 2); + + dest(row*3,col*3) = matrix(0,0); + dest(row*3,col*3 + 1) = matrix(0,1); + dest(row*3,col*3 + 2) = matrix(0,2); + + dest(row*3 + 1,col*3) = matrix(1,0); + dest(row*3 + 1,col*3 + 1) = matrix(1,1); + dest(row*3 + 1,col*3 + 2) = matrix(1,2); + + dest(row*3 + 2,col*3) = matrix(2,0); + dest(row*3 + 2,col*3 + 1) = matrix(2,1); + dest(row*3 + 2,col*3 + 2) = matrix(2,2); +} + +RBDL_DLLAPI bool SpatialMatrixCompareEpsilon ( + const SpatialMatrix &matrix_a, + const SpatialMatrix &matrix_b, + double epsilon) { + assert (epsilon >= 0.); + unsigned int i, j; + + for (i = 0; i < 6; i++) { + for (j = 0; j < 6; j++) { + if (fabs(matrix_a(i,j) - matrix_b(i,j)) >= epsilon) { + std::cerr << "Expected:" + << std::endl << matrix_a << std::endl + << "but was" << std::endl + << matrix_b << std::endl; + return false; + } + } + } + + return true; +} + +RBDL_DLLAPI bool SpatialVectorCompareEpsilon ( + const SpatialVector &vector_a, + const SpatialVector &vector_b, + double epsilon) { + assert (epsilon >= 0.); + unsigned int i; + + for (i = 0; i < 6; i++) { + if (fabs(vector_a[i] - vector_b[i]) >= epsilon) { + std::cerr << "Expected:" + << std::endl << vector_a << std::endl + << "but was" << std::endl + << vector_b << std::endl; + return false; + } + } + + return true; +} + +RBDL_DLLAPI Matrix3d parallel_axis ( + const Matrix3d &inertia, + double mass, + const Vector3d &com) { + Matrix3d com_cross = VectorCrossMatrix (com); + + return inertia + mass * com_cross * com_cross.transpose(); +} + +RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &r) { + return SpatialMatrix ( + 1., 0., 0., 0., 0., 0., + 0., 1., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., r[2], -r[1], 1., 0., 0., + -r[2], 0., r[0], 0., 1., 0., + r[1], -r[0], 0., 0., 0., 1. + ); +} + +RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot) { + double s, c; + s = sin (xrot); + c = cos (xrot); + + return SpatialMatrix( + 1., 0., 0., 0., 0., 0., + 0., c, s, 0., 0., 0., + 0., -s, c, 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., c, s, + 0., 0., 0., 0., -s, c + ); +} + +RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot) { + double s, c; + s = sin (yrot); + c = cos (yrot); + + return SpatialMatrix( + c, 0., -s, 0., 0., 0., + 0., 1., 0., 0., 0., 0., + s, 0., c, 0., 0., 0., + 0., 0., 0., c, 0., -s, + 0., 0., 0., 0., 1., 0., + 0., 0., 0., s, 0., c + ); +} + +RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot) { + double s, c; + s = sin (zrot); + c = cos (zrot); + + return SpatialMatrix( + c, s, 0., 0., 0., 0., + -s, c, 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., c, s, 0., + 0., 0., 0., -s, c, 0., + 0., 0., 0., 0., 0., 1. + ); +} + +RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler ( + const Vector3d &displacement, + const Vector3d &zyx_euler) { + return Xrotz_mat(zyx_euler[0]) * Xroty_mat(zyx_euler[1]) * Xrotx_mat(zyx_euler[2]) * Xtrans_mat(displacement); +} + +RBDL_DLLAPI void SparseFactorizeLTL (Model &model, Math::MatrixNd &H) { + for (unsigned int i = 0; i < model.qdot_size; i++) { + for (unsigned int j = i + 1; j < model.qdot_size; j++) { + H(i,j) = 0.; + } + } + + for (unsigned int k = model.qdot_size; k > 0; k--) { + H(k - 1,k - 1) = sqrt (H(k - 1,k - 1)); + unsigned int i = model.lambda_q[k]; + while (i != 0) { + H(k - 1,i - 1) = H(k - 1,i - 1) / H(k - 1,k - 1); + i = model.lambda_q[i]; + } + + i = model.lambda_q[k]; + while (i != 0) { + unsigned int j = i; + while (j != 0) { + H(i - 1,j - 1) = H(i - 1,j - 1) - H(k - 1,i - 1) * H(k - 1, j - 1); + j = model.lambda_q[j]; + } + i = model.lambda_q[i]; + } + } +} + +RBDL_DLLAPI void SparseMultiplyHx (Model& UNUSED(model), Math::MatrixNd& UNUSED(L)) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseMultiplyLx (Model &UNUSED(model), Math::MatrixNd &UNUSED(L)) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseMultiplyLTx (Model &UNUSED(model), Math::MatrixNd &UNUSED(L)) { + assert (0 && !"Not yet implemented!"); +} + +RBDL_DLLAPI void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) { + for (unsigned int i = 1; i <= model.qdot_size; i++) { + unsigned int j = model.lambda_q[i]; + while (j != 0) { + x[i - 1] = x[i - 1] - L(i - 1,j - 1) * x[j - 1]; + j = model.lambda_q[j]; + } + x[i - 1] = x[i - 1] / L(i - 1,i - 1); + } +} + +RBDL_DLLAPI void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x) { + for (int i = model.qdot_size; i > 0; i--) { + x[i - 1] = x[i - 1] / L(i - 1,i - 1); + unsigned int j = model.lambda_q[i]; + while (j != 0) { + x[j - 1] = x[j - 1] - L(i - 1,j - 1) * x[i - 1]; + j = model.lambda_q[j]; + } + } +} + +} /* Math */ +} /* RigidBodyDynamics */ diff --git a/3rdparty/rbdl/src/rbdl_utils.cc b/3rdparty/rbdl/src/rbdl_utils.cc new file mode 100644 index 0000000..db0eef5 --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_utils.cc @@ -0,0 +1,354 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include "rbdl/rbdl_utils.h" + +#include "rbdl/rbdl_math.h" +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include +#include + +namespace RigidBodyDynamics { + +namespace Utils { + +using namespace std; +using namespace Math; + +string get_dof_name (const SpatialVector &joint_dof) { + if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) + return "RX"; + else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.)) + return "RY"; + else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.)) + return "RZ"; + else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.)) + return "TX"; + else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.)) + return "TY"; + else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.)) + return "TZ"; + + ostringstream dof_stream(ostringstream::out); + dof_stream << "custom_axis (" << joint_dof.transpose() << ")"; + return dof_stream.str(); +} + +string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) { + if (model.mBodies[body_id].mIsVirtual) { + // if there is not a unique child we do not know what to do... + if (model.mu[body_id].size() != 1) + return ""; + + return get_body_name (model, model.mu[body_id][0]); + } + + return model.GetBodyName(body_id); +} + +RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) { + stringstream result (""); + + unsigned int q_index = 0; + for (unsigned int i = 1; i < model.mBodies.size(); i++) { + if (model.mJoints[i].mDoFCount == 1) { + result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.S[i]) << endl; + q_index++; + } else { + for (unsigned int j = 0; j < model.mJoints[i].mDoFCount; j++) { + result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.mJoints[i].mJointAxes[j]) << endl; + q_index++; + } + } + } + + return result.str(); +} + +std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) { + stringstream result (""); + + for (int j = 0; j < indent; j++) + result << " "; + + result << get_body_name (model, body_index); + + if (body_index > 0) + result << " [ "; + + while (model.mBodies[body_index].mIsVirtual) { + if (model.mu[body_index].size() == 0) { + result << " end"; + break; + } else if (model.mu[body_index].size() > 1) { + cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " (name: " << model.GetBodyName(body_index) << ") has more than one child:" << endl; + for (unsigned int ci = 0; ci < model.mu[body_index].size(); ci++) { + cerr << " id: " << model.mu[body_index][ci] << " name: " << model.GetBodyName(model.mu[body_index][ci]) << endl; + } + abort(); + } + + result << get_dof_name(model.S[body_index]) << ", "; + + body_index = model.mu[body_index][0]; + } + + if (body_index > 0) + result << get_dof_name(model.S[body_index]) << " ]"; + result << endl; + + unsigned int child_index = 0; + for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) { + result << print_hierarchy (model, model.mu[body_index][child_index], indent + 1); + } + + // print fixed children + for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) { + if (model.mFixedBodies[fbody_index].mMovableParent == body_index) { + for (int j = 0; j < indent + 1; j++) + result << " "; + + result << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl; + } + } + + + return result.str(); +} + +RBDL_DLLAPI std::string GetModelHierarchy (const Model &model) { + stringstream result (""); + + result << print_hierarchy (model); + + return result.str(); +} + +RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model) { + stringstream result (""); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + UpdateKinematicsCustom (model, &Q, NULL, NULL); + + for (unsigned int body_id = 0; body_id < model.mBodies.size(); body_id++) { + std::string body_name = model.GetBodyName (body_id); + + if (body_name.size() == 0) + continue; + + Vector3d position = CalcBodyToBaseCoordinates (model, Q, body_id, Vector3d (0., 0., 0.), false); + + result << body_name << ": " << position.transpose() << endl; + } + + return result.str(); +} + +RBDL_DLLAPI void CalcCenterOfMass ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + const Math::VectorNd *qddot, + double &mass, + Math::Vector3d &com, + Math::Vector3d *com_velocity, + Math::Vector3d *com_acceleration, + Math::Vector3d *angular_momentum, + Math::Vector3d *change_of_angular_momentum, + bool update_kinematics) { + // If we want to compute com_acceleration or change of angular momentum + // we must have qddot provided. + assert( (com_acceleration == NULL && change_of_angular_momentum == NULL) + || (qddot != NULL) ); + + if (update_kinematics) + UpdateKinematicsCustom (model, &q, &qdot, qddot); + + for (size_t i = 1; i < model.mBodies.size(); i++) { + model.Ic[i] = model.I[i]; + model.hc[i] = model.Ic[i].toMatrix() * model.v[i]; + model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], model.Ic[i] * model.v[i]); + } + + if (qddot && (com_acceleration || change_of_angular_momentum)) { + for (size_t i = 1; i < model.mBodies.size(); i++) { + model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], model.Ic[i] * model.v[i]); + } + } + + SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3)); + SpatialVector htot (SpatialVector::Zero(6)); + SpatialVector hdot_tot (SpatialVector::Zero(6)); + + for (size_t i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]); + model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]); + } else { + Itot = Itot + model.X_lambda[i].applyTranspose (model.Ic[i]); + htot = htot + model.X_lambda[i].applyTranspose (model.hc[i]); + } + } + + if (qddot && (com_acceleration || change_of_angular_momentum)) { + for (size_t i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.hdotc[lambda] = model.hdotc[lambda] + model.X_lambda[i].applyTranspose (model.hdotc[i]); + } else { + hdot_tot = hdot_tot + model.X_lambda[i].applyTranspose (model.hdotc[i]); + } + } + } + + mass = Itot.m; + com = Itot.h / mass; + LOG << "mass = " << mass << " com = " << com.transpose() << " htot = " << htot.transpose() << std::endl; + + if (com_velocity) { + *com_velocity = Vector3d (htot[3] / mass, htot[4] / mass, htot[5] / mass); + } + + if (angular_momentum) { + htot = Xtrans (com).applyAdjoint (htot); + angular_momentum->set (htot[0], htot[1], htot[2]); + } + + if (com_acceleration) { + *com_acceleration = Vector3d (hdot_tot[3] / mass, hdot_tot[4] / mass, hdot_tot[5] / mass); + } + + if (change_of_angular_momentum) { + hdot_tot = Xtrans (com).applyAdjoint (hdot_tot); + change_of_angular_momentum->set (hdot_tot[0], hdot_tot[1], hdot_tot[2]); + } +} + +RBDL_DLLAPI void CalcZeroMomentPoint ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + const Math::VectorNd &qddot, + Vector3d* zmp, + const Math::Vector3d &normal, + const Math::Vector3d &UNUSED(point), + bool update_kinematics +) { + if (zmp == NULL) { + cerr << "ZMP (output) is 'NULL'!" << endl; + abort(); + } + + // update kinematics if required + // NOTE UpdateKinematics computes model.a[i] and model.v[i] required for + // change of momentum + if (update_kinematics) { + UpdateKinematicsCustom (model, &q, &qdot, &qddot); + } + + // compute change of momentum of each single body (same as in RNEA/InverseDynamics) + for (size_t i = 1; i < model.mBodies.size(); i++) { + model.Ic[i] = model.I[i]; + model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], 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)); + + // compute total change of momentum and CoM wrt to root body (idx = 0) + // by recursively summing up local change of momentum + for (size_t i = model.mBodies.size() - 1; i > 0; i--) { + unsigned int lambda = model.lambda[i]; + + if (lambda != 0) { + model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]); + model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]); + model.hdotc[lambda] = model.hdotc[lambda] + model.X_lambda[i].applyTranspose (model.hdotc[i]); + } else { + I_tot = I_tot + model.X_lambda[i].applyTranspose (model.Ic[i]); + h_tot = h_tot + model.X_lambda[i].applyTranspose (model.hc[i]); + hdot_tot = hdot_tot + model.X_lambda[i].applyTranspose (model.hdotc[i]); + } + } + + // compute CoM from mass and total inertia + const double mass = I_tot.m; + const Vector3d com = I_tot.h / mass; + + // project angular momentum onto CoM + SpatialTransform Xcom = Xtrans (com); + hdot_tot = Xcom.applyAdjoint (hdot_tot); + + // compute net external force at CoM by removing effects due to gravity + hdot_tot = hdot_tot - mass * SpatialVector (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]); + + // express total change of momentum in world coordinates + hdot_tot = Xcom.inverse().applyAdjoint (hdot_tot); + + // project purified change of momentum onto surface + // z = n x n_0 + // ------- + // n * f + Vector3d n_0 = hdot_tot.block<3,1>(0,0); + Vector3d f = hdot_tot.block<3,1>(3,0); + *zmp = normal.cross(n_0) / normal.dot(f); + + // double distance = (hdot_tot - point).dot(normal); + // zmp = hdot_tot - distance * normal; + + return; +} + +RBDL_DLLAPI double CalcPotentialEnergy ( + Model &model, + const Math::VectorNd &q, + bool update_kinematics) { + double mass; + Vector3d com; + CalcCenterOfMass ( + model, + q, + VectorNd::Zero (model.qdot_size), + NULL, + mass, + com, + NULL, + NULL, + NULL, + NULL, + update_kinematics); + + Vector3d g = - Vector3d (model.gravity[0], model.gravity[1], model.gravity[2]); + LOG << "pot_energy: " << " mass = " << mass << " com = " << com.transpose() << std::endl; + + return mass * com.dot(g); +} + +RBDL_DLLAPI double CalcKineticEnergy ( + Model &model, + const Math::VectorNd &q, + const Math::VectorNd &qdot, + bool update_kinematics) { + if (update_kinematics) + UpdateKinematicsCustom (model, &q, &qdot, NULL); + + double result = 0.; + + for (size_t i = 1; i < model.mBodies.size(); i++) { + result += 0.5 * model.v[i].transpose() * (model.I[i] * model.v[i]); + } + return result; +} + +} +} diff --git a/3rdparty/rbdl/src/rbdl_version.cc b/3rdparty/rbdl/src/rbdl_version.cc new file mode 100644 index 0000000..d09e22f --- /dev/null +++ b/3rdparty/rbdl/src/rbdl_version.cc @@ -0,0 +1,104 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2018 Martin Felis + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include + +#include +#include +#include +#include + +RBDL_DLLAPI int rbdl_get_api_version() { + static int compile_version = RBDL_API_VERSION; + return compile_version; +} + +RBDL_DLLAPI void rbdl_check_api_version(int version) { + int compile_version = rbdl_get_api_version(); + + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." + << compile_patch; + + int version_major = (version & 0xff0000) >> 16; + int version_minor = (version & 0x00ff00) >> 8; + int version_patch = (version & 0x0000ff); + + std::ostringstream link_version_string (""); + link_version_string << version_major << "." << version_minor << "." + << version_patch; + + if (version_major != compile_major) { + std::cerr << "Error: trying to link against an incompatible RBDL library." + << std::endl; + std::cerr << "The library version is: " << compile_version_string.str() + << " but rbdl_config.h is version " << link_version_string.str() + << std::endl; + abort(); + } else if (version_minor != compile_minor) { + std::cout << "Warning: RBDL library is of version " + << compile_version_string.str() << " but rbdl_config.h is from version " + << link_version_string.str() << std::endl; + } +} + +RBDL_DLLAPI void rbdl_print_version() { + int compile_version = rbdl_get_api_version(); + + int compile_major = (compile_version & 0xff0000) >> 16; + int compile_minor = (compile_version & 0x00ff00) >> 8; + int compile_patch = (compile_version & 0x0000ff); + + std::ostringstream compile_version_string(""); + compile_version_string << compile_major << "." << compile_minor << "." + << compile_patch; + + std::cout << "RBDL version:" << std::endl + << " API version : " << compile_version_string.str() << std::endl; + + if (std::string("unknown") != RBDL_BUILD_COMMIT) { + std::cout << " commit : " << RBDL_BUILD_COMMIT + << " (branch: " << RBDL_BUILD_BRANCH << ")" << std::endl + << " build type : " << RBDL_BUILD_TYPE << std::endl; + } + +#ifdef RBDL_ENABLE_LOGGING + std::cout << " logging : on (warning: reduces performance!)" << std::endl; +#else + std::cout << " logging : off" << std::endl; +#endif +#ifdef RBDL_USE_SIMPLE_MATH + std::cout << " simplemath : on (warning: reduces performance!)" + << std::endl; +#else + std::cout << " simplemath : off" << std::endl; +#endif + +#ifdef RBDL_BUILD_ADDON_LUAMODEL + std::cout << " LuaModel : on" << std::endl; +#else + std::cout << " LuaModel : off" << std::endl; +#endif +#ifdef RBDL_BUILD_ADDON_URDFREADER + std::cout << " URDFReader : on" << std::endl; +#else + std::cout << " URDFReader : off" << std::endl; +#endif + + std::string build_revision (RBDL_BUILD_COMMIT); + if (build_revision == "unknown") { + std::cout << std::endl + << "Version information incomplete: to enable version information re-build" + << std::endl + << "library from valid repository and enable RBDL_STORE_VERSION." + << std::endl; + } +} diff --git a/3rdparty/rbdl/tests/BodyTests.cc b/3rdparty/rbdl/tests/BodyTests.cc new file mode 100644 index 0000000..476fe2c --- /dev/null +++ b/3rdparty/rbdl/tests/BodyTests.cc @@ -0,0 +1,244 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Body.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +/* Tests whether the spatial inertia matches the one specified by its + * parameters + */ +TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + Matrix3d inertia_C ( + 1.4, 0., 0., + 0., 2., 0., + 0., 0., 3.); + + SpatialMatrix reference_inertia ( + 4.843, -1.98, -2.145, 0, -1.43, 1.32, + -1.98, 6.334, -1.716, 1.43, 0, -1.65, + -2.145, -1.716, 7.059, -1.32, 1.65, 0, + 0, 1.43, -1.32, 1.1, 0, 0, + -1.43, 0, 1.65, 0, 1.1, 0, + 1.32, -1.65, 0, 0, 0, 1.1 + ); + + // cout << LogOutput.str() << endl; + + SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia); + + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC); + CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); +} + +TEST ( TestBodyConstructorMassComInertia ) { + double mass = 1.1; + Vector3d com (1.5, 1.2, 1.3); + Matrix3d inertia_C ( + 8.286, -3.96, -4.29, + -3.96, 10.668, -3.432, + -4.29, -3.432, 11.118 + ); + + Body body (mass, com, inertia_C); + + SpatialMatrix reference_inertia ( + 11.729, -5.94, -6.435, 0, -1.43, 1.32, + -5.94, 15.002, -5.148, 1.43, 0, -1.65, + -6.435, -5.148, 15.177, -1.32, 1.65, 0, + 0, 1.43, -1.32, 1.1, 0, 0, + -1.43, 0, 1.65, 0, 1.1, 0, + 1.32, -1.65, 0, 0, 0, 1.1 + ); + + SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC); + CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); +} + +TEST ( TestBodyJoinNullbody ) { + ClearLogOutput(); + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + Body nullbody (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + + Body joined_body = body; + joined_body.Join (Xtrans(Vector3d (0., 0., 0.)), nullbody); + + SpatialRigidBodyInertia body_rbi (body.mMass, body.mCenterOfMass, body.mInertia); + SpatialRigidBodyInertia joined_body_rbi (joined_body.mMass, joined_body.mCenterOfMass, joined_body.mInertia); + + CHECK_EQUAL (1.1, body.mMass); + CHECK_ARRAY_CLOSE (body.mCenterOfMass.data(), joined_body.mCenterOfMass.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (body_rbi.toMatrix().data(), joined_body_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodies ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xtrans(Vector3d (0., 0., 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 9.918, 0, 0, 0, -0, 2.86, + 0, 9.062, 0, 0, 0, -0, + 0, 0, 12.98, -2.86, 0, 0, + 0, 0, -2.86, 2.2, 0, 0, + -0, 0, 0, 0, 2.2, 0, + 2.86, -0, 0, 0, 0, 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodiesDisplaced ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xtrans(Vector3d (1.1, 1.3, 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 9.918, 0, 0, 0, -0, 2.86, + 0, 9.062, 0, 0, 0, -0, + 0, 0, 12.98, -2.86, 0, 0, + 0, 0, -2.86, 2.2, 0, 0, + -0, 0, 0, 0, 2.2, 0, + 2.86, -0, 0, 0, 0, 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); + + +} + +TEST ( TestBodyJoinTwoBodiesRotated ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.3, 3.2)); + + Body body_joined (body_a); + body_joined.Join (Xrotx(-M_PI*0.5), body_b); + + SpatialRigidBodyInertia body_joined_rbi (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 6.2, 0., 0., 0., 0., 0., + 0., 6.4, 0., 0., 0., 0., + 0., 0., 6.6, 0., 0., 0., + 0., 0., 0., 2.2, 0., 0., + 0., 0., 0., 0., 2.2, 0., + 0., 0., 0., 0., 0., 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_EQUAL (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyJoinTwoBodiesRotatedAndTranslated ) { + ClearLogOutput(); + Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3)); + Body body_b(1.1, Vector3d (-1., 1., 0.), Vector3d (3.2, 3.1, 3.3)); + + Body body_joined (body_a); + body_joined.Join (Xrotz(M_PI*0.5) * Xtrans(Vector3d (1., 1., 0.)), body_b); + + SpatialRigidBodyInertia body_joined_rbi (body_joined.mMass, body_joined.mCenterOfMass, body_joined.mInertia); + + SpatialMatrix reference_inertia ( + 6.2, 0., 0., 0., 0., 0., + 0., 6.4, 0., 0., 0., 0., + 0., 0., 6.6, 0., 0., 0., + 0., 0., 0., 2.2, 0., 0., + 0., 0., 0., 0., 2.2, 0., + 0., 0., 0., 0., 0., 2.2 + ); + + CHECK_EQUAL (2.2, body_joined.mMass); + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); +} + +TEST ( TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + + SpatialVector mv (1.1, 1.2, 1.3, 1.4, 1.5, 1.6); + SpatialVector fv_matrix = rbi.toMatrix() * mv; + SpatialVector fv_rbi = rbi * mv; + + CHECK_ARRAY_CLOSE ( + fv_matrix.data(), + fv_rbi.data(), + 6, + TEST_PREC + ); +} + +TEST ( TestBodyConstructorSpatialRigidBodyInertia ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + SpatialMatrix spatial_inertia = rbi.toMatrix(); + + CHECK_ARRAY_CLOSE ( + spatial_inertia.data(), + rbi.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST ( TestBodyConstructorCopySpatialRigidBodyInertia ) { + Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.)); + + SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia( + body.mMass, + body.mCenterOfMass * body.mMass, + body.mInertia + ); + + SpatialRigidBodyInertia rbi_from_matrix; + rbi_from_matrix.createFromMatrix (rbi.toMatrix()); + + // cout << "Spatial Inertia = " << endl << spatial_inertia << endl; + // cout << "rbi = " << endl << rbi.toMatrix() << endl; + // cout << "rbi.m = " << rbi.m << endl; + // cout << "rbi.h = " << rbi.h.transpose() << endl; + // cout << "rbi.I = " << endl << rbi.I << endl; + + CHECK_ARRAY_CLOSE ( + rbi.toMatrix().data(), + rbi_from_matrix.toMatrix().data(), + 36, + TEST_PREC + ); +} diff --git a/3rdparty/rbdl/tests/CMakeLists.txt b/3rdparty/rbdl/tests/CMakeLists.txt new file mode 100644 index 0000000..b6d681e --- /dev/null +++ b/3rdparty/rbdl/tests/CMakeLists.txt @@ -0,0 +1,76 @@ +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) + +# Needed for UnitTest++ +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake ) + +# Look for unittest++ +FIND_PACKAGE (UnitTest++ REQUIRED) +INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR}) + +SET ( TESTS_SRCS + main.cc + MathTests.cc + SpatialAlgebraTests.cc + MultiDofTests.cc + KinematicsTests.cc + BodyTests.cc + ModelTests.cc + FloatingBaseTests.cc + CalcVelocitiesTests.cc + CalcAccelerationsTests.cc + DynamicsTests.cc + InverseDynamicsTests.cc + CompositeRigidBodyTests.cc + ImpulsesTests.cc + TwolegModelTests.cc + ContactsTests.cc + UtilsTests.cc + SparseFactorizationTests.cc + CustomJointSingleBodyTests.cc + CustomJointMultiBodyTests.cc + CustomConstraintsTests.cc + InverseKinematicsTests.cc + LoopConstraintsTests.cc + ScrewJointTests.cc + ForwardDynamicsConstraintsExternalForces.cc + ) + +INCLUDE_DIRECTORIES ( ../src/ ) + +SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runtests + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + ) + +ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} ) + +SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES + LINKER_LANGUAGE CXX + OUTPUT_NAME runtests + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS OFF + ) + +SET (RBDL_LIBRARY rbdl) +IF (RBDL_BUILD_STATIC) + SET (RBDL_LIBRARY rbdl-static) +ENDIF (RBDL_BUILD_STATIC) + +TARGET_LINK_LIBRARIES ( rbdl_tests + ${UNITTEST++_LIBRARY} + ${RBDL_LIBRARY} + ) + +OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) + +IF (RUN_AUTOMATIC_TESTS) + ADD_CUSTOM_COMMAND (TARGET rbdl_tests + POST_BUILD + COMMAND ./runtests + COMMENT "Running automated tests..." + ) +ENDIF (RUN_AUTOMATIC_TESTS) diff --git a/3rdparty/rbdl/tests/CalcAccelerationsTests.cc b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc new file mode 100644 index 0000000..644cec0 --- /dev/null +++ b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc @@ -0,0 +1,235 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) { + QDDot[0] = 1.; + ref_body_id = body_a_id; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE(1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE(0., point_acceleration[2], TEST_PREC); + + // LOG << "Point accel = " << point_acceleration << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimpleRotated) { + Q[0] = 0.5 * M_PI; + + ref_body_id = body_a_id; + QDDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + // LOG << "Point accel = " << point_acceleration << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) { + ref_body_id = 1; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + ClearLogOutput(); + + // if we are on the other side we should have the opposite value + point_position = Vector3d (-1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatedBaseSimple) { + // rotated first joint + + ref_body_id = 1; + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE(-1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + point_position = Vector3d (-1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 1., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatingBodyB) { + // rotating second joint, point at third body + + ref_body_id = 3; + QDot[1] = 1.; + point_position = Vector3d (1., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + + // move it a bit further up (acceleration should stay the same) + point_position = Vector3d (1., 1., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointBodyOrigin) { + // rotating second joint, point at third body + + QDot[0] = 1.; + + ref_body_id = body_b_id; + point_position = Vector3d (0., 0., 0.); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); + CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestAccelerationLinearFuncOfQddot) { + // rotating second joint, point at third body + + QDot[0] = 1.1; + QDot[1] = 1.3; + QDot[2] = 1.5; + + ref_body_id = body_c_id; + point_position = Vector3d (1., 1., 1.); + + VectorNd qddot_1 = VectorNd::Zero (model->dof_count); + VectorNd qddot_2 = VectorNd::Zero (model->dof_count); + VectorNd qddot_0 = VectorNd::Zero (model->dof_count); + + qddot_1[0] = 0.1; + qddot_1[1] = 0.2; + qddot_1[2] = 0.3; + + qddot_2[0] = 0.32; + qddot_2[1] = -0.1; + qddot_2[2] = 0.53; + + Vector3d acc_1 = CalcPointAcceleration(*model, Q, QDot, qddot_1, ref_body_id, point_position); + Vector3d acc_2 = CalcPointAcceleration(*model, Q, QDot, qddot_2, ref_body_id, point_position); + + MatrixNd G = MatrixNd::Zero (3, model->dof_count); + CalcPointJacobian (*model, Q, ref_body_id, point_position, G, true); + + VectorNd net_acc = G * (qddot_1 - qddot_2); + + Vector3d acc_new = acc_1 - acc_2; + + CHECK_ARRAY_CLOSE (net_acc.data(), acc_new.data(), 3, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithUpdateKinematics ) { + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + ClearLogOutput(); + Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), true); + + CHECK_ARRAY_CLOSE (Vector3d (0., -9.81, 0.), accel.data(), 3, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithoutUpdateKinematics ) { + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + //ClearLogOutput(); + Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), false); + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), accel.data(), 3, TEST_PREC); + // cout << LogOutput.str() << endl; + // cout << accel.transpose() << endl; +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJoint) { + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + unsigned int fixed_body_id = model->AddBody (body_c_id, Xtrans (Vector3d (1., -1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + + QDot[0] = 1.; + point_position = Vector3d (0., 0., 0.); + Vector3d point_acceleration_reference = CalcPointAcceleration (*model, Q, QDot, QDDot, body_c_id, Vector3d (1., -1., 0.)); + + ClearLogOutput(); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (point_acceleration_reference.data(), + point_acceleration.data(), + 3, + TEST_PREC); +} + +TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJointRotatedTransform) { + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + SpatialTransform fixed_transform = Xtrans (Vector3d (1., -1., 0.)) * Xrotz(M_PI * 0.5); + unsigned int fixed_body_id = model->AddBody (body_c_id, fixed_transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + QDot[0] = 1.; + point_position = Vector3d (0., 0., 0.); + ClearLogOutput(); + Vector3d point_acceleration_reference = CalcPointAcceleration (*model, Q, QDot, QDDot, body_c_id, Vector3d (1., 1., 0.)); + // cout << LogOutput.str() << endl; + + // cout << "Point position = " << CalcBodyToBaseCoordinates (*model, Q, fixed_body_id, Vector3d (0., 0., 0.)).transpose() << endl; + // cout << "Point position_ref = " << CalcBodyToBaseCoordinates (*model, Q, body_c_id, Vector3d (1., 1., 0.)).transpose() << endl; + + ClearLogOutput(); + point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (point_acceleration_reference.data(), + point_acceleration.data(), + 3, + TEST_PREC); +} + diff --git a/3rdparty/rbdl/tests/CalcVelocitiesTests.cc b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc new file mode 100644 index 0000000..059a16c --- /dev/null +++ b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc @@ -0,0 +1,215 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ModelVelocitiesFixture { + ModelVelocitiesFixture () { + ClearLogOutput(); + model = new Model; + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_c ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + body_c_id = model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero(3); + point_velocity = Vector3d::Zero(3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~ModelVelocitiesFixture () { + delete model; + } + Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + Body body_a, body_b, body_c; + Joint joint_a, joint_b, joint_c; + + VectorNd Q; + VectorNd QDot; + + Vector3d point_position, point_velocity; +}; + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointSimple) { + ref_body_id = 1; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(0., point_velocity[0], TEST_PREC); + CHECK_CLOSE(1., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseSimple) { + // rotated first joint + + ref_body_id = 1; + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(-1., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); + + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBodyB) { + // rotating second joint, point at third body + + ref_body_id = 3; + QDot[1] = 1.; + point_position = Vector3d (1., 0., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBaseXAxis) { + // also rotate the first joint and take a point that is + // on the X direction + + ref_body_id = 3; + QDot[0] = 1.; + QDot[1] = 1.; + point_position = Vector3d (1., -1., 0.); + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 2., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseXAxis) { + // perform the previous test with the first joint rotated by pi/2 + // upwards + ClearLogOutput(); + + ref_body_id = 3; + point_position = Vector3d (1., -1., 0.); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + QDot[1] = 1.; + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE(-2., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); +} + +TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointBodyOrigin) { + // Checks whether the computation is also correct for points at the origin + // of a body + + ref_body_id = body_b_id; + point_position = Vector3d (0., 0., 0.); + + Q[0] = 0.; + QDot[0] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE( 0., point_velocity[0], TEST_PREC); + CHECK_CLOSE( 1., point_velocity[1], TEST_PREC); + CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); +} + +TEST ( FixedJointCalcPointVelocity ) { + // the standard modeling using a null body + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + QDot[0] = 1.; + + ClearLogOutput(); + Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.)); + // cout << LogOutput.str() << endl; + Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point0_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), point1_velocity.data(), 3, TEST_PREC); +} + +TEST ( FixedJointCalcPointVelocityRotated ) { + // the standard modeling using a null body + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xtrans (Vector3d (1., 0., 0.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body, "fixed_body"); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + + ClearLogOutput(); + Vector3d point0_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (0., 0., 0.)); + // cout << LogOutput.str() << endl; + Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (-1., 0., 0.).data(), point0_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (-2., 0., 0.).data(), point1_velocity.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc new file mode 100644 index 0000000..6ce896c --- /dev/null +++ b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc @@ -0,0 +1,261 @@ +#include + +#include + +#include "rbdl/Logging.h" +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct CompositeRigidBodyFixture { + CompositeRigidBodyFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~CompositeRigidBodyFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsFloatingBase) { + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd TauInv = VectorNd::Constant ((size_t) model->dof_count, 0.); + + MatrixNd H = MatrixNd::Constant ((size_t) model->dof_count, (size_t) model->dof_count, 0.); + VectorNd C = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_zero = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_crba = VectorNd::Constant ((size_t) model->dof_count, 0.); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + ClearLogOutput(); + CompositeRigidBodyAlgorithm (*model, Q, H); + // cout << LogOutput.str() << endl; + + InverseDynamics (*model, Q, QDot, QDDot_zero, C); + + CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba)); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoF) { + MatrixNd H = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + VectorNd C = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_zero = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot_crba = VectorNd::Constant ((size_t) model->dof_count, 0.); + + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + Q[ 6] = -1.3; + Q[ 7] = -1.4; + Q[ 8] = -1.5; + Q[ 9] = -0.3; + Q[10] = -0.4; + Q[11] = -0.5; + + QDot[ 0] = 1.1; + QDot[ 1] = -1.2; + QDot[ 2] = 1.3; + QDot[ 3] = -0.1; + QDot[ 4] = 0.2; + QDot[ 5] = -0.3; + QDot[ 6] = -1.1; + QDot[ 7] = 1.2; + QDot[ 8] = -1.3; + QDot[ 9] = 0.1; + QDot[10] = -0.2; + QDot[11] = 0.3; + + Tau[ 0] = -1.1; + Tau[ 1] = 1.2; + Tau[ 2] = -1.3; + Tau[ 3] = 1.1; + Tau[ 4] = -1.2; + Tau[ 5] = 1.3; + Tau[ 6] = 0.1; + Tau[ 7] = -0.2; + Tau[ 8] = 0.3; + Tau[ 9] = -0.1; + Tau[10] = 0.2; + Tau[11] = -0.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + ClearLogOutput(); + CompositeRigidBodyAlgorithm (*model, Q, H); + // cout << LogOutput.str() << endl; + InverseDynamics (*model, Q, QDot, QDDot_zero, C); + + CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba)); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoFInverseDynamics) { + MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + Q[ 6] = -1.3; + Q[ 7] = -1.4; + Q[ 8] = -1.5; + Q[ 9] = -0.3; + Q[10] = -0.4; + Q[11] = -0.5; + + QDot.setZero(); + + assert (model->dof_count == 12); + + UpdateKinematicsCustom (*model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (*model, Q, H_crba, false); + + VectorNd H_col = VectorNd::Zero (model->dof_count); + VectorNd QDDot_zero = VectorNd::Zero (model->dof_count); + + unsigned int i; + for (i = 0; i < model->dof_count; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (model->dof_count); + delta_a[i] = 1.; + // cout << delta_a << endl; + + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, QDDot_zero, id_zero); + + H_col = id_delta - id_zero; + // cout << "H_col = " << H_col << endl; + H_id.block<12, 1>(0, i) = H_col; + } + + // cout << "H (crba) = " << endl << H_crba << endl; + // cout << "H (id) = " << endl << H_id << endl; + + CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC); +} + +TEST_FIXTURE(FixedBase6DoF, TestCRBAFloatingBase12DoFInverseDynamics) { + MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count); + + Q[ 0] = 1.1; + Q[ 1] = 1.2; + Q[ 2] = 1.3; + Q[ 3] = 0.1; + Q[ 4] = 0.2; + Q[ 5] = 0.3; + + QDot.setZero(); + + assert (model->dof_count == 6); + + UpdateKinematicsCustom (*model, &Q, NULL, NULL); + CompositeRigidBodyAlgorithm (*model, Q, H_crba, false); + + VectorNd H_col = VectorNd::Zero (model->dof_count); + VectorNd QDDot_zero = VectorNd::Zero (model->dof_count); + + unsigned int i; + for (i = 0; i < 6; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (model->dof_count); + delta_a[i] = 1.; + + ClearLogOutput(); + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (model->dof_count); + InverseDynamics (*model, Q, QDot, QDDot_zero, id_zero); + + H_col.setZero(); + H_col = id_delta - id_zero; + + H_id.block<6, 1>(0, i) = H_col; + } + + CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC); +} + +TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsSpherical) { + Body base_body(1., Vector3d (0., 0., 0.), Vector3d (1., 2., 3.)); + + model->AddBody(0, SpatialTransform(), Joint(JointTypeSpherical), base_body); + VectorNd Q = VectorNd::Constant ((size_t) model->q_size, 0.); + model->SetQuaternion (1, Quaternion(), Q); + MatrixNd H = MatrixNd::Constant ((size_t) model->qdot_size, (size_t) model->qdot_size, 0.); + CompositeRigidBodyAlgorithm (*model, Q, H, true); + + Matrix3d H_ref ( + 1., 0., 0., + 0., 2., 0., + 0., 0., 3. + ); + + CHECK_ARRAY_CLOSE (H_ref.data(), H.data(), 9, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/ContactsTests.cc b/3rdparty/rbdl/tests/ContactsTests.cc new file mode 100644 index 0000000..1ef580a --- /dev/null +++ b/3rdparty/rbdl/tests/ContactsTests.cc @@ -0,0 +1,724 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +#include "Fixtures.h" +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + +struct FixedBase6DoF9DoF { + FixedBase6DoF9DoF () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body 1 (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, child); + + // child body (3 DoF) + child_2 = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_2_id = model->AddBody (child_id, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, child_2); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + contact_body_id = child_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF9DoF () { + delete model; + } + Model *model; + + unsigned int base_id, child_id, child_2_id; + + Body base, child, child_2; + + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; + + unsigned int contact_body_id; + Vector3d contact_point; + Vector3d contact_normal; + ConstraintSet constraint_set; +}; + +// +// ForwardDynamicsConstraintsDirect +// +TEST ( TestForwardDynamicsConstraintsDirectSimple ) { + Model model; + model.gravity = Vector3d (0., -9.81, 0.); + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[1] = 1.; + QDot[0] = 1.; + QDot[3] = -1.; + + unsigned int contact_body_id = base_body_id; + Vector3d contact_point ( 0., -1., 0.); + + ConstraintSet constraint_set; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z"); + + constraint_set.Bind (model); + + ClearLogOutput(); + +// cout << constraint_set.acceleration.transpose() << endl; + ForwardDynamicsConstraintsDirect (model, Q, QDot, Tau, constraint_set, QDDot); + +// cout << "A = " << endl << constraint_set.A << endl << endl; +// cout << "H = " << endl << constraint_set.H << endl << endl; +// cout << "b = " << endl << constraint_set.b << endl << endl; +// cout << "x = " << endl << constraint_set.x << endl << endl; +// cout << constraint_set.b << endl; +// cout << "QDDot = " << QDDot.transpose() << endl; + + Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 0., 0.).data(), + point_acceleration.data(), + 3, + TEST_PREC + ); + + // cout << "LagrangianSimple Logoutput Start" << endl; + // cout << LogOutput.str() << endl; + // cout << "LagrangianSimple Logoutput End" << endl; +} + +TEST ( TestForwardDynamicsConstraintsDirectMoving ) { + Model model; + model.gravity = Vector3d (0., -9.81, 0.); + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[0] = 0.1; + Q[1] = 0.2; + Q[2] = 0.3; + Q[3] = 0.4; + Q[4] = 0.5; + Q[5] = 0.6; + QDot[0] = 1.1; + QDot[1] = 1.2; + QDot[2] = 1.3; + QDot[3] = -1.4; + QDot[4] = -1.5; + QDot[5] = -1.6; + + unsigned int contact_body_id = base_body_id; + Vector3d contact_point ( 0., -1., 0.); + + ConstraintSet constraint_set; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), "ground_x"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.), "ground_y"); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 0., 1.), "ground_z"); + + constraint_set.Bind (model); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (model, Q, QDot, Tau, constraint_set, QDDot); + + Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 0., 0.).data(), + point_acceleration.data(), + 3, + TEST_PREC + ); + + // cout << "LagrangianSimple Logoutput Start" << endl; + // cout << LogOutput.str() << endl; + // cout << "LagrangianSimple Logoutput End" << endl; +} + +// +// ForwardDynamicsContacts +// +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) { + contact_normal.set (0., 1., 0.); + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); +// cout << LogOutput.str() << endl; + ClearLogOutput(); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) { + Q[0] = 0.6; + Q[3] = M_PI * 0.6; + Q[4] = 0.1; + + contact_normal.set (0., 1., 0.); + + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts, point_accel_contacts_opt; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts_opt = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts_opt); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true); + + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +// +// Similiar to the previous test, this test compares the results of +// - ForwardDynamicsConstraintsDirect +// - ForwardDynamcsContactsOpt +// for the example model in FixedBase6DoF and a moving state (i.e. a +// nonzero QDot) +// +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving) { + Q[0] = 0.6; + Q[3] = M_PI * 0.6; + Q[4] = 0.1; + + QDot[0] = -0.3; + QDot[1] = 0.1; + QDot[2] = -0.5; + QDot[3] = 0.8; + + contact_normal.set (0., 1., 0.); + constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal); + ConstraintSet constraint_set_lagrangian = constraint_set.Copy(); + + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << LogOutput.str() << endl; + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); +// cout << LogOutput.str() << endl; + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContactsOld match + CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC); + + CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal); + constraint_set.AddContactConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContacts match + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + // check whether the point accelerations match + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + + // check whether the generalized accelerations match + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) { + // makes sure that all variables in the constraint set gets reset + // properly when making repeated calls to ForwardDynamicsContacts. + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal); + constraint_set.AddContactConstraint (contact_body_id, Vector3d (0., 1., 0.), contact_normal); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Vector3d point_accel_lagrangian, point_accel_contacts; + + ClearLogOutput(); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + // Call ForwardDynamicsContacts multiple times such that old values might + // be re-used and thus cause erroneus values. + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + + point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true); + point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true); + + // check whether FDContactsLagrangian and FDContacts match + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + // check whether the point accelerations match + CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); + + // check whether the generalized accelerations match + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + // we rotate the joints so that we have full mobility at the contact + // point: + // + // O X (contact point) + // \ / + // \ / + // \ / + // * + // + + Q[0] = M_PI * 0.25; + Q[1] = 0.2; + Q[3] = M_PI * 0.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + VectorNd QDDot_contacts = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot_contacts); + +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl; + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC); + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Q[0] = 0.1; + Q[1] = -0.1; + Q[2] = 0.1; + Q[3] = -0.1; + Q[4] = -0.1; + Q[5] = 0.1; + + QDot[0] = 1.; + QDot[1] = -1.; + QDot[2] = 1; + QDot[3] = -1.5; + QDot[4] = 1.5; + QDot[5] = -1.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase) { + ConstraintSet constraint_set_lagrangian; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_lagrangian = constraint_set.Copy(); + constraint_set_lagrangian.Bind (*model); + constraint_set.Bind (*model); + + VectorNd QDDot_lagrangian = VectorNd::Constant (model->dof_count, 0.); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); +// cout << LogOutput.str() << endl; + + Vector3d point_accel_c, point_accel_2_c; + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_id, contact_point); + +// cout << "point_accel_c = " << point_accel_c.transpose() << endl; + + ClearLogOutput(); + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian); +// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl; +// cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE ( + constraint_set_lagrangian.force.data(), + constraint_set.force.data(), + constraint_set.size(), TEST_PREC + ); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point); + point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point); + + CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); + CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + + CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) { + VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size())); + VectorNd qddot_sparse (VectorNd::Zero(qddot.size())); + + randomizeStates(); + + ConstraintSet constraint_upper_trunk; + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyUpperTrunk], Vector3d (1.1, 2.2, 3.3), Vector3d (1., 0., 0.)); + constraint_upper_trunk.Bind (*model_3dof); + + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot); + + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.); +} + +TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) { + VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size())); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot_3dof[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + Vector3d heel_point (-0.03, 0., -0.03); + + ConstraintSet constraint_upper_trunk; + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (1., 0., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (0., 1., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootLeft], heel_point, Vector3d (0., 0., 1.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (1., 0., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (0., 1., 0.)); + constraint_upper_trunk.AddContactConstraint (body_id_3dof[BodyFootRight], heel_point, Vector3d (0., 0., 1.)); + constraint_upper_trunk.Bind (*model_3dof); + + VectorNd qdotplus (VectorNd::Zero (qdot.size())); + + ComputeConstraintImpulsesDirect (*model_3dof, q, qdot, constraint_upper_trunk, qdotplus); + + Vector3d heel_left_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootLeft], heel_point); + Vector3d heel_right_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootRight], heel_point); + + CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_left_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_right_velocity.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/CustomConstraintsTests.cc b/3rdparty/rbdl/tests/CustomConstraintsTests.cc new file mode 100644 index 0000000..7824ddb --- /dev/null +++ b/3rdparty/rbdl/tests/CustomConstraintsTests.cc @@ -0,0 +1,520 @@ +#include +#include "rbdl/rbdl.h" +#include "rbdl/Constraints.h" +#include + +#include "PendulumModels.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + + +struct PinJointCustomConstraint : public RigidBodyDynamics::CustomConstraint +{ + + PinJointCustomConstraint():RigidBodyDynamics::CustomConstraint(5){ + } + + PinJointCustomConstraint(unsigned int x0y1z2):RigidBodyDynamics::CustomConstraint(5){ + TuP.resize(mConstraintCount); + for(unsigned int i=0; i(3,0) = rmP0.transpose() * (r0S0 - r0P0); + + for(unsigned int i=0; i < TuP.size(); ++i){ + errPos[i+errStartIndex] = TuP[i].transpose()*err; + } + + } + + virtual void CalcVelocityError( Model &UNUSED(model), + unsigned int UNUSED(custom_constraint_id), + const Math::VectorNd &UNUSED(Q), + const Math::VectorNd &QDot, + ConstraintSet &UNUSED(CS), + const Math::MatrixNd &Gblock, + Math::VectorNd &errVel, + unsigned int errStartIndex) + { + //Since this is a time-invariant constraint the expression for + //the velocity error is quite straight forward: + errVelBlock = Gblock*QDot; + for(unsigned int i=0; i < errVelBlock.rows();++i){ + errVel[errStartIndex+i] = errVelBlock[i]; + } + } + + std::vector < SpatialVector > TuP; //Constraint direction vectors resolved in + //the frame that P is on. + SpatialVector err, eT0, eT0Dot, v0P0, v0S0, dv0P0nl ,dv0S0nl; + Vector3d r0P0, r0S0; + Matrix3d rmP0, rmS0, rmPS; + SpatialTransform xP0; + + VectorNd errVelBlock; + +}; + + + +struct DoublePerpendicularPendulumCustomConstraint { + DoublePerpendicularPendulumCustomConstraint() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(1.) + , l2(1.) + , m1(1.) + , m2(1.) + , idB1(0) + , idB2(0) + , X_p1(Xtrans(Vector3d(0., 0., 0.))) + , X_s1(Xtrans(Vector3d(0., 0., 0.))) + , X_p2(Xtrans(Vector3d(0.,-l1, 0.))) + , X_s2(Xtrans(Vector3d(0., 0., 0.))){ + + model.gravity = Vector3d(0.,-9.81,0.); + //Planar pendulum is at 0 when it is hanging down. + // x: points to the right + // y: points up + // z: out of the page + Body link1 = Body(m1, Vector3d( 0., -l1*0.5, 0.), + Matrix3d( m1*l1*l1/3., 0., 0., + 0., m1*l1*l1/30., 0., + 0., 0., m1*l1*l1/3.)); + + + Body link2 = Body(m2, Vector3d( l2*0.5, 0., 0.), + Matrix3d( m2*l2*l2/30., 0., 0., + 0., m2*l2*l2/3., 0., + 0., 0., m2*l2*l2/3.)); + + //Joint joint_free(JointTypeFloatingBase); + Joint jointEA123T123(SpatialVector(0.,0.,0.,1.,0.,0.), + SpatialVector(0.,0.,0.,0.,1.,0.), + SpatialVector(0.,0.,0.,0.,0.,1.), + SpatialVector(0.,0.,1.,0.,0.,0.), + SpatialVector(0.,1.,0.,0.,0.,0.), + SpatialVector(1.,0.,0.,0.,0.,0.)); + + idB1 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0. )), + jointEA123T123, link1); + + idB2 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), + jointEA123T123, link2); + + //Make the revolute joints about the y axis using 5 constraints + //between the end points + + ccPJZaxis = PinJointCustomConstraint(2); + ccPJYaxis = PinJointCustomConstraint(1); + + cs.AddCustomConstraint(&ccPJZaxis, 0,idB1, X_p1, X_s1, false, 0.1); + cs.AddCustomConstraint(&ccPJYaxis,idB1,idB2, X_p2, X_s2, false, 0.1); + + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB1; + unsigned int idB2; + unsigned int idB01; + unsigned int idB02; + + SpatialTransform X_p1; + SpatialTransform X_s1; + SpatialTransform X_p2; + SpatialTransform X_s2; + + PinJointCustomConstraint ccPJZaxis; + PinJointCustomConstraint ccPJYaxis; +}; + + + +TEST(CustomConstraintCorrectnessTest) { + + //Test to add: + // Jacobian vs. num Jacobian + DoublePerpendicularPendulumCustomConstraint dbcc + = DoublePerpendicularPendulumCustomConstraint(); + DoublePerpendicularPendulumAbsoluteCoordinates dba + = DoublePerpendicularPendulumAbsoluteCoordinates(); + DoublePerpendicularPendulumJointCoordinates dbj + = DoublePerpendicularPendulumJointCoordinates(); + + //1. Set the pendulum modeled using joint coordinates to a specific + // state and then compute the spatial acceleration of the body. + dbj.q[0] = M_PI/3.0; //About z0 + dbj.q[1] = M_PI/6.0; //About y1 + dbj.qd[0] = M_PI; //About z0 + dbj.qd[1] = M_PI/2.0; //About y1 + dbj.tau[0]= 0.; + dbj.tau[1]= 0.; + + ForwardDynamics(dbj.model,dbj.q,dbj.qd,dbj.tau,dbj.qdd); + + Vector3d r010 = CalcBodyToBaseCoordinates( + dbj.model,dbj.q,dbj.idB1, + Vector3d(0.,0.,0.),true); + Vector3d r020 = CalcBodyToBaseCoordinates( + dbj.model,dbj.q,dbj.idB2, + Vector3d(0.,0.,0.),true); + // Vector3d r030 = CalcBodyToBaseCoordinates( + // dbj.model,dbj.q,dbj.idB2, + // Vector3d(dbj.l2,0.,0.),true); + + SpatialVector v010 = CalcPointVelocity6D( + dbj.model,dbj.q,dbj.qd,dbj.idB1, + Vector3d(0.,0.,0.),true); + SpatialVector v020 = CalcPointVelocity6D( + dbj.model,dbj.q,dbj.qd,dbj.idB2, + Vector3d(0.,0.,0.),true); + // SpatialVector v030 = CalcPointVelocity6D( + // dbj.model,dbj.q,dbj.qd,dbj.idB2, + // Vector3d(dbj.l2,0.,0.),true); + + SpatialVector a010 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB1,Vector3d(0.,0.,0.),true); + SpatialVector a020 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB2,Vector3d(0.,0.,0.),true); + SpatialVector a030 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB2,Vector3d(dbj.l2,0.,0.),true); + + //2. Set the pendulum modelled using absolute coordinates to the + // equivalent state as the pendulum modelled using joint + // coordinates. Next + + double qError = 1.0; + double qDotError = 1.0; + + //Pefectly initialize the pendulum made with custom constraints and + //perturb the initialization a bit. + dbcc.q[0] = r010[0]; + dbcc.q[1] = r010[1]+qError; + dbcc.q[2] = r010[2]; + dbcc.q[3] = dbj.q[0]; + dbcc.q[4] = 0; + dbcc.q[5] = 0; + dbcc.q[6] = r020[0]; + dbcc.q[7] = r020[1]; + dbcc.q[8] = r020[2]; + dbcc.q[9] = dbj.q[0]; + dbcc.q[10] = dbj.q[1]; + dbcc.q[11] = 0; + + dbcc.qd[0] = v010[3]; + dbcc.qd[1] = v010[4]+qDotError; + dbcc.qd[2] = v010[5]; + dbcc.qd[3] = dbj.qd[0]; + dbcc.qd[4] = 0; + dbcc.qd[5] = 0; + dbcc.qd[6] = v020[3]; + dbcc.qd[7] = v020[4]; + dbcc.qd[8] = v020[5]; + dbcc.qd[9] = dbj.qd[0]; + dbcc.qd[10] = dbj.qd[1]; + dbcc.qd[11] = 0; + + VectorNd err(dbcc.cs.size()); + VectorNd errd(dbcc.cs.size()); + + + CalcConstraintsPositionError(dbcc.model,dbcc.q,dbcc.cs,err,true); + CalcConstraintsVelocityError(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,errd,true); + + + CHECK(err.norm() >= qError); + CHECK(errd.norm() >= qDotError); + + //Solve for the initial q and qdot terms that satisfy the constraints + VectorNd qAsm,qDotAsm,w; + qAsm.resize(dbcc.q.rows()); + qDotAsm.resize(dbcc.q.rows()); + w.resize(dbcc.q.rows()); + for(unsigned int i=0; i + */ + + +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" +#include + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; +const int NUMBER_OF_MODELS = 3; +const int NUMBER_OF_BODIES = 3; + +//============================================================================== +/* + + The purpose of this test is to test that all of the code in RBDL + related to a multibody mechanism that includes a custom joint functions. + Specifically this test is for the multi-pass algorithms in rbdl ... namely + the CompositeRigidBodyAlgorithm. However, because these tests have already + been written for CustomJointSingleBodyTests.cc, we'll run them all on the + multibody models that we will be testing. + + We will be testing 3 models to get good coverage of the + CompositeRigidBodyAlgorithm: + + 1. Rx - multidof - custom + 2. Rx - custom - multidof + 3. custom - multidof - Rx + + As before, to test that the model works, we will create a model using + standard RBDL versions (the reference model), and then we will create + a model using a custom joint in the correct location. The following + algorithms will be tested in this manner: + + UpdateKinematicsCustom + Jacobians + InverseDynamics + CompositeRigidBodyAlgorithm + ForwardDynamics + CalcMInvTimestau + ForwardDynamicsContactsKokkevis + +*/ +//============================================================================== + +struct CustomJointTypeRevoluteX : public CustomJoint { + CustomJointTypeRevoluteX(){ + mDoFCount = 1; + S = MatrixNd::Zero(6,1); + S(0,0)=1.0; + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + model.X_lambda[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + model.X_lambda[joint_id] = + Xrotx (q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + } + +}; + +struct CustomEulerZYXJoint : public CustomJoint { + CustomEulerZYXJoint () { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + SpatialTransform X_J (Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d::Zero()); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0., 0., 0. + ); + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + + + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + } +}; + +//============================================================================== +//Test Fixture +//============================================================================== + + +struct CustomJointMultiBodyFixture { + CustomJointMultiBodyFixture () { + + reference_model.resize(NUMBER_OF_MODELS); + custom_model.resize(NUMBER_OF_MODELS); + + body.resize(NUMBER_OF_MODELS); + custom_joint.resize(NUMBER_OF_MODELS); + + reference_body_id.resize(NUMBER_OF_MODELS); + custom_body_id.resize(NUMBER_OF_MODELS); + + for(int i=0; i < NUMBER_OF_MODELS; ++i){ + body.at(i).resize(3); + custom_joint.at(i).resize(1); + + reference_body_id.at(i).resize(3); + custom_body_id.at(i).resize(3); + + } + + q.resize(NUMBER_OF_MODELS); + qdot.resize(NUMBER_OF_MODELS); + qddot.resize(NUMBER_OF_MODELS); + tau.resize(NUMBER_OF_MODELS); + + //======================================================== + //Test Model 1: Rx - multidof3 - custom + //======================================================== + + custom_rx_joint1 = CustomJointTypeRevoluteX(); + + Matrix3d inertia1 = Matrix3d::Identity(3,3); + + Body body11 = Body (1., Vector3d (1.1, 1.2, 1.3), inertia1); + Body body12 = Body (2., Vector3d (2.1, 2.2, 2.3), inertia1); + Body body13 = Body (3., Vector3d (3.1, 3.2, 3.3), inertia1); + + Model reference1, custom1; + + Vector3d r1 = Vector3d(0.78,-0.125,0.37); + + double th1 = M_PI/6.0; + double sinTh1 = sin(th1); + double cosTh1 = cos(th1); + + Matrix3d rm1 = Matrix3d( 1.0, 0., 0., + 0., cosTh1, -sinTh1, + 0., sinTh1, cosTh1); + + Vector3d r2 = Vector3d(-0.178,0.2125,-0.937); + + double th2 = M_PI/2.15; + double sinTh2 = sin(th2); + double cosTh2 = cos(th2); + + Matrix3d rm2 = Matrix3d( cosTh2, 0., sinTh2, + 0., 1., 0., + -sinTh2, 0., cosTh2); + + + + unsigned int reference_body_id10 = + reference1.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id11 = + reference1.AddBody (reference_body_id10, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int reference_body_id12 = + reference1.AddBody (reference_body_id11, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + + unsigned int custom_body_id10 = + custom1.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int custom_body_id11 = + custom1.AddBody ( custom_body_id10, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int custom_body_id12 = + custom1.AddBodyCustomJoint ( custom_body_id11, + SpatialTransform(rm2,r2), + &custom_rx_joint1, + body13); + + VectorNd q1 = VectorNd::Zero (reference1.q_size); + VectorNd qdot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd qddot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd tau1 = VectorNd::Zero (reference1.qdot_size); + + int idx = 0; + reference_model.at(idx) = (reference1); + custom_model.at(idx) = (custom1); + + reference_body_id.at(idx).at(0) = (reference_body_id10); + reference_body_id.at(idx).at(1) = (reference_body_id11); + reference_body_id.at(idx).at(2) = (reference_body_id12); + + custom_body_id.at(idx).at(0) = (custom_body_id10); + custom_body_id.at(idx).at(1) = (custom_body_id11); + custom_body_id.at(idx).at(2) = (custom_body_id12); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + q.at(idx) = (q1); + qdot.at(idx) = (qdot1); + qddot.at(idx) = (qddot1); + tau.at(idx) = (tau1); + + + + //======================================================== + //Test Model 2: Rx - custom - multidof3 + //======================================================== + + + Model reference2, custom2; + + unsigned int reference_body_id20 = + reference2.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id21 = + reference2.AddBody (reference_body_id20, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body12); + + unsigned int reference_body_id22 = + reference2.AddBody (reference_body_id21, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body13); + + unsigned int custom_body_id20 = + custom2.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int custom_body_id21 = + custom2.AddBodyCustomJoint ( custom_body_id20, + SpatialTransform(rm2,r2), + &custom_rx_joint1, + body12); + + unsigned int custom_body_id22 = + custom2.AddBody ( custom_body_id21, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body13); + + + + VectorNd q2 = VectorNd::Zero (reference2.q_size); + VectorNd qdot2 = VectorNd::Zero (reference2.qdot_size); + VectorNd qddot2 = VectorNd::Zero (reference2.qdot_size); + VectorNd tau2 = VectorNd::Zero (reference2.qdot_size); + + idx = 1; + reference_model.at(idx) = (reference2); + custom_model.at(idx) = (custom2); + + reference_body_id.at(idx).at(0) = (reference_body_id20); + reference_body_id.at(idx).at(1) = (reference_body_id21); + reference_body_id.at(idx).at(2) = (reference_body_id22); + + custom_body_id.at(idx).at(0) = (custom_body_id20); + custom_body_id.at(idx).at(1) = (custom_body_id21); + custom_body_id.at(idx).at(2) = (custom_body_id22); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + + q.at(idx) = (q2); + qdot.at(idx) = (qdot2); + qddot.at(idx) = (qddot2); + tau.at(idx) = (tau2); + + //======================================================== + //Test Model 3: custom - multidof3 - Rx + //======================================================== + + Model reference3, custom3; + + unsigned int reference_body_id30 = + reference3.AddBody (0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body11); + + unsigned int reference_body_id31 = + reference3.AddBody (reference_body_id30, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int reference_body_id32 = + reference3.AddBody (reference_body_id31, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + unsigned int custom_body_id30 = + custom3.AddBodyCustomJoint ( 0, + SpatialTransform(), + &custom_rx_joint1, + body11); + + unsigned int custom_body_id31 = + custom3.AddBody ( custom_body_id30, + SpatialTransform(rm1,r1), + Joint(JointTypeEulerZYX), + body12); + + unsigned int custom_body_id32 = + custom3.AddBody ( custom_body_id31, + SpatialTransform(rm2,r2), + Joint(JointTypeRevoluteX), + body13); + + VectorNd q3 = VectorNd::Zero (reference3.q_size); + VectorNd qdot3 = VectorNd::Zero (reference3.qdot_size); + VectorNd qddot3 = VectorNd::Zero (reference3.qdot_size); + VectorNd tau3 = VectorNd::Zero (reference3.qdot_size); + + idx = 2; + reference_model.at(idx) = (reference3); + custom_model.at(idx) = (custom3); + + reference_body_id.at(idx).at(0) = (reference_body_id30); + reference_body_id.at(idx).at(1) = (reference_body_id31); + reference_body_id.at(idx).at(2) = (reference_body_id32); + + custom_body_id.at(idx).at(0) = (custom_body_id30); + custom_body_id.at(idx).at(1) = (custom_body_id31); + custom_body_id.at(idx).at(2) = (custom_body_id32); + + body.at(idx).at(0) = (body11); + body.at(idx).at(1) = (body12); + body.at(idx).at(2) = (body13); + custom_joint.at(idx).at(0) = (&custom_rx_joint1); + + q.at(idx) = (q3); + qdot.at(idx) = (qdot3); + qddot.at(idx) = (qddot3); + tau.at(idx) = (tau3); + + } + + /* + ~CustomJointMultiBodyFixture () { + }*/ + + vector < Model > reference_model; + vector < Model > custom_model; + + vector < vector < Body > > body; + vector < vector < CustomJoint* > > custom_joint; + + vector < vector< unsigned int > > reference_body_id; + vector < vector< unsigned int > > custom_body_id; + + vector < VectorNd > q; + vector < VectorNd > qdot; + vector < VectorNd > qddot; + vector < VectorNd > tau; + + CustomJointTypeRevoluteX custom_rx_joint1; + CustomJointTypeRevoluteX custom_rx_joint2; + CustomJointTypeRevoluteX custom_rx_joint3; +}; + + +//============================================================================== +// +// Tests +// UpdateKinematicsCustom +// Jacobians +// InverseDynamics +// CompositeRigidBodyAlgorithm +// ForwardDynamics +// CalcMInvTimestau +// ForwardDynamicsContactsKokkevis +// +//============================================================================== + +TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) { + + VectorNd test; + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof ; i++) { + q.at(idx)[i] = i * 0.1; + qdot.at(idx)[i] = i * 0.15; + qddot.at(idx)[i] = i * 0.17; + } + + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + + // Matrix3d Eref = reference_model.at(idx).X_base[ + // reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + // ].E; + // Matrix3d Ecus = custom_model.at(idx).X_base[ + // custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + // ].E; + // Matrix3d Eerr = Eref-Ecus; + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + custom_model.at(idx).X_base[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + 9, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).v[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).a[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), NULL, NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), NULL, NULL); + + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + custom_model.at(idx).X_base[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].E.data(), + 9, + TEST_PREC); + + + //velocity + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), NULL); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).v[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + + + //All + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[ + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + custom_model.at(idx).a[ + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1) + ].data(), + 6, + TEST_PREC); + } + + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) { + + for(int idx = 0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + //Check the Spatial Jacobian + MatrixNd Gref = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + MatrixNd Gcus = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + CalcBodySpatialJacobian ( reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + Gref); + + CalcBodySpatialJacobian ( custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + Gcus); + + for(int i=0; i<6;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + Gref(i,j), + Gcus(i,j), + TEST_PREC); + } + } + + //Check the 6d point Jacobian + Vector3d point_position (1.1, 1.2, 2.1); + + + CalcPointJacobian6D (reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + point_position, + Gref); + + CalcPointJacobian6D (custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + point_position, + Gcus); + + for(int i=0; i<6;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + Gref(i,j), + Gcus(i,j), + TEST_PREC); + } + } + + //Check the 3d point Jacobian + MatrixNd GrefPt = MatrixNd::Constant (3, + reference_model.at(idx).dof_count, + 0.); + MatrixNd GcusPt = MatrixNd::Constant (3, + reference_model.at(idx).dof_count, + 0.); + + CalcPointJacobian (reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + point_position, + GrefPt); + + CalcPointJacobian (custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + point_position, + GcusPt); + + for(int i=0; i<3;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + GrefPt(i,j), + GcusPt(i,j), + TEST_PREC); + } + } + } + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, InverseDynamics) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + VectorNd tauRef = VectorNd::Zero (reference_model.at(idx).qdot_size); + VectorNd tauCus = VectorNd::Zero (reference_model.at(idx).qdot_size); + + InverseDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx), + tauRef); + + InverseDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx), + tauCus); + + VectorNd tauErr = tauRef-tauCus; + + CHECK_ARRAY_CLOSE ( + tauRef.data(), + tauCus.data(), + tauRef.rows(), + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + MatrixNd h_ref = MatrixNd::Constant (dof, dof, 0.); + VectorNd c_ref = VectorNd::Constant (dof, 0.); + VectorNd qddot_zero_ref = VectorNd::Constant (dof, 0.); + VectorNd qddot_crba_ref = VectorNd::Constant (dof, 0.); + + MatrixNd h_cus = MatrixNd::Constant (dof, dof, 0.); + VectorNd c_cus = VectorNd::Constant (dof, 0.); + VectorNd qddot_zero_cus = VectorNd::Constant (dof, 0.); + VectorNd qddot_crba_cus = VectorNd::Constant (dof, 0.); + + VectorNd qddotRef = VectorNd::Zero (dof); + VectorNd qddotCus = VectorNd::Zero (dof); + + //Ref + ForwardDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotRef); + + CompositeRigidBodyAlgorithm (reference_model.at(idx), + q.at(idx), + h_ref); + + InverseDynamics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot_zero_ref, + c_ref); + + LinSolveGaussElimPivot (h_ref, + c_ref * -1. + tau.at(idx), + qddot_crba_ref); + + //Custom + ForwardDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotCus); + + CompositeRigidBodyAlgorithm (custom_model.at(idx), + q.at(idx), + h_cus); + + InverseDynamics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot_zero_cus, + c_cus); + + LinSolveGaussElimPivot (h_cus, + c_cus * -1. + tau.at(idx), + qddot_crba_cus); + + CHECK_ARRAY_CLOSE(qddot_crba_ref.data(), + qddot_crba_cus.data(), + dof, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamics) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + qddot.at(idx)[i] = (i+0.1) * 2.323592499940952e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + + VectorNd qddotRef = VectorNd::Zero (reference_model.at(idx).qdot_size); + VectorNd qddotCus = VectorNd::Zero (reference_model.at(idx).qdot_size); + + ForwardDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotRef); + + ForwardDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotCus); + + CHECK_ARRAY_CLOSE ( + qddotRef.data(), + qddotCus.data(), + dof, + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, CalcMInvTimestau) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + + } + + //reference + VectorNd qddot_minv_ref = VectorNd::Zero(dof); + + CalcMInvTimesTau(reference_model.at(idx), + q.at(idx), + tau.at(idx), + qddot_minv_ref, + true); + + //custom + VectorNd qddot_minv_cus = VectorNd::Zero(dof); + CalcMInvTimesTau(custom_model.at(idx), + q.at(idx), + tau.at(idx), + qddot_minv_cus, + true); + + //check. + CHECK_ARRAY_CLOSE(qddot_minv_ref.data(), + qddot_minv_cus.data(), + dof, + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamicsContactsKokkevis){ + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + //Adding a 1 constraint to a system with 1 dof is + //a no-no + if(dof > 1){ + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + VectorNd qddot_ref = VectorNd::Zero(dof); + VectorNd qddot_cus = VectorNd::Zero(dof); + + VectorNd qdot_plus_ref = VectorNd::Zero(dof); + VectorNd qdot_plus_cus = VectorNd::Zero(dof); + + Vector3d contact_point ( 0., 1., 0.); + + ConstraintSet constraint_set_ref; + ConstraintSet constraint_set_cus; + + + //Reference + constraint_set_ref.AddContactConstraint( + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_ref.AddContactConstraint( + reference_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_ref.Bind (reference_model.at(idx)); + + //Custom + constraint_set_cus.AddContactConstraint( + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_cus.AddContactConstraint( + custom_body_id.at(idx).at(NUMBER_OF_BODIES-1), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_cus.Bind (custom_model.at(idx)); + + ComputeConstraintImpulsesDirect(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_ref, + qdot_plus_ref); + + ForwardDynamicsContactsKokkevis (reference_model.at(idx), + q.at(idx), + qdot_plus_ref, + tau.at(idx), + constraint_set_ref, + qddot_ref); + + ComputeConstraintImpulsesDirect(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_cus, + qdot_plus_cus); + + ForwardDynamicsContactsKokkevis (custom_model.at(idx), + q.at(idx), + qdot_plus_cus, + tau.at(idx), + constraint_set_cus, + qddot_cus); + + VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus; + VectorNd qddot_error = qddot_ref - qddot_cus; + + CHECK_ARRAY_CLOSE (qdot_plus_ref.data(), + qdot_plus_cus.data(), + dof, + TEST_PREC); + + CHECK_ARRAY_CLOSE (qddot_ref.data(), + qddot_cus.data(), + dof, + TEST_PREC); + } + } + +} + +// +//Completed? +// x : implement test for UpdateKinematicsCustom +// x : implement test for Jacobians +// x : implement test for InverseDynamics +// x : implement test for CompositeRigidBodyAlgorithm +// x : implement test for ForwardDynamics +// x : implement test for CalcMInvTimestau +// x : implement test for ForwardDynamicsContactsKokkevis diff --git a/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc new file mode 100644 index 0000000..5962181 --- /dev/null +++ b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc @@ -0,0 +1,859 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2016-2018 Matthew Millard + */ + + +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" +#include + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 2.0e-12; +const int NUMBER_OF_MODELS = 2; + +//============================================================================== +/* + + The purpose of this test is to test that all of the code in RBDL + related to a single CustomJoint functions. To do this we will implement + joints that already exist in RBDL but using the CustomJoint interface. The + test will then numerically compare the results produced by the CustomJoint + and the equivalent built-in joint in RBDL. The following algorithms will + be tested: + + UpdateKinematicsCustom + Jacobians + InverseDynamics + CompositeRigidBodyAlgorithm + ForwardDynamics + CalcMInvTimestau + ForwardDynamicsContactsKokkevis + +*/ +//============================================================================== +//============================================================================== +/* + As a note, below are the basic fields and functions that every CustomJoint + class member must provide. Refer to Featherstone's dynamics text if the field + names below don't make sense to you. + + 1. Extend from CustomJoint: + + struct CustomJointClass: public CustomJoint + + 2. Make a default constructor, and initialize member variables + mDoFCount + S + d_u + + e.g. + + CustomJointClass() + + 3. Implement the method jcalc. This method must populate X_lambda, v_J, c_J, + and S. + + virtual void jcalc + model.X_lambda[joint_id] + model.v_J + model.c_J + model.mCustomJoints[joint.custom_joint_index]->S = S + + 4. Implement the method jcalc_X_lambda_S. This method must populate X_lambda + and S. + + virtual void jcalc_X_lambda_S + model.X_lambda + model.mCustomJoints[joint.custom_joint_index]->S = S; + + */ +//============================================================================== +//Custom Joint Code +//============================================================================== +struct CustomJointTypeRevoluteX : public CustomJoint +{ + CustomJointTypeRevoluteX(){ + mDoFCount = 1; + S = MatrixNd::Zero(6,1); + S(0, 0) = 1.0; + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + model.X_lambda[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index]; + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + model.X_lambda[joint_id] = + Xrotx (q[model.mJoints[joint_id].q_index]) + * model.X_T[joint_id]; + + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + } + +}; + +struct CustomEulerZYXJoint : public CustomJoint +{ + CustomEulerZYXJoint () + { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + d_u = MatrixNd::Zero(mDoFCount,1); + } + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + SpatialTransform X_J (Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d::Zero()); + model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + -c1*qdot0*qdot1, + -s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2, + -s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2, + 0., 0., 0. + ); + } + + virtual void jcalc_X_lambda_S ( Model &model, + unsigned int joint_id, + const Math::VectorNd &q) + { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + + model.X_lambda[joint_id] = SpatialTransform ( + Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ), + Vector3d (0., 0., 0.)) * model.X_T[joint_id]; + + S.setZero(); + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + const Joint &joint = model.mJoints[joint_id]; + model.mCustomJoints[joint.custom_joint_index]->S = S; + + //assert (false && "Not yet implemented!"); + } +}; + +//============================================================================== +//Test Fixture +//============================================================================== + +struct CustomJointSingleBodyFixture { + CustomJointSingleBodyFixture () { + + reference_model.resize(NUMBER_OF_MODELS); + custom_model.resize(NUMBER_OF_MODELS); + + body.resize(NUMBER_OF_MODELS); + custom_joint.resize(NUMBER_OF_MODELS); + + reference_body_id.resize(NUMBER_OF_MODELS); + custom_body_id.resize(NUMBER_OF_MODELS); + + q.resize(NUMBER_OF_MODELS); + qdot.resize(NUMBER_OF_MODELS); + qddot.resize(NUMBER_OF_MODELS); + tau.resize(NUMBER_OF_MODELS); + + //======================================================== + //Test Model 0: 3dof rotational custom joint vs. standard. + //======================================================== + + custom_joint0 = CustomEulerZYXJoint(); + + Matrix3d inertia0 = Matrix3d::Identity(3,3); + Body body0 = Body (1., Vector3d (1.1, 1.2, 1.3), inertia0); + + Model reference0, custom0; + + unsigned int reference_body_id0 = + reference0.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeEulerZYX), + body0); + + unsigned int custom_body_id0 = + custom0.AddBodyCustomJoint ( 0, + SpatialTransform(), + &custom_joint0, + body0); + + VectorNd q0 = VectorNd::Zero (reference0.q_size); + VectorNd qdot0 = VectorNd::Zero (reference0.qdot_size); + VectorNd qddot0 = VectorNd::Zero (reference0.qdot_size); + VectorNd tau0 = VectorNd::Zero (reference0.qdot_size); + + reference_model.at(0) = reference0; + custom_model.at(0) = custom0; + + reference_body_id.at(0) = (reference_body_id0); + custom_body_id.at(0) = (custom_body_id0); + + body.at(0) = (body0); + custom_joint.at(0) = (&custom_joint0); + + q.at(0) = (q0); + qdot.at(0) = (qdot0); + qddot.at(0) = (qddot0); + tau.at(0) = (tau0); + + //======================================================== + //Test Model 1: 1 dof rotational custom joint vs. standard. + //======================================================== + + custom_joint1 = CustomJointTypeRevoluteX(); + + Model reference1, custom1; + + unsigned int reference_body_id1 = + reference1.AddBody ( 0, + SpatialTransform(), + Joint(JointTypeRevoluteX), + body0); + + unsigned int custom_body_id1 = + custom1.AddBodyCustomJoint (0, + SpatialTransform(), + &custom_joint1, + body0); + + VectorNd q1 = VectorNd::Zero (reference1.q_size); + VectorNd qdot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd qddot1 = VectorNd::Zero (reference1.qdot_size); + VectorNd tau1 = VectorNd::Zero (reference1.qdot_size); + + reference_model.at(1) = (reference1); + custom_model.at(1) = (custom1); + + reference_body_id.at(1) = (reference_body_id1); + custom_body_id.at(1) = (custom_body_id1); + + body.at(1) = (body0); + custom_joint.at(1) = (&custom_joint1); + + q.at(1) = (q1); + qdot.at(1) = (qdot1); + qddot.at(1) = (qddot1); + tau.at(1) = (tau1); + + } + + /* + ~CustomJointSingleBodyFixture () { + delete reference_model; + delete custom_model; + + delete body; + delete custom_joint; + + delete reference_body_id; + delete custom_body_id; + + delete q; + delete qdot; + delete qddot; + delete tau; + }*/ + + vector < Model > reference_model; + vector < Model > custom_model; + + vector < Body > body; + vector < CustomJoint* > custom_joint; + + vector < unsigned int > reference_body_id; + vector < unsigned int > custom_body_id; + + vector < VectorNd > q; + vector < VectorNd > qdot; + vector < VectorNd > qddot; + vector < VectorNd > tau; + CustomJointTypeRevoluteX custom_joint1; + CustomEulerZYXJoint custom_joint0; + +}; + +//============================================================================== +// +// Tests +// UpdateKinematicsCustom +// Jacobians +// InverseDynamics +// CompositeRigidBodyAlgorithm +// ForwardDynamics +// CalcMInvTimestau +// ForwardDynamicsContactsKokkevis +// +//============================================================================== + +TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) { + + VectorNd test; + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof ; i++) { + q.at(idx)[i] = i * 0.1; + qdot.at(idx)[i] = i * 0.15; + qddot.at(idx)[i] = i * 0.17; + } + + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(), + custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(), + 9, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[reference_body_id.at(idx)].data(), + custom_model.at(idx).v[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[reference_body_id.at(idx)].data(), + custom_model.at(idx).a[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), NULL, NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), NULL, NULL); + + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(), + custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(), + 9, + TEST_PREC); + + + //velocity + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + NULL); + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + NULL); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).v[reference_body_id.at(idx)].data(), + custom_model.at(idx).v[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + + + //All + UpdateKinematicsCustom (reference_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + UpdateKinematicsCustom (custom_model.at(idx), + &q.at(idx), + &qdot.at(idx), + &qddot.at(idx)); + + CHECK_ARRAY_CLOSE ( + reference_model.at(idx).a[reference_body_id.at(idx)].data(), + custom_model.at(idx).a[ custom_body_id.at(idx)].data(), + 6, + TEST_PREC); + } + + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + UpdateKinematics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + UpdateKinematics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx)); + + //Check the Spatial Jacobian + MatrixNd Gref = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + MatrixNd Gcus = MatrixNd( + MatrixNd::Zero(6,reference_model.at(idx).dof_count)); + + CalcBodySpatialJacobian ( reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx), + Gref); + + CalcBodySpatialJacobian ( custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx), + Gcus); + + for(int i=0; i<6;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + Gref(i,j), + Gcus(i,j), + TEST_PREC); + } + } + + //Check the 6d point Jacobian + Vector3d point_position (1.1, 1.2, 2.1); + + CalcPointJacobian6D (reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx), + point_position, + Gref); + + CalcPointJacobian6D (custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx), + point_position, + Gcus); + + for(int i=0; i<6;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + Gref(i,j), + Gcus(i,j), + TEST_PREC); + } + } + + + + //Check the 3d point Jacobian + MatrixNd GrefPt = MatrixNd::Constant (3, + reference_model.at(idx).dof_count, + 0.); + MatrixNd GcusPt = MatrixNd::Constant (3, + reference_model.at(idx).dof_count, + 0.); + + + + CalcPointJacobian (reference_model.at(idx), + q.at(idx), + reference_body_id.at(idx), + point_position, + GrefPt); + + CalcPointJacobian (custom_model.at(idx), + q.at(idx), + custom_body_id.at(idx), + point_position, + GcusPt); + + for(int i=0; i<3;++i){ + for(unsigned int j = 0; j < dof; ++j){ + CHECK_CLOSE ( + GrefPt(i,j), + GcusPt(i,j), + TEST_PREC); + } + } + } + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = i * 9.133758561390194e-01; + qdot.at(idx)[i] = i * 6.323592462254095e-01; + qddot.at(idx)[i] = i * 9.754040499940952e-02; + } + + //position + + VectorNd tauRef = VectorNd::Zero (reference_model.at(idx).qdot_size); + VectorNd tauCus = VectorNd::Zero (reference_model.at(idx).qdot_size); + + InverseDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx), + tauRef); + + InverseDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot.at(idx), + tauCus); + + VectorNd tauErr = tauRef-tauCus; + + CHECK_ARRAY_CLOSE ( + tauRef.data(), + tauCus.data(), + tauRef.rows(), + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + MatrixNd h_ref = MatrixNd::Constant (dof, dof, 0.); + VectorNd c_ref = VectorNd::Constant (dof, 0.); + VectorNd qddot_zero_ref = VectorNd::Constant (dof, 0.); + VectorNd qddot_crba_ref = VectorNd::Constant (dof, 0.); + + MatrixNd h_cus = MatrixNd::Constant (dof, dof, 0.); + VectorNd c_cus = VectorNd::Constant (dof, 0.); + VectorNd qddot_zero_cus = VectorNd::Constant (dof, 0.); + VectorNd qddot_crba_cus = VectorNd::Constant (dof, 0.); + + VectorNd qddotRef = VectorNd::Zero (dof); + VectorNd qddotCus = VectorNd::Zero (dof); + + //Ref + ForwardDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotRef); + + CompositeRigidBodyAlgorithm (reference_model.at(idx), + q.at(idx), + h_ref); + + InverseDynamics (reference_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot_zero_ref, + c_ref); + + LinSolveGaussElimPivot (h_ref, + c_ref * -1. + tau.at(idx), + qddot_crba_ref); + + //Custom + ForwardDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotCus); + + CompositeRigidBodyAlgorithm (custom_model.at(idx), + q.at(idx), + h_cus); + + InverseDynamics (custom_model.at(idx), + q.at(idx), + qdot.at(idx), + qddot_zero_cus, + c_cus); + + LinSolveGaussElimPivot (h_cus, + c_cus * -1. + tau.at(idx), + qddot_crba_cus); + + CHECK_ARRAY_CLOSE(qddot_crba_ref.data(), + qddot_crba_cus.data(), + dof, + TEST_PREC); + } +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + qddot.at(idx)[i] = (i+0.1) * 2.323592499940952e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + + VectorNd qddotRef = VectorNd::Zero (reference_model.at(idx).qdot_size); + VectorNd qddotCus = VectorNd::Zero (reference_model.at(idx).qdot_size); + + ForwardDynamics(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotRef); + + ForwardDynamics(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + tau.at(idx), + qddotCus); + + CHECK_ARRAY_CLOSE ( qddotRef.data(), + qddotCus.data(), + dof, + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) { + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + + } + + //reference + VectorNd qddot_minv_ref = VectorNd::Zero(dof); + + CalcMInvTimesTau(reference_model.at(idx), + q.at(idx), + tau.at(idx), + qddot_minv_ref, + true); + + //custom + VectorNd qddot_minv_cus = VectorNd::Zero(dof); + + CalcMInvTimesTau(custom_model.at(idx), + q.at(idx), + tau.at(idx), + qddot_minv_cus, + true); + //check. + CHECK_ARRAY_CLOSE(qddot_minv_ref.data(), + qddot_minv_cus.data(), + dof, + TEST_PREC); + } + +} + +TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){ + + for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){ + unsigned int dof = reference_model.at(idx).dof_count; + //Adding a 1 constraint to a system with 1 dof is + //a no-no + if(dof > 1){ + + for (unsigned int i = 0; i < dof; i++) { + q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01; + qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01; + + tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02; + } + + VectorNd qddot_ref = VectorNd::Zero(dof); + VectorNd qddot_cus = VectorNd::Zero(dof); + + VectorNd qdot_plus_ref = VectorNd::Zero(dof); + VectorNd qdot_plus_cus = VectorNd::Zero(dof); + + Vector3d contact_point ( 0., 1., 0.); + + ConstraintSet constraint_set_ref; + ConstraintSet constraint_set_cus; + + //Reference + constraint_set_ref.AddContactConstraint( reference_body_id.at(idx), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_ref.AddContactConstraint( reference_body_id.at(idx), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_ref.Bind (reference_model.at(idx)); + + //Custom + constraint_set_cus.AddContactConstraint( custom_body_id.at(idx), + contact_point, + Vector3d (1., 0., 0.), + "ground_x"); + + constraint_set_cus.AddContactConstraint( custom_body_id.at(idx), + contact_point, + Vector3d (0., 1., 0.), + "ground_y"); + + constraint_set_cus.Bind (custom_model.at(idx)); + + ComputeConstraintImpulsesDirect(reference_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_ref, + qdot_plus_ref); + + ForwardDynamicsContactsKokkevis (reference_model.at(idx), + q.at(idx), + qdot_plus_ref, + tau.at(idx), + constraint_set_ref, + qddot_ref); + + ComputeConstraintImpulsesDirect(custom_model.at(idx), + q.at(idx), + qdot.at(idx), + constraint_set_cus, + qdot_plus_cus); + + ForwardDynamicsContactsKokkevis (custom_model.at(idx), + q.at(idx), + qdot_plus_cus, + tau.at(idx), + constraint_set_cus, + qddot_cus); + + VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus; + VectorNd qddot_error = qddot_ref - qddot_cus; + + CHECK_ARRAY_CLOSE (qdot_plus_ref.data(), + qdot_plus_cus.data(), + dof, + TEST_PREC); + + CHECK_ARRAY_CLOSE (qddot_ref.data(), + qddot_cus.data(), + dof, + TEST_PREC); + } + } + +} + + diff --git a/3rdparty/rbdl/tests/CustomJointTests.cc b/3rdparty/rbdl/tests/CustomJointTests.cc new file mode 100644 index 0000000..9f19344 --- /dev/null +++ b/3rdparty/rbdl/tests/CustomJointTests.cc @@ -0,0 +1,137 @@ +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct CustomEulerZYXJoint : public CustomJoint { + CustomEulerZYXJoint () { + mDoFCount = 3; + S = MatrixNd::Zero (6,3); + }; + + virtual void jcalc (Model &model, + unsigned int joint_id, + const Math::VectorNd &q, + const Math::VectorNd &qdot + ) { + double q0 = q[model.mJoints[joint_id].q_index]; + double q1 = q[model.mJoints[joint_id].q_index + 1]; + double q2 = q[model.mJoints[joint_id].q_index + 2]; + + double s0 = sin (q0); + double c0 = cos (q0); + double s1 = sin (q1); + double c1 = cos (q1); + double s2 = sin (q2); + double c2 = cos (q2); + + model.X_J[joint_id].E = Matrix3d( + c0 * c1, s0 * c1, -s1, + c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, + c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2 + ); + + S(0,0) = -s1; + S(0,2) = 1.; + + S(1,0) = c1 * s2; + S(1,1) = c2; + + S(2,0) = c1 * c2; + S(2,1) = - s2; + + double qdot0 = qdot[model.mJoints[joint_id].q_index]; + double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; + double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; + + model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2); + + model.c_J[joint_id].set( + - c1 * qdot0 * qdot1, + -s1 * s2 * qdot0 * qdot1 + c1 * c2 * qdot0 * qdot2 - s2 * qdot1 * qdot2, + -s1 * c2 * qdot0 * qdot1 - c1 * s2 * qdot0 * qdot2 - c2 * qdot1 * qdot2, + 0., 0., 0. + ); + } + virtual void jcalc_X_lambda_S (Model &model, + unsigned int joint_id, + const Math::VectorNd &q + ) { + // TODO + assert (false && "Not yet implemented!"); + } +}; + +struct CustomJointFixture { + CustomJointFixture () { + custom_joint = new CustomEulerZYXJoint(); + + Matrix3d inertia = Matrix3d::Identity(3,3); + body = Body (1., Vector3d (1.1, 1.2, 1.3), inertia); + reference_body_id = reference_model.AddBody (0,SpatialTransform(), Joint(JointTypeEulerZYX), body); + custom_body_id = custom_model.AddBodyCustomJoint (0, SpatialTransform(), custom_joint, body); + + q = VectorNd::Zero (reference_model.q_size); + qdot = VectorNd::Zero (reference_model.qdot_size); + qddot = VectorNd::Zero (reference_model.qdot_size); + tau = VectorNd::Zero (reference_model.qdot_size); + } + + ~CustomJointFixture () { + delete custom_joint; + } + + Model reference_model; + Model custom_model; + + Body body; + CustomJoint* custom_joint; + + unsigned int reference_body_id; + unsigned int custom_body_id; + + VectorNd q; + VectorNd qdot; + VectorNd qddot; + VectorNd tau; +}; + +TEST_FIXTURE ( CustomJointFixture, UpdateKinematics ) { + for (unsigned int i = 0; i < 3; i++) { + q[i] = i * 0.1; + qdot[i] = i * 0.15; + qddot[i] = i * 0.17; + } + + UpdateKinematics (reference_model, q, qdot, qddot); + UpdateKinematics (custom_model, q, qdot, qddot); + + CHECK_ARRAY_EQUAL (reference_model.X_base[reference_body_id].E.data(), custom_model.X_base[custom_body_id].E.data(), 9); + + CHECK_ARRAY_EQUAL (reference_model.v[reference_body_id].data(), custom_model.v[custom_body_id].data(), 6); + + CHECK_ARRAY_EQUAL (reference_model.a[reference_body_id].data(), custom_model.a[custom_body_id].data(), 6); +} + +// TODO: implement test for UpdateKinematicsCustom +// TODO: implement test for Jacobians +// TODO: implement test for InverseDynamics +// TODO: implement test for CompositeRigidBodyAlgorithm +// TODO: implement test for ForwardDynamics +// TODO: implement test for CalcMInvTimestau +// TODO: implement test for ForwardDynamicsContacts diff --git a/3rdparty/rbdl/tests/DynamicsTests.cc b/3rdparty/rbdl/tests/DynamicsTests.cc new file mode 100644 index 0000000..5914272 --- /dev/null +++ b/3rdparty/rbdl/tests/DynamicsTests.cc @@ -0,0 +1,735 @@ +#include + +#include +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +#include "Fixtures.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-13; + +struct DynamicsFixture { + DynamicsFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~DynamicsFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) { + Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + CHECK_EQUAL (-4.905, QDDot[0]); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) { + // This function checks the value for a non-trivial spatial inertia + Body body(1., Vector3d (1.5, 1., 1.), Vector3d (1., 2., 3.)); + + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + CHECK_EQUAL (-2.3544, QDDot[0]); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + +// cout << "--- Double Chain ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-5.88600000000000E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 3.92400000000000E+00, QDDot[1], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicTripleChain) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + Body body_c (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_c ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(2, Xtrans(Vector3d(1., 0., 0.)), joint_c, body_c); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- Triple Chain ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-6.03692307692308E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 3.77307692307692E+00, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 1.50923076923077E+00, QDDot[2], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain3D) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- Double Chain 3D ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-3.92400000000000E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE ( 0.00000000000000E+00, QDDot[1], TEST_PREC); +} + +TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSimpleTree3D) { + Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + Body body_b1 (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b1 ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b1, body_b1); + + Body body_c1 (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + Joint joint_c1 ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c1, body_c1); + + Body body_b2 (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + Joint joint_b2 ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + model->AddBody(1, Xtrans(Vector3d(-0.5, 0., 0.)), joint_b2, body_b2); + + Body body_c2 (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + Joint joint_c2 ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + model->AddBody(4, Xtrans(Vector3d(0., -0.5, 0.)), joint_c2, body_c2); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + // cout << "--- SimpleTree ---" << endl; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a[i] << endl; + } + + // cout << LogOutput.str() << endl; + + CHECK_CLOSE (-1.60319066147860E+00, QDDot[0], TEST_PREC); + CHECK_CLOSE (-5.34396887159533E-01, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 4.10340466926070E+00, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 2.67198443579767E-01, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 5.30579766536965E+00, QDDot[4], TEST_PREC); +} + +TEST (TestForwardDynamicsLagrangian) { + Model model; + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + VectorNd Tau = VectorNd::Zero (model.dof_count); + + VectorNd QDDot_aba = VectorNd::Zero (model.dof_count); + VectorNd QDDot_lagrangian = VectorNd::Zero (model.dof_count); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(model, Q, QDot, Tau, QDDot_aba); + ForwardDynamicsLagrangian(model, Q, QDot, Tau, QDDot_lagrangian); + + CHECK_EQUAL (QDDot_aba.size(), QDDot_lagrangian.size()); + CHECK_ARRAY_CLOSE (QDDot_aba.data(), QDDot_lagrangian.data(), QDDot_aba.size(), TEST_PREC); +} + +/* + * A simple test for a model with 3 rotational dof. The reference value was + * computed with Featherstones spatial_v1 code. This test was written + * because my benchmark tool showed up inconsistencies, however this was + * due to the missing gravity term. But as the test now works, I just leave + * it here. + */ +TEST (TestForwardDynamics3DoFModel) { + Model model; + + model.gravity = Vector3d (0., -9.81, 0.); + + Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.)); + Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + unsigned int base_id_rot_z, base_id_rot_y; + + // we can reuse both bodies and joints as they are copied + base_id_rot_z = model.AddBody (0, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, null_body); + base_id_rot_y = model.AddBody (base_id_rot_z, Xtrans (Vector3d(0., 0., 0.)), joint_rot_y, null_body); + model.AddBody (base_id_rot_y, Xtrans (Vector3d(0., 0., 0.)), joint_rot_x, base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + VectorNd QDDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot_ref = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[0] = 1.; + + ClearLogOutput(); + + ForwardDynamics (model, Q, QDot, Tau, QDDot); + +// cout << LogOutput.str() << endl; + + QDDot_ref[0] = 3.301932144386186; + + CHECK_ARRAY_CLOSE (QDDot_ref.data(), QDDot.data(), QDDot.size(), TEST_PREC); +} + +/* + * Another simple 3 dof model test which showed some problems when + * computing forward dynamics with the Lagrangian formulation. A proplem + * occured as the CRBA does not update the kinematics of the model, hence + * invalid body transformations and joint axis were used in the CRBA. + * Running the CRBA after the InverseDynamics calculation fixes this. This + * test ensures that the error does not happen when calling ForwardLagrangian. + */ +TEST (TestForwardDynamics3DoFModelLagrangian) { + Model model; + + model.gravity = Vector3d (0., -9.81, 0.); + + Body null_body (0., Vector3d(0., 0., 0.), Vector3d (0., 0., 0.)); + Body base_body (1., Vector3d(0., 0.5, 0.), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + unsigned int base_id_rot_z, base_id_rot_y; + + // we can reuse both bodies and joints as they are copied + base_id_rot_z = model.AddBody (0, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, null_body); + base_id_rot_y = model.AddBody (base_id_rot_z, Xtrans (Vector3d(0., 0., 0.)), joint_rot_y, null_body); + model.AddBody (base_id_rot_y, Xtrans (Vector3d(0., 0., 0.)), joint_rot_x, base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model.dof_count, 0.); + + VectorNd QDDot_ab = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDDot_lagrangian = VectorNd::Constant ((size_t) model.dof_count, 0.); + + Q[1] = 1.; + ClearLogOutput(); + + Q[0] = 0.; + Q[1] = 1.; + Q[2] = 0.; + ForwardDynamicsLagrangian (model, Q, QDot, Tau, QDDot_lagrangian); + Q[0] = 0.; + Q[1] = 0.; + Q[2] = 1.; + ForwardDynamicsLagrangian (model, Q, QDot, Tau, QDDot_lagrangian); + ForwardDynamics (model, Q, QDot, Tau, QDDot_ab); + +// cout << QDDot_lagrangian << endl; +// cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (QDDot_ab.data(), QDDot_lagrangian.data(), QDDot_ab.size(), TEST_PREC); +} + +/* + * This is a test for a model where I detected incosistencies between the + * Lagragian method and the ABA. + */ +TEST (TestForwardDynamicsTwoLegModelLagrangian) { + Model *model = NULL; + + unsigned int hip_id, + // upper_leg_right_id, + // lower_leg_right_id, + foot_right_id, + // upper_leg_left_id, + // lower_leg_left_id, + foot_left_id; + Body hip_body, + upper_leg_right_body, + lower_leg_right_body, + foot_right_body, + upper_leg_left_body, + lower_leg_left_body, + foot_left_body; + + Joint joint_rot_z, joint_rot_y, joint_rot_x; + Joint joint_trans_z, joint_trans_y, joint_trans_x; + + ConstraintSet CS_right; + ConstraintSet CS_left; + ConstraintSet CS_both; + + model = new Model(); + + model->gravity = Vector3d (0., -9.81, 0.); + + joint_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + joint_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + joint_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + joint_trans_z = Joint ( SpatialVector (0., 0., 0., 0., 0., 1.)); + joint_trans_y = Joint ( SpatialVector (0., 0., 0., 0., 1., 0.)); + joint_trans_x = Joint ( SpatialVector (0., 0., 0., 1., 0., 0.)); + + Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.)); + + // hip + hip_body = Body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + + // lateral right + upper_leg_right_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + lower_leg_right_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + foot_right_body = Body (1., Vector3d (0.15, -0.1, 0.), Vector3d (1., 1., 1.)); + + // lateral left + upper_leg_left_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + lower_leg_left_body = Body (1., Vector3d (0., -0.25, 0.), Vector3d (1., 1., 1.)); + foot_left_body = Body (1., Vector3d (0.15, -0.1, 0.), Vector3d (1., 1., 1.)); + + // temporary value to store most recent body id + unsigned int temp_id; + + // add hip to the model (planar, 3 DOF) + temp_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_trans_x, null_body); + temp_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_trans_y, null_body); + hip_id = model->AddBody (temp_id, Xtrans (Vector3d (0., 0., 0.)), joint_rot_z, hip_body); + + // + // right leg + // + + // add right upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_right_body); + // upper_leg_right_id = temp_id; + + // add the right lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, lower_leg_right_body); + // lower_leg_right_id = temp_id; + + // add the right foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, foot_right_body); + foot_right_id = temp_id; + + // + // left leg + // + + // add left upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_left_body); + // upper_leg_left_id = temp_id; + + // add the left lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, lower_leg_left_body); + // lower_leg_left_id = temp_id; + + // add the left foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (Vector3d(0., -0.5, 0.)), joint_rot_z, foot_left_body); + foot_left_id = temp_id; + + LOG << "--- model created (" << model->dof_count << " DOF) ---" << endl; + + // contact data + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x"); + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y"); + CS_right.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z"); + + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x"); + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y"); + CS_left.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z"); + + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_right_x"); + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_right_y"); + CS_both.AddContactConstraint(foot_right_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_right_z"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (1., 0., 0.), "foot_left_x"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.), "foot_left_y"); + CS_both.AddContactConstraint(foot_left_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.), "foot_left_z"); + + CS_right.Bind(*model); + CS_left.Bind(*model); + CS_both.Bind(*model); + + VectorNd Q(model->dof_count); + VectorNd QDot(model->dof_count); + VectorNd Tau(model->dof_count); + VectorNd QDDot(model->dof_count); + VectorNd QDDotABA(model->dof_count); + + Q[0] = 0.8; + Q[1] = -7.76326e-06; + Q[2] = -1.58205e-07; + Q[3] = 1.57391e-07; + Q[4] = -1.03029e-09; + Q[5] = 7.92143e-08; + Q[6] = 1.57391e-07; + Q[7] = -1.03029e-09; + Q[8] = 7.92143e-08; + + QDot[0] = -1.77845e-06; + QDot[1] = -0.00905283; + QDot[2] = -0.000184484; + QDot[3] = 0.000183536; + QDot[4] = -1.20144e-06; + QDot[5] = 9.23727e-05; + QDot[6] = 0.000183536; + QDot[7] = -1.20144e-06; + QDot[8] = 9.23727e-05; + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 0.1; + Tau[4] = 0.1; + Tau[5] = 0.1; + Tau[6] = 0.1; + Tau[7] = 0.1; + Tau[8] = 0.1; + + // QDDot = 6.31843e-07 -6.12442e-07 9.22595e-14 3.3712e-07 4.27368e-07 -7.91795e-07 3.3712e-07 4.27368e-07 -7.91795e-07 + // QDDAB = -0.00192794 -9.81419 -0.2 0.198972 -0.00130243 0.100141 0.198972 -0.00130243 0.100141 + + ForwardDynamics (*model, Q, QDot, Tau, QDDotABA); + ClearLogOutput(); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot); + +// cout << LogOutput.str() << endl; + + // run it again to make sure the calculations give the same results and + // no invalid state information lingering in the model structure is being used + ForwardDynamics (*model, Q, QDot, Tau, QDDotABA); + ClearLogOutput(); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE (QDDotABA.data(), QDDot.data(), QDDotABA.size(), TEST_PREC); + + delete model; +} + +TEST_FIXTURE(FixedAndMovableJoint, TestForwardDynamicsFixedJoint) { + Q_fixed[0] = 1.1; + Q_fixed[1] = 2.2; + + QDot_fixed[0] = -3.2; + QDot_fixed[1] = -2.3; + + Tau_fixed[0] = 1.2; + Tau_fixed[1] = 2.1; + + Q = CreateDofVectorFromReducedVector (Q_fixed); + QDot = CreateDofVectorFromReducedVector (QDot_fixed); + + QDDot.setZero(); + + InverseDynamics (*model_movable, Q, QDot, QDDot, C_movable); + CompositeRigidBodyAlgorithm (*model_movable, Q, H_movable); + + H_fixed = CreateReducedInertiaMatrix (H_movable); + + C_fixed[0] = C_movable[0]; + C_fixed[1] = C_movable[2]; + + VectorNd QDDot_fixed_emulate(2); + CHECK (LinSolveGaussElimPivot (H_fixed, C_fixed * -1. + Tau_fixed, QDDot_fixed_emulate)); + + ForwardDynamics (*model_fixed, Q_fixed, QDot_fixed, Tau_fixed, QDDot_fixed); + + CHECK_ARRAY_CLOSE (QDDot_fixed_emulate.data(), QDDot_fixed.data(), 2, TEST_PREC); +} + +TEST_FIXTURE(FixedAndMovableJoint, TestInverseDynamicsFixedJoint) { + Q_fixed[0] = 1.1; + Q_fixed[1] = 2.2; + + QDot_fixed[0] = -3.2; + QDot_fixed[1] = -2.3; + + QDDot_fixed[0] = 1.2; + QDDot_fixed[1] = 2.1; + + Q = CreateDofVectorFromReducedVector (Q_fixed); + QDot = CreateDofVectorFromReducedVector (QDot_fixed); + QDDot = CreateDofVectorFromReducedVector (QDDot_fixed); + + InverseDynamics (*model_movable, Q, QDot, QDDot, Tau); + InverseDynamics (*model_fixed, Q_fixed, QDot_fixed, QDDot_fixed, Tau_fixed); + + VectorNd Tau_2dof (2); + Tau_2dof[0] = Tau[0]; + Tau_2dof[1] = Tau[2]; + + CHECK_ARRAY_CLOSE (Tau_2dof.data(), Tau_fixed.data(), 2, TEST_PREC); +} + +TEST_FIXTURE ( FloatingBase12DoF, TestForwardDynamicsLagrangianPrealloc ) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = static_cast(i + 1) * 0.1; + QDot[i] = static_cast(i + 1) * 1.1; + Tau[i] = static_cast(i + 1) * -1.2; + } + + ForwardDynamicsLagrangian (*model, + Q, + QDot, + Tau, + QDDot, + Math::LinearSolverPartialPivLU, + NULL, + NULL, + NULL + ); + + MatrixNd H (MatrixNd::Zero(model->dof_count, model->dof_count)); + VectorNd C (VectorNd::Zero(model->dof_count)); + VectorNd QDDot_prealloc (VectorNd::Zero (model->dof_count)); + ForwardDynamicsLagrangian (*model, + Q, + QDot, + Tau, + QDDot_prealloc, + Math::LinearSolverPartialPivLU, + NULL, + &H, + &C + ); + + CHECK_ARRAY_EQUAL (QDDot.data(), QDDot_prealloc.data(), model->dof_count); +} + +TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = rand() / static_cast(RAND_MAX); + Tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, Q, M); + + VectorNd qddot_solve_llt = M.llt().solve(Tau); + + VectorNd qddot_minv (Q); + CalcMInvTimesTau (*model, Q, Tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC); +} + +TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = rand() / static_cast(RAND_MAX); + Tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, Q, M); + + VectorNd qddot_solve_llt = M.llt().solve(Tau); + + VectorNd qddot_minv (Q); + CalcMInvTimesTau (*model, Q, Tau, qddot_minv); + + for (unsigned int j = 0; j < 1; j++) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Tau[i] = rand() / static_cast(RAND_MAX); + } + + CompositeRigidBodyAlgorithm (*model, Q, M); + qddot_solve_llt = M.llt().solve(Tau); + + CalcMInvTimesTau (*model, Q, Tau, qddot_minv, false); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC); + } +} + +TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesNonZeroQDotKinematicsUpdate) { + for (unsigned int i = 0; i < model->dof_count; i++) { + Q[i] = rand() / static_cast(RAND_MAX); + QDot[i] = rand() / static_cast(RAND_MAX); + Tau[i] = rand() / static_cast(RAND_MAX); + } + + UpdateKinematicsCustom(*model, &Q, &QDot, NULL); + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, Q, M); + + VectorNd qddot_solve_llt = M.llt().solve(Tau); + + VectorNd qddot_minv (Q); + CalcMInvTimesTau (*model, Q, Tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/Fixtures.h b/3rdparty/rbdl/tests/Fixtures.h new file mode 100644 index 0000000..2487d79 --- /dev/null +++ b/3rdparty/rbdl/tests/Fixtures.h @@ -0,0 +1,763 @@ +#include "rbdl/rbdl.h" + +struct FixedBase3DoF { + FixedBase3DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector(0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(1, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(2, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~FixedBase3DoF () { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +struct FixedBase6DoF { + FixedBase6DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_base_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + base_rot_z_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_z, base_rot_z); + + base_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_base_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + base_rot_y_id = model->AppendBody (Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_y, base_rot_y); + + base_rot_x = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_base_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + base_rot_x_id = model->AddBody (base_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_base_rot_x, base_rot_x); + + // child body (3 DoF) + child_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_rot_z_id = model->AddBody (base_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_rot_z, child_rot_z); + + child_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_rot_y_id = model->AddBody (child_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_y, child_rot_y); + + child_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_rot_x_id = model->AddBody (child_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_x, child_rot_x); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + contact_body_id = child_rot_x_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_rot_z_id, base_rot_y_id, base_rot_x_id, + child_rot_z_id, child_rot_y_id, child_rot_x_id, + base_body_id; + + RigidBodyDynamics::Body base_rot_z, base_rot_y, base_rot_x, + child_rot_z, child_rot_y, child_rot_x; + + RigidBodyDynamics::Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x, + joint_child_rot_z, joint_child_rot_y, joint_child_rot_x; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + unsigned int contact_body_id; + RigidBodyDynamics::Math::Vector3d contact_point; + RigidBodyDynamics::Math::Vector3d contact_normal; + RigidBodyDynamics::ConstraintSet constraint_set; +}; + +struct FloatingBase12DoF { + FloatingBase12DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + base_rot_x = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + base_rot_x_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_rot_x); + + // child body 1 (3 DoF) + child_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_rot_z_id = model->AddBody (base_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_rot_z, child_rot_z); + + child_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_rot_y_id = model->AddBody (child_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_y, child_rot_y); + + child_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_rot_x_id = model->AddBody (child_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_rot_x, child_rot_x); + + // child body (3 DoF) + child_2_rot_z = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_2_rot_z = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + child_2_rot_z_id = model->AddBody (child_rot_x_id, Xtrans (Vector3d (1., 0., 0.)), joint_child_2_rot_z, child_2_rot_z); + + child_2_rot_y = Body ( + 0., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_child_2_rot_y = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + child_2_rot_y_id = model->AddBody (child_2_rot_z_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_2_rot_y, child_2_rot_y); + + child_2_rot_x = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_child_2_rot_x = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + child_2_rot_x_id = model->AddBody (child_2_rot_y_id, Xtrans (Vector3d (0., 0., 0.)), joint_child_2_rot_x, child_2_rot_x); + + Q = VectorNd::Constant (model->dof_count, 0.); + QDot = VectorNd::Constant (model->dof_count, 0.); + QDDot = VectorNd::Constant (model->dof_count, 0.); + Tau = VectorNd::Constant (model->dof_count, 0.); + + ClearLogOutput(); + } + + ~FloatingBase12DoF () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_rot_z_id, base_rot_y_id, base_rot_x_id, + child_rot_z_id, child_rot_y_id, child_rot_x_id, + child_2_rot_z_id, child_2_rot_y_id,child_2_rot_x_id, + base_body_id; + + RigidBodyDynamics::Body base_rot_z, base_rot_y, base_rot_x, + child_rot_z, child_rot_y, child_rot_x, + child_2_rot_z, child_2_rot_y, child_2_rot_x; + + RigidBodyDynamics::Joint joint_base_rot_z, joint_base_rot_y, joint_base_rot_x, + joint_child_rot_z, joint_child_rot_y, joint_child_rot_x, + joint_child_2_rot_z, joint_child_2_rot_y, joint_child_2_rot_x; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; +}; + +struct SimpleFixture { + SimpleFixture () { + ClearLogOutput(); + model = new RigidBodyDynamics::Model; + model->gravity = RigidBodyDynamics::Math::Vector3d (0., -9.81, 0.); + } + ~SimpleFixture () { + delete model; + } + void ResizeVectors () { + Q = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + QDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + QDDot = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + Tau = RigidBodyDynamics::Math::VectorNd::Zero (model->dof_count); + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; +}; + +struct FixedJoint2DoF { + FixedJoint2DoF () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint (JointTypeFixed); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + ~FixedJoint2DoF () { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +/** \brief Fixture that contains two models of which one has one joint fixed. +*/ +struct FixedAndMovableJoint { + FixedAndMovableJoint () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model_movable = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * Z + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model_movable->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model_movable->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model_movable->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model_movable->dof_count, 0.); + C_movable = VectorNd::Zero ((size_t) model_movable->dof_count); + H_movable = MatrixNd::Zero ((size_t) model_movable->dof_count, (size_t) model_movable->dof_count); + + // Assemble the fixed joint model + model_fixed = new Model; + + body_a_fixed_id = model_fixed->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + Joint joint_fixed (JointTypeFixed); + body_b_fixed_id = model_fixed->AddBody(body_a_fixed_id, Xtrans(Vector3d(1., 0., 0.)), joint_fixed, body_b); + body_c_fixed_id = model_fixed->AddBody(body_b_fixed_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + Q_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + QDot_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + QDDot_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + Tau_fixed = VectorNd::Constant ((size_t) model_fixed->dof_count, 0.); + C_fixed = VectorNd::Zero ((size_t) model_fixed->dof_count); + H_fixed = MatrixNd::Zero ((size_t) model_fixed->dof_count, (size_t) model_fixed->dof_count); + + point_position = Vector3d::Zero (3); + point_acceleration = Vector3d::Zero (3); + + ref_body_id = 0; + + ClearLogOutput(); + } + + ~FixedAndMovableJoint () { + delete model_movable; + delete model_fixed; + } + RigidBodyDynamics::Math::VectorNd CreateDofVectorFromReducedVector (const RigidBodyDynamics::Math::VectorNd &q_fixed) { + assert (q_fixed.size() == model_fixed->dof_count); + + RigidBodyDynamics::Math::VectorNd q_movable (model_movable->dof_count); + + q_movable[0] = q_fixed[0]; + q_movable[1] = 0.; + q_movable[2] = q_fixed[1]; + + return q_movable; + } + + RigidBodyDynamics::Math::MatrixNd CreateReducedInertiaMatrix(const RigidBodyDynamics::Math::MatrixNd &H_movable) { + assert (H_movable.rows() == model_movable->dof_count); + assert (H_movable.cols() == model_movable->dof_count); + RigidBodyDynamics::Math::MatrixNd H (model_fixed->dof_count, model_fixed->dof_count); + + H (0,0) = H_movable(0,0); H (0,1) = H_movable(0,2); + H (1,0) = H_movable(2,0); H (1,1) = H_movable(2,2); + + return H; + } + + RigidBodyDynamics::Model *model_fixed; + RigidBodyDynamics::Model *model_movable; + + unsigned int body_a_id, body_b_id, body_c_id, ref_body_id; + unsigned int body_a_fixed_id, body_b_fixed_id, body_c_fixed_id; + + RigidBodyDynamics::Body body_a, body_b, body_c; + RigidBodyDynamics::Joint joint_a, joint_b, joint_c; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + RigidBodyDynamics::Math::VectorNd C_movable; + RigidBodyDynamics::Math::MatrixNd H_movable; + + RigidBodyDynamics::Math::VectorNd Q_fixed; + RigidBodyDynamics::Math::VectorNd QDot_fixed; + RigidBodyDynamics::Math::VectorNd QDDot_fixed; + RigidBodyDynamics::Math::VectorNd Tau_fixed; + RigidBodyDynamics::Math::VectorNd C_fixed; + RigidBodyDynamics::Math::MatrixNd H_fixed; + + RigidBodyDynamics::Math::Vector3d point_position, point_acceleration; +}; + +/** Model with two moving bodies and one fixed body +*/ +struct RotZRotZYXFixed { + RotZRotZYXFixed() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Body body_a(1., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + Body body_b(2., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + Body body_fixed(10., RigidBodyDynamics::Math::Vector3d (1., 0.4, 0.4), RigidBodyDynamics::Math::Vector3d (1., 1., 1.)); + + fixture_transform_a = Xtrans (RigidBodyDynamics::Math::Vector3d(1., 2., 3.)); + fixture_transform_b = Xtrans (RigidBodyDynamics::Math::Vector3d(4., 5., 6.)); + fixture_transform_fixed = Xtrans (RigidBodyDynamics::Math::Vector3d(-1., -2., -3.)); + + body_a_id = model->AddBody (0, fixture_transform_a, joint_rot_z, body_a); + body_b_id = model->AppendBody (fixture_transform_b, joint_rot_zyx, body_b); + body_fixed_id = model->AppendBody (fixture_transform_fixed, Joint(JointTypeFixed), body_fixed); + + ClearLogOutput(); + } + ~RotZRotZYXFixed() { + delete model; + } + + RigidBodyDynamics::Model *model; + + unsigned int body_a_id, body_b_id, body_fixed_id; + + RigidBodyDynamics::Math::SpatialTransform fixture_transform_a; + RigidBodyDynamics::Math::SpatialTransform fixture_transform_b; + RigidBodyDynamics::Math::SpatialTransform fixture_transform_fixed; +}; + +struct TwoArms12DoF { + TwoArms12DoF() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * *----O----* + * | | + * | | + * * * + * | | + * | | + * + */ + + Body body_upper = Body (1., Vector3d (0., -0.2, 0.), Vector3d (1.1, 1.3, 1.5)); + Body body_lower = Body (0.5, Vector3d(0., -0.15, 0.), Vector3d (0.3, 0.5, 0.2)); + + Joint joint_zyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + right_upper_arm = model->AppendBody (Xtrans (Vector3d (0., 0., -0.3)), joint_zyx, body_upper, "RightUpper"); + // model->AppendBody (Xtrans (Vector3d (0., -0.4, 0.)), joint_zyx, body_lower, "RightLower"); + left_upper_arm = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.3)), joint_zyx, body_upper, "LeftUpper"); + // model->AppendBody (Xtrans (Vector3d (0., -0.4, 0.)), joint_zyx, body_lower, "LeftLower"); + + q = VectorNd::Constant ((size_t) model->dof_count, 0.); + qdot = VectorNd::Constant ((size_t) model->dof_count, 0.); + qddot = VectorNd::Constant ((size_t) model->dof_count, 0.); + tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ClearLogOutput(); + } + ~TwoArms12DoF() { + delete model; + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qdot; + RigidBodyDynamics::Math::VectorNd qddot; + RigidBodyDynamics::Math::VectorNd tau; + + unsigned int right_upper_arm, left_upper_arm; + +}; + +struct FixedBase6DoF12DoFFloatingBase { + FixedBase6DoF12DoFFloatingBase () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* 3 DoF (rot.) joint at base + * 3 DoF (rot.) joint child origin + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + base_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + // child body 1 (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + // child body (3 DoF) + child_2 = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_2_id = model->AddBody (child_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child_2); + + Q = VectorNd::Constant (model->dof_count, 0.); + QDot = VectorNd::Constant (model->dof_count, 0.); + QDDot = VectorNd::Constant (model->dof_count, 0.); + Tau = VectorNd::Constant (model->dof_count, 0.); + + contact_body_id = child_id; + contact_point = Vector3d (0.5, 0.5, 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~FixedBase6DoF12DoFFloatingBase () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int base_id, child_id, child_2_id; + + RigidBodyDynamics::Body base, child, child_2; + + RigidBodyDynamics::Joint joint_rotzyx; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + unsigned int contact_body_id; + RigidBodyDynamics::Math::Vector3d contact_point; + RigidBodyDynamics::Math::Vector3d contact_normal; + RigidBodyDynamics::ConstraintSet constraint_set; +}; + + +struct LinearInvertedPendulumModel { + LinearInvertedPendulumModel () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., 0., -9.81); + + /* point mass + * / + * - - - o - - - + * / \ + * / prismatic joint to move parallel to the ground + * / + * X <- contact point at worl frame center + */ + + // point mass + point_mass_body = Body ( + 1., + Vector3d (0., 0., 0.), + Vector3d (0., 0., 0.) + ); + joint_trans_xy = Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.) + ); + point_mass_id = model->AddBody ( + 0, Xtrans (Vector3d (0., 0., 1.)), joint_trans_xy, point_mass_body + ); + + Q = VectorNd::Constant (model->dof_count, 0.); + QDot = VectorNd::Constant (model->dof_count, 0.); + QDDot = VectorNd::Constant (model->dof_count, 0.); + Tau = VectorNd::Constant (model->dof_count, 0.); + + contact_point = Vector3d (0.0, 0.0, 0.); + contact_normal = Vector3d (0., 0., 1.); + + ClearLogOutput(); + } + + ~LinearInvertedPendulumModel () { + delete model; + } + RigidBodyDynamics::Model *model; + + unsigned int point_mass_id; + + RigidBodyDynamics::Body point_mass_body; + + RigidBodyDynamics::Joint joint_trans_xy; + + RigidBodyDynamics::Math::VectorNd Q; + RigidBodyDynamics::Math::VectorNd QDot; + RigidBodyDynamics::Math::VectorNd QDDot; + RigidBodyDynamics::Math::VectorNd Tau; + + RigidBodyDynamics::Math::Vector3d contact_point; + RigidBodyDynamics::Math::Vector3d contact_normal; +}; diff --git a/3rdparty/rbdl/tests/FloatingBaseTests.cc b/3rdparty/rbdl/tests/FloatingBaseTests.cc new file mode 100644 index 0000000..b77b310 --- /dev/null +++ b/3rdparty/rbdl/tests/FloatingBaseTests.cc @@ -0,0 +1,550 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct FloatingBaseFixture { + FloatingBaseFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + + base = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + } + ~FloatingBaseFixture () { + delete model; + } + Model *model; + Body base; + unsigned int base_body_id; + + VectorNd q, qdot, qddot, tau; +}; + +TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) { + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + q = VectorNd::Constant(model->dof_count, 0.); + qdot = VectorNd::Constant(model->dof_count, 0.); + qddot = VectorNd::Constant(model->dof_count, 0.); + tau = VectorNd::Constant(model->dof_count, 0.); + + q[1] = 1.; + ForwardDynamics (*model, q, qdot, tau, qddot); + + Vector3d test_point; + + test_point = CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false); + CHECK_ARRAY_CLOSE (Vector3d (0., -1., 0.).data(), test_point.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) { + // floating base + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + // body_a + Body body_a (1., Vector3d (1., 0., 0), Vector3d (1., 1., 1.)); + Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(base_body_id, Xtrans(Vector3d(2., 0., 0.)), joint_a, body_a); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Zero ((size_t) model->dof_count); + VectorNd QDot = VectorNd::Zero ((size_t) model->dof_count); + VectorNd QDDot = VectorNd::Zero ((size_t) model->dof_count); + VectorNd Tau = VectorNd::Zero ((size_t) model->dof_count); + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + unsigned int i; + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC); + + // We rotate the base... let's see what happens... + Q[3] = 0.8; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC); + + // We apply a torqe let's see what happens... + Q[3] = 0.; + /* + rot_B[0] = 0.0; + X_B = XtransRotZYXEuler(pos_B, rot_B); + */ + + Tau[6] = 1.; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + for (i = 0; i < QDDot.size(); i++) { + LOG << "QDDot[" << i << "] = " << QDDot[i] << endl; + } + + for (i = 0; i < model->a.size(); i++) { + LOG << "a[" << i << "] = " << model->a.at(i) << endl; + } + + // std::cout << LogOutput.str() << std::endl; + + CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC); + CHECK_CLOSE (-8.8100, QDDot[1], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC); + CHECK_CLOSE (-1.0000, QDDot[3], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC); + CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC); + CHECK_CLOSE ( 2.0000, QDDot[6], TEST_PREC); +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) { + // floating base + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when moving along the X axis + QDot[0] = 1.; + Vector3d point_position(1., 0., 0.); + Vector3d point_velocity; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(1., point_velocity[0], TEST_PREC); + CHECK_CLOSE(0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; + + ClearLogOutput(); + + // Now we calculate the velocity when rotating around the Z axis + QDot[0] = 0.; + QDot[3] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(0., point_velocity[0], TEST_PREC); + CHECK_CLOSE(1., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; + + // Now we calculate the velocity when rotating around the Z axis and the + // base is rotated around the z axis by 90 degrees + ClearLogOutput(); + Q[3] = M_PI * 0.5; + QDot[3] = 1.; + + point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position); + + CHECK_CLOSE(-1., point_velocity[0], TEST_PREC); + CHECK_CLOSE(0., point_velocity[1], TEST_PREC); + CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + + LOG << "Point velocity = " << point_velocity << endl; + // cout << LogOutput.str() << endl; +} + +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (1., 0., 0.); + Vector3d point_base_position; + Vector3d point_base_velocity; + Vector3d point_base_velocity_reference; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + point_base_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + point_base_velocity_reference = Vector3d ( + -3.888503432977729e-01, + -3.171179347202455e-01, + 1.093894197498446e+00 + ); + + CHECK_ARRAY_CLOSE (point_base_velocity_reference.data(), point_base_velocity.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero qddot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * Here we omit the term of the generalized acceleration by setting qddot + * to zero. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + // call ForwardDynamics to update the model + ForwardDynamics(*model, q, qdot, tau, qddot); + qddot = VectorNd::Zero (qddot.size()); + + qdot = qdot; + + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + // we set the generalized acceleration to zero + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00 + ); + Vector3d humans_point_velocity ( + 3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00 + ); + Vector3d humans_point_acceleration ( + -5.302760158847160e+00, 6.541369639625232e+00, -4.795115077652286e+00 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero q and qdot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * + * Here we set q and qdot to zero and only take into account values that + * are dependent on qddot. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + qddot = VectorNd::Zero (qddot.size()); + + qddot[0] = 0.1; + qddot[1] = 1.1; + qddot[2] = 1.2; + qddot[3] = 1.3; + qddot[4] = 1.5; + qddot[5] = 1.7; + + // cout << "ref_body_id = " << ref_body_id << endl; + // cout << "point_body_position = " << point_body_position << endl; + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -1.900000000000000e+00, -1.800000000000000e+00, 0.000000000000000e+00 + ); + Vector3d humans_point_velocity ( + 0.000000000000000e+00, 0.000000000000000e+00, 0.000000000000000e+00 + ); + Vector3d humans_point_acceleration ( + 2.440000000000000e+00, -1.370000000000000e+00, 9.899999999999999e-01 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + +/** \brief Compares computation of acceleration values for zero q and qdot + * + * Ensures that computation of position, velocity, and acceleration of a + * point produce the same values as in an equivalent model that was + * created with the HuMAnS toolbox + * http://www.inrialpes.fr/bipop/software/humans/ . + * + * Here we set q and qdot to zero and only take into account values that + * are dependent on qddot. + */ +TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationFull) { + // floating base + base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + base_body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + unsigned int ref_body_id = base_body_id; + + // first we calculate the velocity when rotating around the Z axis + Vector3d point_body_position (-1.9, -1.8, 0.); + Vector3d point_world_position; + Vector3d point_world_velocity; + Vector3d point_world_acceleration; + + q[0] = 0.1; + q[1] = 1.1; + q[2] = 1.2; + q[3] = 1.3; + q[4] = 1.5; + q[5] = 1.7; + + qdot[0] = 0.1; + qdot[1] = 1.1; + qdot[2] = 1.2; + qdot[3] = 1.3; + qdot[4] = 1.5; + qdot[5] = 1.7; + + ForwardDynamics(*model, q, qdot, tau, qddot); + + qddot[0] = 0.1; + qddot[1] = 1.1; + qddot[2] = 1.2; + qddot[3] = 1.3; + qddot[4] = 1.5; + qddot[5] = 1.7; + + // cout << "ref_body_id = " << ref_body_id << endl; + // cout << "point_body_position = " << point_body_position << endl; + point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false); + point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position); + + ClearLogOutput(); + + point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position); + + Vector3d humans_point_position ( + -6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00 + ); + Vector3d humans_point_velocity ( + 3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00 + ); + Vector3d humans_point_acceleration ( + -4.993637532756404e+00, 1.043238173517606e+01, -6.948370826218673e-01 + ); + + // cout << LogOutput.str() << endl; + // + // cout << "q = " << q << endl; + // cout << "qdot = " << qdot << endl; + // cout << "qddot = " << qddot << endl; + // + // cout << "body_coords = " << point_body_position << endl; + // cout << "world_pos = " << point_world_position << endl; + // cout << "world_vel = " << point_world_velocity << endl; + // cout << "world_accel = " << point_world_acceleration << endl; + + CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); +} + + diff --git a/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc b/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc new file mode 100644 index 0000000..bff003a --- /dev/null +++ b/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc @@ -0,0 +1,231 @@ +#include +#include "rbdl/rbdl.h" +#include + + +#include "PendulumModels.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + +// Reduce an angle to the (-pi, pi] range. +// static double inRange(double angle) { +// while(angle > M_PI) { +// angle -= 2. * M_PI; +// } +// while(angle <= -M_PI) { +// angle += 2. * M_PI; +// } +// return angle; +// } + + +TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) { + DoublePerpendicularPendulumAbsoluteCoordinates dba + = DoublePerpendicularPendulumAbsoluteCoordinates(); + DoublePerpendicularPendulumJointCoordinates dbj + = DoublePerpendicularPendulumJointCoordinates(); + + //1. Set the pendulum modeled using joint coordinates to a specific + // state and then compute the spatial acceleration of the body. + dbj.q[0] = M_PI/3.0; //About z0 + dbj.q[1] = M_PI/6.0; //About y1 + dbj.qd[0] = M_PI; //About z0 + dbj.qd[1] = M_PI/2.0; //About y1 + dbj.tau[0]= 0.; + dbj.tau[1]= 0.; + + std::vector < SpatialVector > fext_dbj,fext_dba; + + fext_dbj.resize( dbj.model.mBodies.size() ); + fext_dba.resize( dba.model.mBodies.size() ); + + for(unsigned int i=0; igravity = Vector3d (0., 0., -9.81); + + body_id_emulated[BodyPelvis] = model_emulated->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + body_id_emulated[BodyThighRight] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_r"); + body_id_emulated[BodyShankRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + body_id_emulated[BodyFootRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + body_id_emulated[BodyThighLeft] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_l"); + body_id_emulated[BodyShankLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + body_id_emulated[BodyFootLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + body_id_emulated[BodyMiddleTrunk] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_emulated, middle_trunk_body, "middletrunk"); + body_id_emulated[BodyUpperTrunk] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + body_id_emulated[BodyUpperArmRight] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_r"); + body_id_emulated[BodyLowerArmRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + body_id_emulated[BodyHandRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + body_id_emulated[BodyUpperArmLeft] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_l"); + body_id_emulated[BodyLowerArmLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + body_id_emulated[BodyHandLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + body_id_emulated[BodyHead] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "head"); + + // Generate 3dof model + model_3dof->gravity = Vector3d (0., 0., -9.81); + + unsigned int pelvis_trans = model_3dof->AddBody (0, Xtrans(Vector3d (0., 0., 0.)), trans_xyz, null_body, "pelvis_trans_xyz"); + + body_id_3dof[BodyPelvis] = model_3dof->AddBody (pelvis_trans, Xtrans (Vector3d (0., 0., 0.)), rot_yxz_3dof, pelvis_body, "pelvis"); + // body_id_3dof[BodyPelvis] = model_3dof->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis"); + + // right leg + body_id_3dof[BodyThighRight] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_r"); + body_id_3dof[BodyShankRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r"); + body_id_3dof[BodyFootRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r"); + + // left leg + body_id_3dof[BodyThighLeft] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_l"); + body_id_3dof[BodyShankLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l"); + body_id_3dof[BodyFootLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l"); + + // trunk + body_id_3dof[BodyMiddleTrunk] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_3dof, middle_trunk_body, "middletrunk"); + body_id_3dof[BodyUpperTrunk] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk"); + + // right arm + body_id_3dof[BodyUpperArmRight] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_r"); + body_id_3dof[BodyLowerArmRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r"); + body_id_3dof[BodyHandRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r"); + + // left arm + body_id_3dof[BodyUpperArmLeft] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_l"); + body_id_3dof[BodyLowerArmLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l"); + body_id_3dof[BodyHandLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l"); + + // head + body_id_3dof[BodyHead] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "head"); + } + + void initConstraintSets () { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + unsigned int foot_r_emulated = model_emulated->GetBodyId ("foot_r"); + unsigned int foot_l_emulated = model_emulated->GetBodyId ("foot_l"); + unsigned int hand_r_emulated = model_emulated->GetBodyId ("hand_r"); + unsigned int hand_l_emulated = model_emulated->GetBodyId ("hand_l"); + + constraints_1B1C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B1C_emulated.Bind (*model_emulated); + + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_emulated.Bind (*model_emulated); + + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_emulated.Bind (*model); + + unsigned int foot_r_3dof = model_3dof->GetBodyId ("foot_r"); + unsigned int foot_l_3dof = model_3dof->GetBodyId ("foot_l"); + unsigned int hand_r_3dof = model_3dof->GetBodyId ("hand_r"); + unsigned int hand_l_3dof = model_3dof->GetBodyId ("hand_l"); + + constraints_1B1C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B1C_3dof.Bind (*model_3dof); + + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_1B4C_3dof.Bind (*model_3dof); + + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.)); + constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.)); + constraints_4B4C_3dof.Bind (*model_3dof); + } + + void randomizeStates () { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.5 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + qddot_emulated = qddot; + qddot_3dof = qddot; + } + + Human36 () { + ClearLogOutput(); + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + initParameters(); + model_emulated = new RigidBodyDynamics::Model(); + model_3dof = new RigidBodyDynamics::Model(); + model = model_emulated; + generate(); + initConstraintSets(); + + q = VectorNd::Zero (model_emulated->q_size); + qdot = VectorNd::Zero (model_emulated->qdot_size); + qddot = VectorNd::Zero (model_emulated->qdot_size); + tau = VectorNd::Zero (model_emulated->qdot_size); + + qddot_emulated = VectorNd::Zero (model_emulated->qdot_size); + qddot_3dof= VectorNd::Zero (model_emulated->qdot_size); + }; + ~Human36 () { + delete model_emulated; + delete model_3dof; + } + +}; + +#endif diff --git a/3rdparty/rbdl/tests/ImpulsesTests.cc b/3rdparty/rbdl/tests/ImpulsesTests.cc new file mode 100644 index 0000000..265d152 --- /dev/null +++ b/3rdparty/rbdl/tests/ImpulsesTests.cc @@ -0,0 +1,281 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ImpulsesFixture { + ImpulsesFixture () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + // base body + base = Body ( + 1., + Vector3d (0., 1., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body (3 DoF) + child = Body ( + 1., + Vector3d (0., 1., 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + Q = VectorNd::Zero(model->dof_count); + QDot = VectorNd::Zero(model->dof_count); + QDDot = VectorNd::Zero(model->dof_count); + Tau = VectorNd::Zero(model->dof_count); + + contact_body_id = child_id; + contact_point = Vector3d (0., 1., 0.); + contact_normal = Vector3d (0., 1., 0.); + + ClearLogOutput(); + } + + ~ImpulsesFixture () { + delete model; + } + Model *model; + + unsigned int base_id, child_id; + Body base, child; + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; + + unsigned int contact_body_id; + Vector3d contact_point; + Vector3d contact_normal; + ConstraintSet constraint_set; +}; + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 0.; + constraint_set.v_plus[1] = 0.; + constraint_set.v_plus[2] = 0.; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 0.; + constraint_set.v_plus[1] = 0.; + constraint_set.v_plus[2] = 0.; + + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) { + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + Vector3d point_velocity; + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, QDot, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + + VectorNd qdot_post (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post); + + // cout << LogOutput.str() << endl; + // cout << "QdotPost = " << qdot_post << endl; + + { + SUPPRESS_LOGGING; + point_velocity = CalcPointVelocity (*model, Q, qdot_post, contact_body_id, contact_point, true); + } + + // cout << "Point Velocity = " << point_velocity << endl; + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) { + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + ConstraintSet constraint_set_rangespace; + constraint_set_rangespace = constraint_set.Copy(); + constraint_set_rangespace.Bind (*model); + + VectorNd qdot_post_direct (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post_direct); + + VectorNd qdot_post_rangespace (QDot.size()); + ComputeConstraintImpulsesRangeSpaceSparse (*model, Q, QDot, constraint_set_rangespace, qdot_post_rangespace); + + Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true); + Vector3d point_velocity_rangespace = CalcPointVelocity (*model, Q, qdot_post_rangespace, contact_body_id, contact_point, true); + + CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_rangespace.data(), qdot_post_direct.rows(), TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_rangespace.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) { + Q[0] = 0.2; + Q[1] = -0.5; + Q[2] = 0.1; + Q[3] = -0.4; + Q[4] = -0.1; + Q[5] = 0.4; + + QDot[0] = 0.1; + QDot[1] = -0.2; + QDot[2] = 0.1; + + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); + constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); + + constraint_set.Bind (*model); + + constraint_set.v_plus[0] = 1.; + constraint_set.v_plus[1] = 2.; + constraint_set.v_plus[2] = 3.; + + ConstraintSet constraint_set_nullspace; + constraint_set_nullspace = constraint_set.Copy(); + constraint_set_nullspace.Bind (*model); + + VectorNd qdot_post_direct (QDot.size()); + ComputeConstraintImpulsesDirect (*model, Q, QDot, constraint_set, qdot_post_direct); + + VectorNd qdot_post_nullspace (QDot.size()); + ComputeConstraintImpulsesNullSpace (*model, Q, QDot, constraint_set, qdot_post_nullspace); + + Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true); + Vector3d point_velocity_nullspace = CalcPointVelocity (*model, Q, qdot_post_nullspace, contact_body_id, contact_point, true); + + CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_nullspace.data(), qdot_post_direct.rows(), TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_nullspace.data(), 3, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/InverseDynamicsTests.cc b/3rdparty/rbdl/tests/InverseDynamicsTests.cc new file mode 100644 index 0000000..943bd8d --- /dev/null +++ b/3rdparty/rbdl/tests/InverseDynamicsTests.cc @@ -0,0 +1,76 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct InverseDynamicsFixture { + InverseDynamicsFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~InverseDynamicsFixture () { + delete model; + } + Model *model; +}; + +#ifndef USE_SLOW_SPATIAL_ALGEBRA +TEST_FIXTURE(InverseDynamicsFixture, TestInverseForwardDynamicsFloatingBase) { + Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + // Initialization of the input vectors + VectorNd Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + VectorNd TauInv = VectorNd::Constant ((size_t) model->dof_count, 0.); + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.1; + Q[4] = 0.2; + Q[5] = 0.3; + + QDot[0] = 1.1; + QDot[1] = -1.2; + QDot[2] = 1.3; + QDot[3] = -0.1; + QDot[4] = 0.2; + QDot[5] = -0.3; + + Tau[0] = 2.1; + Tau[1] = 2.2; + Tau[2] = 2.3; + Tau[3] = 1.1; + Tau[4] = 1.2; + Tau[5] = 1.3; + + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + InverseDynamics(*model, Q, QDot, QDDot, TauInv); + + CHECK_ARRAY_CLOSE (Tau.data(), TauInv.data(), Tau.size(), TEST_PREC); +} +#endif diff --git a/3rdparty/rbdl/tests/InverseKinematicsTests.cc b/3rdparty/rbdl/tests/InverseKinematicsTests.cc new file mode 100644 index 0000000..a50065a --- /dev/null +++ b/3rdparty/rbdl/tests/InverseKinematicsTests.cc @@ -0,0 +1,272 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" + +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +void print_ik_set (const InverseKinematicsConstraintSet &cs) { + int label_width = 18; + cout.width (label_width); + cout << "lambda: " << cs.lambda << endl; + cout.width (label_width); + cout << "num_steps: " << cs.num_steps << endl; + cout.width (label_width); + cout << "max_steps: " << cs.max_steps << endl; + cout.width (label_width); + cout << "step_tol: " << cs.step_tol << endl; + cout.width (label_width); + cout << "constraint_tol: " << cs.constraint_tol << endl; + cout.width (label_width); + cout << "error_norm: " << cs.error_norm << endl; +} + +/// Checks whether a single point in a 3-link chain can be reached +TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + + Vector3d local_point (1., 0., 0.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point); + + q.setZero(); + + InverseKinematicsConstraintSet cs; + cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point, target_position); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + if (!result) { + print_ik_set (cs); + } + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); +} + + +TEST_FIXTURE ( Human36, ManyPointConstraints ) { + + randomizeStates(); + + Vector3d local_point1 (1., 0., 0.); + Vector3d local_point2 (-1., 0., 0.); + Vector3d local_point3 (0., 1., 0.); + Vector3d local_point4 (1., 0., 1.); + Vector3d local_point5 (0.,0.,-1.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1); + Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2); + Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3); + Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4); + Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5); + + q.setZero(); + UpdateKinematicsCustom (*model, &q, NULL, NULL); + + InverseKinematicsConstraintSet cs; + cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1); + cs.AddPointConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2); + cs.AddPointConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3); + cs.AddPointConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4); + cs.AddPointConstraint (body_id_emulated[BodyHead], local_point5, target_position5); + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1); + Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2); + Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3); + Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4); + Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5); + + CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); +} + +/// Checks whether the end of a 3-link chain can aligned with a given +// orientation. +TEST_FIXTURE ( Human36, ChainSingleBodyOrientation ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + + InverseKinematicsConstraintSet cs; + cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation); + + VectorNd qres (q); + q.setZero(); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); +} + +TEST_FIXTURE ( Human36, ManyBodyOrientations ) { + + randomizeStates(); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false); + Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false); + Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false); + Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false); + + q.setZero(); + + InverseKinematicsConstraintSet cs; + cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation1); + cs.AddOrientationConstraint (body_id_emulated[BodyFootLeft], target_orientation2); + cs.AddOrientationConstraint (body_id_emulated[BodyHandRight], target_orientation3); + cs.AddOrientationConstraint (body_id_emulated[BodyHandLeft], target_orientation4); + cs.AddOrientationConstraint (body_id_emulated[BodyHead], target_orientation5); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, TEST_PREC); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false); + Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false); + Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false); + Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false); + Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false); + + CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); +} + +TEST_FIXTURE ( Human36, ChainSingleBodyFullConstraint ) { + q[HipRightRY] = 0.3; + q[HipRightRX] = 0.3; + q[HipRightRZ] = 0.3; + q[KneeRightRY] = 0.3; + q[AnkleRightRY] = 0.3; + q[AnkleRightRZ] = 0.3; + Vector3d local_point (1., 0., 0.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point); + + InverseKinematicsConstraintSet cs; + cs.AddFullConstraint(body_id_emulated[BodyFootRight],local_point,target_position, target_orientation); + + VectorNd qres (q); + q.setZero(); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); +} + +TEST_FIXTURE ( Human36, ManyBodyFullConstraints ) { + + randomizeStates(); + + Vector3d local_point1 (1., 0., 0.); + Vector3d local_point2 (-1., 0., 0.); + Vector3d local_point3 (0., 1., 0.); + Vector3d local_point4 (1., 0., 1.); + Vector3d local_point5 (0.,0.,-1.); + + UpdateKinematicsCustom (*model, &q, NULL, NULL); + + Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1); + Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2); + Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3); + Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4); + Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5); + + Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false); + Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false); + Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false); + Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false); + Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false); + + InverseKinematicsConstraintSet cs; + cs.AddFullConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1, target_orientation1); + cs.AddFullConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2, target_orientation2); + cs.AddFullConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3, target_orientation3); + cs.AddFullConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4, target_orientation4); + cs.AddFullConstraint (body_id_emulated[BodyHead], local_point5, target_position5, target_orientation5); + cs.step_tol = 1e-12; + + q.setZero(); + + VectorNd qres (q); + + bool result = InverseKinematics (*model, q, cs, qres); + + CHECK (result); + + CHECK_CLOSE (0., cs.error_norm, cs.step_tol); + + UpdateKinematicsCustom (*model, &qres, NULL, NULL); + Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false); + Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false); + Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false); + Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false); + Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false); + + Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1); + Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2); + Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3); + Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4); + Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5); + + CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); + CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); + + CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/KinematicsTests.cc b/3rdparty/rbdl/tests/KinematicsTests.cc new file mode 100644 index 0000000..e939431 --- /dev/null +++ b/3rdparty/rbdl/tests/KinematicsTests.cc @@ -0,0 +1,680 @@ +#include + +#include + +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +#include "Human36Fixture.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct KinematicsFixture { + KinematicsFixture () { + ClearLogOutput(); + model = new Model; + + /* Basically a model like this, where X are the Center of Masses + * and the CoM of the last (3rd) body comes out of the Y=X=0 plane. + * + * X + * * + * _/ + * _/ (-Z) + * Z / + * *---* + * | + * | + * Z | + * O---* + * Y + */ + + body_a = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_a = Joint( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_a_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_a, body_a); + + body_b = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.)); + joint_b = Joint ( SpatialVector (0., 1., 0., 0., 0., 0.)); + + body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b); + + body_c = Body (1., Vector3d (0., 0., 1.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + body_c_id = model->AddBody(body_b_id, Xtrans(Vector3d(0., 1., 0.)), joint_c, body_c); + + body_d = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + joint_c = Joint ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + body_d_id = model->AddBody(body_c_id, Xtrans(Vector3d(0., 0., -1.)), joint_c, body_d); + + Q = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + QDDot = VectorNd::Constant ((size_t) model->dof_count, 0.); + Tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + ClearLogOutput(); + } + + ~KinematicsFixture () { + delete model; + } + Model *model; + + unsigned int body_a_id, body_b_id, body_c_id, body_d_id; + Body body_a, body_b, body_c, body_d; + Joint joint_a, joint_b, joint_c, joint_d; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; +}; + +struct KinematicsFixture6DoF { + KinematicsFixture6DoF () { + ClearLogOutput(); + model = new Model; + + model->gravity = Vector3d (0., -9.81, 0.); + + /* + * + * X Contact point (ref child) + * | + * Base | + * / body | + * O-------* + * \ + * Child body + */ + + // base body (3 DoF) + base = Body ( + 1., + Vector3d (0.5, 0., 0.), + Vector3d (1., 1., 1.) + ); + joint_rotzyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + base_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_rotzyx, base); + + // child body (3 DoF) + child = Body ( + 1., + Vector3d (0., 0.5, 0.), + Vector3d (1., 1., 1.) + ); + child_id = model->AddBody (base_id, Xtrans (Vector3d (1., 0., 0.)), joint_rotzyx, child); + + Q = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + QDDot = VectorNd::Constant (model->mBodies.size() - 1, 0.); + Tau = VectorNd::Constant (model->mBodies.size() - 1, 0.); + + ClearLogOutput(); + } + + ~KinematicsFixture6DoF () { + delete model; + } + Model *model; + + unsigned int base_id, child_id; + Body base, child; + Joint joint_rotzyx; + + VectorNd Q; + VectorNd QDot; + VectorNd QDDot; + VectorNd Tau; +}; + + + +TEST_FIXTURE(KinematicsFixture, TestPositionNeutral) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotated90Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + + Q[0] = 0.5 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + // cout << LogOutput.str() << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (-1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (-1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotatedNeg45Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + + Q[0] = -0.25 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + // cout << LogOutput.str() << endl; + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0.707106781186547, -0.707106781186547, 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotated90Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + Q[1] = 0.5 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotatedNeg45Deg) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + Q[1] = -0.25 * M_PI; + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + Vector3d body_position; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); + CHECK_ARRAY_CLOSE (Vector3d (1 + 0.707106781186547, 1., -0.707106781186547), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); +} + +TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinates) { + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 2., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)), + 3, TEST_PREC + ); +} + +TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinatesRotated) { + Q[2] = 0.5 * M_PI; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 1., 0.).data(), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false).data(), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (0., 1., 0.).data(), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false).data(), + 3, TEST_PREC + ); + + // Rotate the other way round + Q[2] = -0.5 * M_PI; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (1., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (2., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), + 3, TEST_PREC + ); + + // Rotate around the base + Q[0] = 0.5 * M_PI; + Q[2] = 0.; + + // We call ForwardDynamics() as it updates the spatial transformation + // matrices + ForwardDynamics(*model, Q, QDot, Tau, QDDot); + + CHECK_ARRAY_CLOSE ( + Vector3d (-1., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), + 3, TEST_PREC + ); + + CHECK_ARRAY_CLOSE ( + Vector3d (-2., 1., 0.), + CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), + 3, TEST_PREC + ); + + // cout << LogOutput.str() << endl; +} + +TEST(TestCalcPointJacobian) { + Model model; + Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.)); + + unsigned int base_body_id = model.AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + base_body); + + VectorNd Q = VectorNd::Constant ((size_t) model.dof_count, 0.); + VectorNd QDot = VectorNd::Constant ((size_t) model.dof_count, 0.); + MatrixNd G = MatrixNd::Constant (3, model.dof_count, 0.); + Vector3d point_position (1.1, 1.2, 2.1); + Vector3d point_velocity_ref; + Vector3d point_velocity; + + Q[0] = 1.1; + Q[1] = 1.2; + Q[2] = 1.3; + Q[3] = 0.7; + Q[4] = 0.8; + Q[5] = 0.9; + + QDot[0] = -1.1; + QDot[1] = 2.2; + QDot[2] = 1.3; + QDot[3] = -2.7; + QDot[4] = 1.8; + QDot[5] = -2.9; + + // Compute the reference velocity + point_velocity_ref = CalcPointVelocity (model, Q, QDot, base_body_id, point_position); + + G.setZero(); + CalcPointJacobian (model, Q, base_body_id, point_position, G); + + point_velocity = G * QDot; + + CHECK_ARRAY_CLOSE ( + point_velocity_ref.data(), + point_velocity.data(), + 3, TEST_PREC + ); +} + +TEST_FIXTURE(KinematicsFixture, TestInverseKinematicSimple) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = body_d_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (1.3, 0., 0.); + + body_ids.push_back (body_d_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres); + // cout << LogOutput.str() << endl; + CHECK_EQUAL (true, res); + + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false); + + CHECK_ARRAY_CLOSE (target.data(), effector.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicUnreachable) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = child_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (2.2, 0., 0.); + + body_ids.push_back (body_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-8, 0.9, 1000); + // cout << LogOutput.str() << endl; + CHECK_EQUAL (true, res); + + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false); + + CHECK_ARRAY_CLOSE (Vector3d (2.0, 0., 0.).data(), effector.data(), 3, 1.0e-7); +} + +TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicTwoPoints) { + std::vector body_ids; + std::vector body_points; + std::vector target_pos; + + Q[0] = 0.2; + Q[1] = 0.1; + Q[2] = 0.1; + + VectorNd Qres = VectorNd::Zero ((size_t) model->dof_count); + + unsigned int body_id = child_id; + Vector3d body_point = Vector3d (1., 0., 0.); + Vector3d target (2., 0., 0.); + + body_ids.push_back (body_id); + body_points.push_back (body_point); + target_pos.push_back (target); + + body_ids.push_back (base_id); + body_points.push_back (Vector3d (0.6, 1.0, 0.)); + target_pos.push_back (Vector3d (0.5, 1.1, 0.)); + + ClearLogOutput(); + bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-3, 0.9, 200); + CHECK_EQUAL (true, res); + + // cout << LogOutput.str() << endl; + UpdateKinematicsCustom (*model, &Qres, NULL, NULL); + + Vector3d effector; + + // testing with very low precision + effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[0], body_points[0], false); + CHECK_ARRAY_CLOSE (target_pos[0].data(), effector.data(), 3, 1.0e-1); + + effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[1], body_points[1], false); + CHECK_ARRAY_CLOSE (target_pos[1].data(), effector.data(), 3, 1.0e-1); +} + +TEST ( FixedJointBodyCalcBodyToBase ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 1., 0.1)); + + CHECK_ARRAY_CLOSE (Vector3d (1., 2., 0.1).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBodyToBaseRotated ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector(0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + + ClearLogOutput(); + Q[0] = M_PI * 0.5; + Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q, fixed_body_id, Vector3d (1., 0., 0.)); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBaseToBody ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 2., 0.1)); + + CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.1).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyCalcBaseToBodyRotated ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(1., 0., 0.)), Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + + ClearLogOutput(); + Q[0] = M_PI * 0.5; + Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q, fixed_body_id, Vector3d (0., 2., 0.)); + // cout << LogOutput.str() << endl; + + CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.).data(), base_coords.data(), 3, TEST_PREC); +} + +TEST ( FixedJointBodyWorldOrientation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xrotz(0.25) * Xtrans (Vector3d (1., 2., 3.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body); + + VectorNd Q_zero = VectorNd::Zero (model.dof_count); + Matrix3d orientation = CalcBodyWorldOrientation (model, Q_zero, fixed_body_id); + + Matrix3d reference = transform.E; + + CHECK_ARRAY_CLOSE (reference.data(), orientation.data(), 9, TEST_PREC); +} + +TEST ( FixedJointCalcPointJacobian ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + + SpatialTransform transform = Xrotz(0.25) * Xtrans (Vector3d (1., 2., 3.)); + unsigned int fixed_body_id = model.AppendBody (transform, Joint(JointTypeFixed), fixed_body); + + VectorNd Q = VectorNd::Zero (model.dof_count); + VectorNd QDot = VectorNd::Zero (model.dof_count); + + Q[0] = 1.1; + QDot[0] = 1.2; + + Vector3d point_position (1., 0., 0.); + + MatrixNd G = MatrixNd::Zero (3, model.dof_count); + CalcPointJacobian (model, Q, fixed_body_id, point_position, G); + Vector3d point_velocity_jacobian = G * QDot; + Vector3d point_velocity_reference = CalcPointVelocity (model, Q, QDot, fixed_body_id, point_position); + + CHECK_ARRAY_CLOSE (point_velocity_reference.data(), point_velocity_jacobian.data(), 3, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SpatialJacobianSimple ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + + CalcBodySpatialJacobian (*model, q, foot_r_id, G); + + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_body = SpatialVector(G * qdot); + + CHECK_ARRAY_CLOSE (model->v[foot_r_id].data(), v_body.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SpatialJacobianFixedBody ) { + randomizeStates(); + + unsigned int uppertrunk_id = model->GetBodyId ("uppertrunk"); + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + + CalcBodySpatialJacobian (*model, q, uppertrunk_id, G); + + unsigned int fixed_body_id = uppertrunk_id - model->fixed_body_discriminator; + unsigned int movable_parent = model->mFixedBodies[fixed_body_id].mMovableParent; + + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_body = SpatialVector(G * qdot); + + SpatialVector v_fixed_body = model->mFixedBodies[fixed_body_id].mParentTransform.apply (model->v[movable_parent]); + + CHECK_ARRAY_CLOSE (v_fixed_body.data(), v_body.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointJacobian6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D velocity using the 6-D Jacobian + MatrixNd G (MatrixNd::Zero (6, model->dof_count)); + CalcPointJacobian6D (*model, q, foot_r_id, point_local, G); + SpatialVector v_foot_0_jac = SpatialVector (G * qdot); + + // Compute the 6-D velocity by transforming the body velocity to the + // reference point and aligning it with the base coordinate system + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id])); + + CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0_jac.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointVelocity6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D velocity + SpatialVector v_foot_0 = CalcPointVelocity6D (*model, q, qdot, foot_r_id, point_local); + + // Compute the 6-D velocity by transforming the body velocity to the + // reference point and aligning it with the base coordinate system + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id])); + + CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0.data(), 6, TEST_PREC); +} + +TEST_FIXTURE ( Human36, CalcPointAcceleration6D ) { + randomizeStates(); + + unsigned int foot_r_id = model->GetBodyId ("foot_r"); + Vector3d point_local (1.1, 2.2, 3.3); + + // Compute the 6-D acceleration + SpatialVector a_foot_0 = CalcPointAcceleration6D (*model, q, qdot, qddot, foot_r_id, point_local); + + // Compute the 6-D acceleration by adding the coriolis term to the + // acceleration of the body and transforming the result to the + // point and align it with the base coordinate system. + Vector3d r_point = CalcBodyToBaseCoordinates (*model, q, foot_r_id, point_local); + Vector3d v_foot_0 = CalcPointVelocity (*model, q, qdot, foot_r_id, point_local); + SpatialVector rdot (0., 0., 0., v_foot_0[0], v_foot_0[1], v_foot_0[2]); + + SpatialTransform X_foot (Matrix3d::Identity(), r_point); + UpdateKinematicsCustom (*model, &q, &qdot, NULL); + SpatialVector a_foot_0_ref = X_foot.apply( + model->X_base[foot_r_id].inverse().apply(model->a[foot_r_id]) + - crossm(rdot, + model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id]) + ) + ); + + CHECK_ARRAY_CLOSE (a_foot_0_ref.data(), a_foot_0.data(), 6, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/LoopConstraintsTests.cc b/3rdparty/rbdl/tests/LoopConstraintsTests.cc new file mode 100644 index 0000000..36ca6d4 --- /dev/null +++ b/3rdparty/rbdl/tests/LoopConstraintsTests.cc @@ -0,0 +1,2362 @@ +#include +#include "rbdl/rbdl.h" +#include + +#include "Fixtures.h" +#include "PendulumModels.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-11; + +// Reduce an angle to the (-pi, pi] range. +static double inRange(double angle) { + while(angle > M_PI) { + angle -= 2. * M_PI; + } + while(angle <= -M_PI) { + angle += 2. * M_PI; + } + return angle; +} + + +struct FourBarLinkage { + + FourBarLinkage() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(2.) + , l2(2.) + , m1(2.) + , m2(2.) + , idB1(0) + , idB2(0) + , idB3(0) + , idB4(0) + , idB5(0) + , X_p(Xtrans(Vector3d(l2, 0., 0.))) + , X_s(Xtrans(Vector3d(0., 0., 0.))) { + + Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.) + , Vector3d(0., 0., m1 * l1 * l1 / 3.)); + Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.) + , Vector3d(0., 0., m2 * l2 * l2 / 3.)); + Vector3d vector3d_zero = Vector3d::Zero(); + Body virtual_body(0., vector3d_zero, vector3d_zero); + Joint joint_rev_z(JointTypeRevoluteZ); + + idB1 = model.AddBody(0 , Xtrans(Vector3d(0., 0., 0.)), + joint_rev_z, link1); + idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), + joint_rev_z, link2); + idB3 = model.AddBody(0 , Xtrans(Vector3d(0., 0., 0.)), + joint_rev_z, link1); + idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), + joint_rev_z, link2); + idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), + joint_rev_z + , virtual_body); + + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0,0,0,1,0,0), true, 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0,0,0,0,1,0), true, 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0,0,1,0,0,0), true, 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB1; + unsigned int idB2; + unsigned int idB3; + unsigned int idB4; + unsigned int idB5; + + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct FloatingFourBarLinkage { + + FloatingFourBarLinkage() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(2.) + , l2(2.) + , m1(2.) + , m2(2.) + , idB0(0) + , idB1(0) + , idB2(0) + , idB3(0) + , idB4(0) + , idB5(0) + , X_p(Xtrans(Vector3d(l2, 0., 0.))) + , X_s(Xtrans(Vector3d(0., 0., 0.))) { + + Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.) + , Vector3d(0., 0., m1 * l1 * l1 / 3.)); + Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.) + , Vector3d(0., 0., m2 * l2 * l2 / 3.)); + Vector3d vector3d_zero = Vector3d::Zero(); + Body virtual_body(0., vector3d_zero, vector3d_zero); + Joint joint_trans(JointTypeTranslationXYZ); + Joint joint_rev_z(JointTypeRevoluteZ); + + idB0 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_trans + , virtual_body); + idB1 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), + joint_rev_z, link1); + idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), + joint_rev_z, link2); + idB3 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), + joint_rev_z, link1); + idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), + joint_rev_z, link2); + idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), + joint_rev_z + , virtual_body); + + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(1.,0.,0.)); + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0.,1.,0.)); + cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0.,0.,1.)); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0.,0.,0.,1.,0.,0.), true, 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0.,0.,0.,0.,1.,0.), true, 0.1); + cs.AddLoopConstraint(idB2, idB5, X_p, X_s, + SpatialVector(0.,0.,1.,0.,0.,0.), true, 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB0; + unsigned int idB1; + unsigned int idB2; + unsigned int idB3; + unsigned int idB4; + unsigned int idB5; + + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct SliderCrank3D { + + SliderCrank3D() + : model() + , cs() + , q() + , qd() + , id_p(0) + , id_s(0) + , X_p() + , X_s() { + + double slider_mass = 5.; + double slider_height = 0.1; + double crank_link1_mass = 3.; + double crank_link1_length = 1.; + double crank_link2_mass = 1.; + double crank_link2_radius = 0.2; + double crank_link2_length = 3.; + double crank_link1_height = crank_link2_length - crank_link1_length + + slider_height; + + Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.)); + Body crankLink1(crank_link1_mass + , Vector3d(0.5 * crank_link1_length, 0., 0.) + , Vector3d(0., 0. + , crank_link1_mass * crank_link1_length * crank_link1_length / 3.)); + Body crankLink2(crank_link2_mass + , Vector3d(0.5 * crank_link2_length, 0., 0.) + , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius/2. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12.)); + + Joint joint_rev_z(JointTypeRevoluteZ); + Joint joint_sphere(JointTypeEulerZYX); + Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.)); + + id_p = model.AddBody(0 + , SpatialTransform() + , joint_prs_x, slider); + unsigned int id_b1 = model.AddBody(0 + , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) + , joint_rev_z, crankLink1); + id_s = model.AddBody(id_b1 + , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.)) + , joint_sphere, crankLink2); + + X_p = Xtrans(Vector3d(0., 0., slider_height)); + X_s = SpatialTransform(roty(-0.5 * M_PI), + Vector3d(crank_link2_length, 0, 0)); + + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,1,0,0), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,0,1,0), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,0,0,1), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,1,0,0,0), true, 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model,q,id_p).transpose()*X_p.E).transpose() + * CalcBodyWorldOrientation(model,q,id_s).transpose()*X_s.E; + + CHECK_CLOSE(rot_ps(0,0), -1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,1), 1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,2), -1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(0,1), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(0,2), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,0), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,2), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,0), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,1), 0.0, TEST_PREC); + + CHECK_ARRAY_CLOSE( + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + CalcBodyToBaseCoordinates(model, q, id_s, X_s.r), + 3, TEST_PREC); + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + unsigned int id_p; + unsigned int id_s; + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +struct SliderCrank3DSphericalJoint { + + SliderCrank3DSphericalJoint() + : model() + , cs() + , q() + , qd() + , id_p(0) + , id_s(0) + , X_p() + , X_s() { + + double slider_mass = 5.; + double slider_height = 0.1; + double crank_link1_mass = 3.; + double crank_link1_length = 1.; + double crank_link2_mass = 1.; + double crank_link2_radius = 0.2; + double crank_link2_length = 3.; + double crank_link1_height = crank_link2_length - crank_link1_length + + slider_height; + + Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.)); + Body crankLink1(crank_link1_mass + , Vector3d(0.5 * crank_link1_length, 0., 0.) + , Vector3d(0., 0. + , crank_link1_mass * crank_link1_length * crank_link1_length / 3.)); + Body crankLink2(crank_link2_mass + , Vector3d(0.5 * crank_link2_length, 0., 0.) + , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius/2. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12. + , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius + + crank_link2_length * crank_link2_length) / 12.)); + + Joint joint_rev_z(JointTypeRevoluteZ); + Joint joint_sphere(JointTypeSpherical); + Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.)); + + id_p = model.AddBody(0 + , SpatialTransform() + , joint_prs_x, slider); + unsigned int id_b1 = model.AddBody(0 + , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) + , joint_rev_z, crankLink1); + id_s = model.AddBody(id_b1 + , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.)) + , joint_sphere, crankLink2); + + X_p = Xtrans(Vector3d(0., 0., slider_height)); + X_s = SpatialTransform(roty(-0.5 * M_PI),Vector3d(crank_link2_length,0,0)); + + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,1,0,0), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,0,1,0), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,0,0,0,1), true, 0.1); + cs.AddLoopConstraint(id_p, id_s, X_p, X_s, + SpatialVector(0,0,1,0,0,0), true, 0.1); + + cs.Bind(model); + + q = VectorNd::Zero(model.q_size); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model,q,id_p).transpose()*X_p.E).transpose() + * CalcBodyWorldOrientation(model,q,id_s).transpose()*X_s.E; + CHECK_CLOSE(rot_ps(0,0), -1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,1), 1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,2), -1.0, TEST_PREC); + CHECK_CLOSE(rot_ps(0,1), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(0,2), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,0), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(1,2), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,0), 0.0, TEST_PREC); + CHECK_CLOSE(rot_ps(2,1), 0.0, TEST_PREC); + + CHECK_ARRAY_CLOSE( + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + CalcBodyToBaseCoordinates(model, q, id_s, X_s.r), + 3, TEST_PREC); + + } + + Model model; + ConstraintSet cs; + + VectorNd q; + VectorNd qd; + VectorNd qdd; + VectorNd tau; + + unsigned int id_p; + unsigned int id_s; + SpatialTransform X_p; + SpatialTransform X_s; + +}; + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { + VectorNd err = VectorNd::Zero(cs.size()); + Vector3d pos1; + Vector3d pos2; + Vector3d posErr; + Matrix3d rot_p; + double angleErr; + + // Test in zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(0., err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(0., err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = M_PI * 3 / 4; + q[1] = -M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = 0.; + angleErr = sin(-0.5 * M_PI); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + assert(std::fabs(posErr[1]) < TEST_PREC); + assert(std::fabs(posErr[2]) < TEST_PREC); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = M_PI + 0.1; + q[3] = 0.; + q[4] = 0.; + angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(posErr[1], err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); + + // Test in non-zero position. + q[0] = 0.8; + q[1] = -0.4; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = 0.; + angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]); + + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(posErr[0], err[0], TEST_PREC); + CHECK_CLOSE(posErr[1], err[1], TEST_PREC); + CHECK_CLOSE(angleErr, err[2], TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + + // Zero Q configuration, both arms of the 4-bar laying on the x-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -1.; + qd[3] = -1.; + qd[4] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Both arms of the 4-bar laying on the y-axis + q[0] = 0.5 * M_PI; + q[1] = 0.; + q[2] = 0.5 * M_PI; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -1.; + qd[3] = -1.; + qd[4] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Arms symmetric wrt y axis. + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + + // Arms symmetric wrt y axis. + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + + // Invalid velocities. + qd[0] = -1.; + qd[1] = -1.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { + VectorNd weights(q.size()); + VectorNd err(cs.size()); + VectorNd errRef(VectorNd::Zero(cs.size())); + + weights[0] = 1.; + weights[1] = 0.; + weights[2] = 1.; + weights[3] = 0.; + weights[4] = 0.; + + VectorNd qRef = VectorNd::Zero(q.size()); + qRef[0] = M_PI * 3 / 4; + qRef[1] = -0.5 * M_PI; + qRef[2] = M_PI - qRef[0]; + qRef[3] = -qRef[1]; + qRef[4] = qRef[0] + qRef[1] - qRef[2] - qRef[3]; + assert(qRef[0] + qRef[1] - qRef[2] - qRef[3] - qRef[4] == 0.); + + bool success; + + // Feasible initial guess. + VectorNd qInit = VectorNd::Zero(q.size()); + qInit = qRef; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0]; + qInit[1] = qRef[1]; + qInit[2] = qRef[2]; + qInit[3] = qRef[3]; + qInit[4] = qRef[4] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0] - 0.2; + qInit[1] = qRef[1] - 0.; + qInit[2] = qRef[2] + 0.1; + qInit[3] = qRef[3] - 0.03; + qInit[4] = qRef[4] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[0] = qRef[0] + 0.01; + qInit[1] = qRef[1] + 0.02; + qInit[2] = qRef[2] - 0.03; + qInit[3] = qRef[3] - 0.02; + qInit[4] = qRef[4] + 0.01; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); + CHECK_CLOSE(qInit[0], q[0], TEST_PREC); + CHECK_CLOSE(qInit[2], q[2], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) { + VectorNd weights(q.size()); + + weights[0] = 1.; + weights[1] = 0.; + weights[2] = 1.; + weights[3] = 0.; + weights[4] = 0.; + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + VectorNd qdInit = VectorNd::Zero(q.size()); + qdInit[0] = 0.01; + qdInit[1] = 0.5; + qdInit[2] = -0.7; + qdInit[3] = -0.5; + qdInit[4] = 0.3; + + CalcAssemblyQDot(model, q, qdInit, cs, qd, weights); + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + CalcConstraintsJacobian(model, q, cs, G); + err = G * qd; + + CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); + CHECK_CLOSE(qdInit[2], qd[2], TEST_PREC); +} + +TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { + VectorNd qddDirect; + VectorNd qddNullSpace; + + cs.SetSolver(LinearSolverColPivHouseholderQR); + +#ifndef RBDL_USE_SIMPLE_MATH + // SimpleMath has no solver that can solve the system in this configuration. + // Configuration 1. + + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 1.; + tau[1] = -2.; + tau[2] = 3.; + tau[3] = -5.; + tau[4] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +#endif + + // Configuration 2. + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = -1.; + qd[1] = -1.; + qd[2] = -2.; + qd[3] = +1.; + qd[4] = -1.; + assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 1.; + tau[1] = -2.; + tau[2] = 3.; + tau[3] = -5.; + tau[4] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); + + // Note: + // The Range Space Sparse method can't be used because the H matrix has a 0 + // on the diagonal and the LTL factorization tries to divide by 0. + + // Note: + // LinearSolverPartialPivLU does not work because the A matrix in the dynamic + // system is not invertible. + + // Note: + // LinearSolverHouseholderQR sometimes does not work well when the system is + // in a singular configuration. +} + +TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errd(cs.size()); + + q[0] = M_PI * 3 / 4; + q[1] = -0.5 * M_PI; + q[2] = M_PI - q[0]; + q[3] = -q[1]; + q[4] = q[0] + q[1] - q[2] - q[3]; + assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); +} + + + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) { + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + Vector3d rotationVec; + + // Test in zero position. + CalcConstraintsPositionError(model, q, cs, err); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Test in another configurations. + + q[0] = 0.4; + q[1] = 0.25 * M_PI; + q[2] = -0.25 * M_PI; + q[3] = 0.01; + q[4] = 0.01; + + CalcConstraintsPositionError(model, q, cs, err); + + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + rotationVec = - 0.5 * Vector3d + ( rot_ps(1,2) - rot_ps(2,1) + , rot_ps(2,0) - rot_ps(0,2) + , rot_ps(0,1) - rot_ps(1,0)); + errRef.block<3,1>(0,0) = pos_s - pos_p; + errRef[3] = rotationVec[2]; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + + // Test in zero position. + + G.setZero(); + CalcConstraintsJacobian(model, q, cs, G); + + VectorNd errRef(VectorNd::Zero(cs.size())); + VectorNd err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + VectorNd qWeights(q.size()); + VectorNd qInit(q.size()); + bool success = false; + + // Compute assembled configuration. + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + CHECK_EQUAL(success, true); + + // Some random velocity. + qd[0] = -0.2; + qd[1] = 0.1 * M_PI; + qd[2] = -0.1 * M_PI; + qd[3] = 0.; + qd[4] = 0.1 * M_PI; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQ) { + VectorNd weights(q.size()); + VectorNd qInit(q.size()); + + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + + bool success; + + weights[0] = 1.; + weights[1] = 1.; + weights[2] = 1.; + weights[3] = 1.; + weights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, TEST_PREC); + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + + CHECK(success); + CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); + CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) { + VectorNd qWeights(q.size()); + VectorNd qdWeights(q.size()); + VectorNd qInit(q.size()); + VectorNd qdInit(q.size()); + + SpatialVector vel_p; + SpatialVector vel_s; + + bool success = false; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + CHECK_EQUAL(success, true); + + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r); + vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + } + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { + VectorNd qWeights(q.size()); + VectorNd qdWeights(q.size()); + VectorNd qInit(q.size()); + VectorNd qdInit(q.size()); + + SpatialVector acc_p; + SpatialVector acc_s; + + bool success = false; + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Test with zero q and qdot. + + tau[0] = 0.12; + tau[1] = -0.3; + tau[2] = 0.05; + tau[3] = 0.7; + tau[4] = -0.1; + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +#endif + + // Compute non-zero assembly q and qdot; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + qdInit.setZero(); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + CHECK_EQUAL(success, true); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose()*X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + + CHECK_CLOSE(rot_ps(0,1), rot_ps(0,1), TEST_PREC); + + CHECK_ARRAY_CLOSE( + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + 3, TEST_PREC); + + CHECK_ARRAY_CLOSE( + CalcPointVelocity6D(model, q, qd, id_p, X_p.r), + CalcPointVelocity6D(model, q, qd, id_p, X_p.r), + 3, TEST_PREC); + + // Test with non-zero q and qdot. + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +} + +TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errdDirect(cs.size()); + VectorNd errdSpaceSparse(cs.size()); + VectorNd errdNullSpace(cs.size()); + + VectorNd qWeights(q.size()); + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + VectorNd qInit(q.size()); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + qInit[2] = -0.25 * M_PI; + qInit[3] = 0.1; + qInit[4] = 0.1; + + bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); + CHECK_EQUAL(success, true); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintErrors) { + + VectorNd err = VectorNd::Zero(cs.size()); + Vector3d pos0; + Vector3d pos1; + Vector3d pos2; + Vector3d posErr; + Matrix3d rot_p; + double angleErr; + + // Test in zero position. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_CLOSE(0., err[0], TEST_PREC); + CHECK_CLOSE(0., err[1], TEST_PREC); + CHECK_CLOSE(0., err[2], TEST_PREC); + CHECK_CLOSE(0., err[3], TEST_PREC); + CHECK_CLOSE(0., err[4], TEST_PREC); + CHECK_CLOSE(0., err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = 0.; + angleErr = sin(-0.5 * M_PI); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + assert(std::fabs(posErr[1]) < TEST_PREC); + assert(std::fabs(posErr[2]) < TEST_PREC); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(0., err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = 0.; + q[4] = 0.; + q[5] = M_PI + 0.1; + q[6] = 0.; + q[7] = 0.; + angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(posErr[1], err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); + + // Test in non-zero position. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = 0.8; + q[4] = -0.4; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = 0.; + angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]); + + pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero()); + pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r); + pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; + posErr = rot_p.transpose() * (pos2 - pos1); + + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); + CHECK_CLOSE(posErr[0], err[3], TEST_PREC); + CHECK_CLOSE(posErr[1], err[4], TEST_PREC); + CHECK_CLOSE(angleErr, err[5], TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintJacobian) { + + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + + // Zero Q configuration, both arms of the 4-bar laying on the x-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -1.; + qd[6] = -1.; + qd[7] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Both arms of the 4-bar laying on the y-axis + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.5 * M_PI; + q[4] = 0.; + q[5] = 0.5 * M_PI; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -1.; + qd[6] = -1.; + qd[7] = 0.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Arms symmetric wrt y axis. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + CalcConstraintsJacobian(model, q, cs, G); + + err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageConstraintsVelocityErrors) { + + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + + // Arms symmetric wrt y axis. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + + // Invalid velocities. + qd[0] = -1.; + qd[1] = -1.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = 0.; + qd[6] = 0.; + qd[7] = 0.; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { + VectorNd weights(q.size()); + VectorNd err(cs.size()); + VectorNd errRef(VectorNd::Zero(cs.size())); + + weights[0] = 0.; + weights[1] = 0.; + weights[2] = 0.; + weights[3] = 1.; + weights[4] = 0.; + weights[5] = 1.; + weights[6] = 0.; + weights[7] = 0.; + + VectorNd qRef = VectorNd::Zero(q.size()); + qRef[0] = 1.; + qRef[1] = 2.; + qRef[2] = 3.; + qRef[3] = M_PI * 3 / 4; + qRef[4] = -0.5 * M_PI; + qRef[5] = M_PI - qRef[3]; + qRef[6] = -qRef[4]; + qRef[7] = qRef[3] + qRef[4] - qRef[5] - qRef[6]; + + assert(qRef[3] + qRef[4] - qRef[5] - qRef[6] - qRef[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, qRef, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, qRef, idB5, X_s.r)).norm() < TEST_PREC); + + bool success; + + // Feasible initial guess. + VectorNd qInit = VectorNd::Zero(q.size()); + qInit = qRef; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3]; + qInit[4] = qRef[4]; + qInit[5] = qRef[5]; + qInit[6] = qRef[6]; + qInit[7] = qRef[7] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3] - 0.2; + qInit[4] = qRef[4] - 0.; + qInit[5] = qRef[5] + 0.1; + qInit[6] = qRef[6] - 0.03; + qInit[7] = qRef[7] + 0.05; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Perturbed initial guess. + qInit[3] = qRef[3] + 0.01; + qInit[4] = qRef[4] + 0.02; + qInit[5] = qRef[5] - 0.03; + qInit[6] = qRef[6] - 0.02; + qInit[7] = qRef[7] + 0.01; + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); + CalcConstraintsPositionError(model, q, cs, err); + + CHECK(success); + CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); + CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); + CHECK_CLOSE(qInit[3], q[3], TEST_PREC); + CHECK_CLOSE(qInit[5], q[5], TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) { + VectorNd weights(q.size()); + + weights[0] = 0.; + weights[1] = 0.; + weights[2] = 0.; + weights[3] = 1.; + weights[4] = 0.; + weights[5] = 1.; + weights[6] = 0.; + weights[7] = 0.; + + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + VectorNd qdInit = VectorNd::Zero(q.size()); + qdInit[0] = 1.; + qdInit[1] = 2.; + qdInit[2] = 3.; + qdInit[3] = 0.01; + qdInit[4] = 0.5; + qdInit[5] = -0.7; + qdInit[6] = -0.5; + qdInit[7] = 0.3; + + CalcAssemblyQDot(model, q, qdInit, cs, qd, weights); + MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + CalcConstraintsJacobian(model, q, cs, G); + err = G * qd; + + CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + CHECK_CLOSE(qdInit[3], qd[3], TEST_PREC); + CHECK_CLOSE(qdInit[5], qd[5], TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage + , TestFloatingFourBarLinkageForwardDynamics) { + + VectorNd qddDirect; + VectorNd qddNullSpace; + + cs.SetSolver(LinearSolverColPivHouseholderQR); + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Configuration 1. + q[0] = 0.; + q[1] = 0.; + q[2] = 0.; + q[3] = 0.; + q[4] = 0.; + q[5] = 0.; + q[6] = 0.; + q[7] = 0.; + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = 0.; + qd[4] = 0.; + qd[5] = 0.; + qd[6] = 0.; + qd[7] = 0.; + assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 0.; + tau[1] = 0.; + tau[2] = 0.; + tau[3] = 1.; + tau[4] = -2.; + tau[5] = 3.; + tau[6] = -5.; + tau[7] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +#endif + + // Configuration 2. + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + qd[0] = 0.; + qd[1] = 0.; + qd[2] = 0.; + qd[3] = -1.; + qd[4] = -1.; + qd[5] = -2.; + qd[6] = +1.; + qd[7] = -1.; + assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); + + tau[0] = 0.; + tau[1] = 0.; + tau[2] = 0.; + tau[3] = 1.; + tau[4] = -2.; + tau[5] = 3.; + tau[6] = -5.; + tau[7] = 7.; + + qddDirect = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) + , 6, TEST_PREC); + + qddNullSpace = VectorNd::Zero(q.size()); + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); + + CHECK_ARRAY_CLOSE + (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) + , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) + , 6, TEST_PREC); +} + +TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { + VectorNd qdPlusDirect(qd.size()); + VectorNd qdPlusRangeSpaceSparse(qd.size()); + VectorNd qdPlusNullSpace(qd.size()); + VectorNd errd(cs.size()); + + q[0] = 1.; + q[1] = 2.; + q[2] = 3.; + q[3] = M_PI * 3 / 4; + q[4] = -0.5 * M_PI; + q[5] = M_PI - q[3]; + q[6] = -q[4]; + q[7] = q[3] + q[4] - q[5] - q[6]; + + assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + cs.v_plus[4] = 5.; + cs.v_plus[5] = 6.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + cs.v_plus[4] = 0.; + cs.v_plus[5] = 0.; + + qd[0] = 1.; + qd[2] = 2.; + qd[4] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); + + CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintErrors) { + VectorNd err(VectorNd::Zero(cs.size())); + VectorNd errRef(VectorNd::Zero(cs.size())); + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + Vector3d rotationVec; + + // Test in zero position. + CalcConstraintsPositionError(model, q, cs, err); + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + + // Test in another configuration. + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25*M_PI,0.01,0.01)); + q[0] = 0.4; + q[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, q); + + CalcConstraintsPositionError(model, q, cs, err); + + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + rotationVec = - 0.5 * Vector3d + ( rot_ps(1,2) - rot_ps(2,1) + , rot_ps(2,0) - rot_ps(0,2) + , rot_ps(0,1) - rot_ps(1,0)); + errRef.block<3,1>(0,0) = pos_s - pos_p; + errRef[3] = rotationVec[2]; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintJacobian) { + MatrixNd G(MatrixNd::Zero(cs.size(), model.dof_count)); + + // Test in zero position. + + G.setZero(); + CalcConstraintsJacobian(model, q, cs, G); + + VectorNd errRef(VectorNd::Zero(cs.size())); + VectorNd err = G * qd; + + CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointConstraintsVelocityErrors) { + VectorNd errd(VectorNd::Zero(cs.size())); + VectorNd errdRef(VectorNd::Zero(cs.size())); + MatrixNd G(cs.size(), model.dof_count); + VectorNd qWeights(model.dof_count); + VectorNd qInit(model.q_size); + bool success = false; + + // Compute assembled configuration. + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1,0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + CHECK_EQUAL(success, true); + + // Some random velocity. + qd[0] = -0.2; + qd[1] = 0.1 * M_PI; + qd[2] = -0.1 * M_PI; + qd[3] = 0.; + qd[4] = 0.1 * M_PI; + + CalcConstraintsVelocityError(model, q, qd, cs, errd); + CalcConstraintsJacobian(model, q, cs, G); + errdRef = G * qd; + + CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointAssemblyQ) { + VectorNd weights(model.dof_count); + VectorNd qInit(model.q_size); + + Vector3d pos_p; + Vector3d pos_s; + Matrix3d rot_p; + Matrix3d rot_s; + Matrix3d rot_ps; + + bool success; + + weights[0] = 1.; + weights[1] = 1.; + weights[2] = 1.; + weights[3] = 1.; + weights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1,0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + success = CalcAssemblyQ(model, qInit, cs, q, weights, 1e-14, 800); + pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r); + pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r); + rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E; + rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + rot_ps = rot_p.transpose() * rot_s; + + CHECK(success); + CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); + CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointAssemblyQDot) { + VectorNd qWeights(model.dof_count); + VectorNd qdWeights(model.dof_count); + VectorNd qInit(model.q_size); + VectorNd qdInit(model.dof_count); + + SpatialVector vel_p; + SpatialVector vel_s; + + bool success = false; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1,0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + CHECK_EQUAL(success, true); + + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r); + vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + } + CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointForwardDynamics) { + + VectorNd qWeights(model.dof_count); + VectorNd qdWeights(model.dof_count); + VectorNd qInit(model.q_size); + VectorNd qdInit(model.dof_count); + + SpatialVector acc_p; + SpatialVector acc_s; + + bool success = false; + +#ifndef RBDL_USE_SIMPLE_MATH + // The SimpleMath solver cannot solve the system close to a singular + // configuration. + // Test with zero q and qdot. + + tau[0] = 0.12; + tau[1] = -0.3; + tau[2] = 0.05; + tau[3] = 0.7; + tau[4] = -0.1; + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +#endif + + // Compute non-zero assembly q and qdot; + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1,0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + qdWeights[0] = 1.; + qdWeights[1] = 0.; + qdWeights[2] = 0.; + qdWeights[3] = 0.; + qdWeights[4] = 0.; + + qdInit[0] = -0.2; + qdInit[1] = 0.1 * M_PI; + qdInit[2] = -0.1 * M_PI; + qdInit[3] = 0.; + qdInit[4] = 0.1 * M_PI; + + qdInit.setZero(); + + success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + CHECK_EQUAL(success, true); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); + + Matrix3d rot_ps + = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() + * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; + + CHECK_CLOSE(rot_ps(0,1), rot_ps(0,1), TEST_PREC); + CHECK_ARRAY_CLOSE( + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), + 3, TEST_PREC); + CHECK_ARRAY_CLOSE( + CalcPointVelocity6D(model, q, qd, id_p, X_p.r), + CalcPointVelocity6D(model, q, qd, id_p, X_p.r), + 3, TEST_PREC); + + // Test with non-zero q and qdot. + + ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } + + ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); + + acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r); + acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); + + for(size_t i = 2; i < 6; ++i) { + CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + } +} + +TEST_FIXTURE(SliderCrank3DSphericalJoint + , TestSliderCrank3DSphericalJointImpulse) { + + VectorNd qdPlusDirect(model.dof_count); + VectorNd qdPlusRangeSpaceSparse(model.dof_count); + VectorNd qdPlusNullSpace(model.dof_count); + VectorNd errdDirect(cs.size()); + VectorNd errdSpaceSparse(cs.size()); + VectorNd errdNullSpace(cs.size()); + VectorNd qWeights(model.dof_count); + VectorNd qInit(model.q_size); + + qWeights[0] = 1.; + qWeights[1] = 1.; + qWeights[2] = 1.; + qWeights[3] = 1.; + qWeights[4] = 1.; + + Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1,0.1)); + qInit[0] = 0.4; + qInit[1] = 0.25 * M_PI; + model.SetQuaternion(id_s, quat, qInit); + + bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); + CHECK_EQUAL(success, true); + + cs.v_plus[0] = 1.; + cs.v_plus[1] = 2.; + cs.v_plus[2] = 3.; + cs.v_plus[3] = 4.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + + cs.v_plus[0] = 0.; + cs.v_plus[1] = 0.; + cs.v_plus[2] = 0.; + cs.v_plus[3] = 0.; + + qd[0] = 1.; + qd[1] = 2.; + qd[2] = 3.; + + ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); + CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs + , qdPlusRangeSpaceSparse); + CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs + , errdSpaceSparse); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); + CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); + + CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); +} + +TEST(ConstraintCorrectnessTest) { + DoublePerpendicularPendulumAbsoluteCoordinates dba + = DoublePerpendicularPendulumAbsoluteCoordinates(); + DoublePerpendicularPendulumJointCoordinates dbj + = DoublePerpendicularPendulumJointCoordinates(); + + //1. Set the pendulum modeled using joint coordinates to a specific + // state and then compute the spatial acceleration of the body. + dbj.q[0] = M_PI/3.0; //About z0 + dbj.q[1] = M_PI/6.0; //About y1 + dbj.qd[0] = M_PI; //About z0 + dbj.qd[1] = M_PI/2.0; //About y1 + dbj.tau[0]= 0.; + dbj.tau[1]= 0.; + + ForwardDynamics(dbj.model,dbj.q,dbj.qd,dbj.tau,dbj.qdd); + + Vector3d r010 = CalcBodyToBaseCoordinates( + dbj.model,dbj.q,dbj.idB1, + Vector3d(0.,0.,0.),true); + Vector3d r020 = CalcBodyToBaseCoordinates( + dbj.model,dbj.q,dbj.idB2, + Vector3d(0.,0.,0.),true); + // Vector3d r030 = CalcBodyToBaseCoordinates( + // dbj.model,dbj.q,dbj.idB2, + // Vector3d(dbj.l2,0.,0.),true); + + SpatialVector v010 = CalcPointVelocity6D( + dbj.model,dbj.q,dbj.qd,dbj.idB1, + Vector3d(0.,0.,0.),true); + SpatialVector v020 = CalcPointVelocity6D( + dbj.model,dbj.q,dbj.qd,dbj.idB2, + Vector3d(0.,0.,0.),true); + // SpatialVector v030 = CalcPointVelocity6D( + // dbj.model,dbj.q,dbj.qd,dbj.idB2, + // Vector3d(dbj.l2,0.,0.),true); + + SpatialVector a010 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB1,Vector3d(0.,0.,0.),true); + SpatialVector a020 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB2,Vector3d(0.,0.,0.),true); + SpatialVector a030 = CalcPointAcceleration6D( + dbj.model,dbj.q,dbj.qd,dbj.qdd, + dbj.idB2,Vector3d(dbj.l2,0.,0.),true); + + //2. Set the pendulum modelled using absolute coordinates to the + // equivalent state as the pendulum modelled using joint + // coordinates. Next + + /* + dba.q[0] = dbj.q[0]; + dba.q[1] = r020[0]; + dba.q[2] = r020[1]; + dba.q[3] = r020[2]; + dba.q[4] = 0; + dba.q[5] = 0; + dba.q[6] = dbj.q[0]+dbj.q[1]; + + dba.qd[0] = dbj.qd[0]; + dba.qd[1] = v020[0]; + dba.qd[2] = v020[1]; + dba.qd[3] = v020[2]; + dba.qd[4] = 0; + dba.qd[5] = 0; + dba.qd[6] = dbj.qd[0]+dbj.qd[1]; + */ + + + dba.q[0] = r010[0]; + dba.q[1] = r010[1]; + dba.q[2] = r010[2]; + dba.q[3] = dbj.q[0]; + dba.q[4] = 0; + dba.q[5] = 0; + dba.q[6] = r020[0]; + dba.q[7] = r020[1]; + dba.q[8] = r020[2]; + dba.q[9] = dbj.q[0]; + dba.q[10] = dbj.q[1]; + dba.q[11] = 0; + + dba.qd[0] = v010[3]; + dba.qd[1] = v010[4]; + dba.qd[2] = v010[5]; + dba.qd[3] = dbj.qd[0]; + dba.qd[4] = 0; + dba.qd[5] = 0; + dba.qd[6] = v020[3]; + dba.qd[7] = v020[4]; + dba.qd[8] = v020[5]; + dba.qd[9] = dbj.qd[0]; + dba.qd[10] = dbj.qd[1]; + dba.qd[11] = 0; + + VectorNd err(dba.cs.size()); + VectorNd errd(dba.cs.size()); + + CalcConstraintsPositionError(dba.model,dba.q,dba.cs,err,true); + CalcConstraintsVelocityError(dba.model,dba.q,dba.qd,dba.cs,errd,true); + + //The constraint errors at the position and velocity level + //must be zero before the accelerations can be tested. + for(unsigned int i=0; i + +#include "rbdl/Logging.h" +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" +#include + +const double TEST_PREC = 1.0e-14; + +using namespace std; +using namespace RigidBodyDynamics::Math; + +struct MathFixture { +}; + +TEST (GaussElimPivot) { + ClearLogOutput(); + + MatrixNd A; + A.resize(3,3); + VectorNd b(3); + VectorNd x(3); + + A(0,0) = 0; A(0,1) = 2; A(0,2) = 1; + A(1,0) = 1; A(1,1) = 1; A(1,2) = 5; + A(2,0) = 0; A(2,1) = 0; A(2,2) = 1; + + b[0] = 1; + b[1] = 2; + b[2] = 3; + + VectorNd test_result (3); + + test_result[0] = -12; + test_result[1] = -1; + test_result[2] = 3; + + LinSolveGaussElimPivot (A, b, x); + + CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC); + + A(0,0) = 0; A(0,1) = -2; A(0,2) = 1; + A(1,0) = 1; A(1,1) = 1; A(1,2) = 5; + A(2,0) = 0; A(2,1) = 0; A(2,2) = 1; + + LinSolveGaussElimPivot (A, b, x); + test_result[0] = -14; + test_result[1] = 1; + test_result[2] = 3; + + CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC); +} + +TEST (Dynamic_1D_initialize_value) { + VectorNd myvector_10 = VectorNd::Constant ((size_t) 10, 12.); + + double *test_values = new double[10]; + for (unsigned int i = 0; i < 10; i++) + test_values[i] = 12.; + + CHECK_ARRAY_EQUAL (test_values, myvector_10.data(), 10); + delete[] test_values; +} + +TEST (Dynamic_2D_initialize_value) { + MatrixNd mymatrix_10x10 = MatrixNd::Constant (10, 10, 12.); + + double *test_values = new double[10 * 10]; + for (unsigned int i = 0; i < 10; i++) + for (unsigned int j = 0; j < 10; j++) + test_values[i*10 + j] = 12.; + + CHECK_ARRAY_EQUAL (test_values, mymatrix_10x10.data(), 10*10); + delete[] test_values; +} + +TEST (SpatialMatrix_Multiplication) { + SpatialMatrix X_1 ( + 1., 2., 3., 4., 5., 6., + 11., 12., 13., 14., 15., 16., + 21., 22., 23., 24., 25., 26., + 31., 32., 33., 34., 35., 36., + 41., 42., 43., 44., 45., 46., + 51., 52., 53., 54., 55., 56. + ); + + SpatialMatrix X_2 (X_1); + + X_2 *= 2.; + + SpatialMatrix correct_result ( + 1442, 1484, 1526, 1568, 1610, 1652, + 4562, 4724, 4886, 5048, 5210, 5372, + 7682, 7964, 8246, 8528, 8810, 9092, + 10802, 11204, 11606, 12008, 12410, 12812, + 13922, 14444, 14966, 15488, 16010, 16532, + 17042, 17684, 18326, 18968, 19610, 20252 + ); + + SpatialMatrix test_result = X_1 * X_2; + + CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC); + + // check the *= operator: + test_result = X_1; + test_result *= X_2; + + CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC); +} diff --git a/3rdparty/rbdl/tests/ModelTests.cc b/3rdparty/rbdl/tests/ModelTests.cc new file mode 100644 index 0000000..6439084 --- /dev/null +++ b/3rdparty/rbdl/tests/ModelTests.cc @@ -0,0 +1,632 @@ +#include + +#include + +#include "Fixtures.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +struct ModelFixture { + ModelFixture () { + ClearLogOutput(); + model = new Model; + model->gravity = Vector3d (0., -9.81, 0.); + } + ~ModelFixture () { + delete model; + } + Model *model; +}; + +TEST_FIXTURE(ModelFixture, TestInit) { + CHECK_EQUAL (1u, model->lambda.size()); + CHECK_EQUAL (1u, model->mu.size()); + CHECK_EQUAL (0u, model->dof_count); + + CHECK_EQUAL (0u, model->q_size); + CHECK_EQUAL (0u, model->qdot_size); + + CHECK_EQUAL (1u, model->v.size()); + CHECK_EQUAL (1u, model->a.size()); + + CHECK_EQUAL (1u, model->mJoints.size()); + CHECK_EQUAL (1u, model->S.size()); + + CHECK_EQUAL (1u, model->c.size()); + CHECK_EQUAL (1u, model->IA.size()); + CHECK_EQUAL (1u, model->pA.size()); + CHECK_EQUAL (1u, model->U.size()); + CHECK_EQUAL (1u, model->d.size()); + CHECK_EQUAL (1u, model->u.size()); + CHECK_EQUAL (1u, model->Ic.size()); + CHECK_EQUAL (1u, model->I.size()); + + CHECK_EQUAL (1u, model->X_lambda.size()); + CHECK_EQUAL (1u, model->X_base.size()); + CHECK_EQUAL (1u, model->mBodies.size()); +} + +TEST_FIXTURE(ModelFixture, TestAddBodyDimensions) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int body_id = 0; + body_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + CHECK_EQUAL (1u, body_id); + CHECK_EQUAL (2u, model->lambda.size()); + CHECK_EQUAL (2u, model->mu.size()); + CHECK_EQUAL (1u, model->dof_count); + + CHECK_EQUAL (2u, model->v.size()); + CHECK_EQUAL (2u, model->a.size()); + + CHECK_EQUAL (2u, model->mJoints.size()); + CHECK_EQUAL (2u, model->S.size()); + + CHECK_EQUAL (2u, model->c.size()); + CHECK_EQUAL (2u, model->IA.size()); + CHECK_EQUAL (2u, model->pA.size()); + CHECK_EQUAL (2u, model->U.size()); + CHECK_EQUAL (2u, model->d.size()); + CHECK_EQUAL (2u, model->u.size()); + CHECK_EQUAL (2u, model->Ic.size()); + CHECK_EQUAL (2u, model->I.size()); + + SpatialVector spatial_zero; + spatial_zero.setZero(); + + CHECK_EQUAL (2u, model->X_lambda.size()); + CHECK_EQUAL (2u, model->X_base.size()); + CHECK_EQUAL (2u, model->mBodies.size()); +} + +TEST_FIXTURE(ModelFixture, TestFloatingBodyDimensions) { + Body body; + Joint float_base_joint (JointTypeFloatingBase); + + model->AppendBody (SpatialTransform(), float_base_joint, body); + + CHECK_EQUAL (3u, model->lambda.size()); + CHECK_EQUAL (3u, model->mu.size()); + CHECK_EQUAL (6u, model->dof_count); + CHECK_EQUAL (7u, model->q_size); + CHECK_EQUAL (6u, model->qdot_size); + + CHECK_EQUAL (3u, model->v.size()); + CHECK_EQUAL (3u, model->a.size()); + + CHECK_EQUAL (3u, model->mJoints.size()); + CHECK_EQUAL (3u, model->S.size()); + + CHECK_EQUAL (3u, model->c.size()); + CHECK_EQUAL (3u, model->IA.size()); + CHECK_EQUAL (3u, model->pA.size()); + CHECK_EQUAL (3u, model->U.size()); + CHECK_EQUAL (3u, model->d.size()); + CHECK_EQUAL (3u, model->u.size()); + + SpatialVector spatial_zero; + spatial_zero.setZero(); + + CHECK_EQUAL (3u, model->X_lambda.size()); + CHECK_EQUAL (3u, model->X_base.size()); + CHECK_EQUAL (3u, model->mBodies.size()); +} + +/** \brief Tests whether the joint and body information stored in the Model are computed correctly +*/ +TEST_FIXTURE(ModelFixture, TestAddBodySpatialValues) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body); + + SpatialVector spatial_joint_axis(0., 0., 1., 0., 0., 0.); + CHECK_EQUAL (spatial_joint_axis, joint.mJointAxes[0]); + + // \Todo: Dynamic properties +} + +TEST_FIXTURE(ModelFixture, TestAddBodyTestBodyName) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body, "mybody"); + + unsigned int body_id = model->GetBodyId("mybody"); + + CHECK_EQUAL (1u, body_id); + CHECK_EQUAL (std::numeric_limits::max(), model->GetBodyId("unknownbody")); +} + +TEST_FIXTURE(ModelFixture, TestjcalcSimple) { + Body body; + Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, body); + + VectorNd Q = VectorNd::Zero (model->q_size); + VectorNd QDot = VectorNd::Zero (model->q_size); + + QDot[0] = 1.; + jcalc (*model, 1, Q, QDot); + + SpatialMatrix test_matrix ( + 1., 0., 0., 0., 0., 0., + 0., 1., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., 0., 0., 1. + ); + SpatialVector test_vector ( + 0., 0., 1., 0., 0., 0. + ); + SpatialVector test_joint_axis ( + 0., 0., 1., 0., 0., 0. + ); + + CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16)); + CHECK_EQUAL (test_joint_axis, model->S[1]); + + Q[0] = M_PI * 0.5; + QDot[0] = 1.; + + jcalc (*model, 1, Q, QDot); + + test_matrix << + 0., 1., 0., 0., 0., 0., + -1., 0., 0., 0., 0., 0., + 0., 0., 1., 0., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., -1., 0., 0., + 0., 0., 0., 0., 0., 1.; + + CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16)); + CHECK_EQUAL (test_joint_axis, model->S[1]); +} + +TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) { + Body body; + + unsigned int body_id = model->AddBody (0, SpatialTransform(), + Joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ), + body); + + VectorNd q = VectorNd::Zero (model->dof_count); + VectorNd qdot = VectorNd::Zero (model->dof_count); + VectorNd qddot = VectorNd::Zero (model->dof_count); + VectorNd tau = VectorNd::Zero (model->dof_count); + + Vector3d base_coords (0., 0., 0.); + Vector3d body_coords; + Vector3d base_coords_back; + + UpdateKinematics (*model, q, qdot, qddot); + body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false); + base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false); + + CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC); + + q[0] = 1.; + q[1] = 0.2; + q[2] = -2.3; + q[3] = -2.3; + q[4] = 0.03; + q[5] = -0.23; + + UpdateKinematics (*model, q, qdot, qddot); + body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false); + base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false); + + CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC); +} + +TEST ( Model2DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( Model3DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + unsigned int body_id; + + // in total we add two bodies to make sure that the transformations are + // correct. + model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + // in total we add two bodies to make sure that the transformations are + // correct. + body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( Model6DoFJoint ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.)); + Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.)); + + Model model_std; + model_std.gravity = Vector3d (0., -9.81, 0.); + + unsigned int body_id; + + Joint joint_floating_base ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.), + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + body_id = model_std.AddBody (0, SpatialTransform(), joint_floating_base, body); + + model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); + model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); + body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); + + // using a model with a 2 DoF joint + Joint joint_rot_zyx ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + + Model model_2; + model_2.gravity = Vector3d (0., -9.81, 0.); + + // in total we add two bodies to make sure that the transformations are + // correct. + body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_floating_base, body); + body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body); + + VectorNd Q = VectorNd::Zero(model_std.dof_count); + VectorNd QDot = VectorNd::Zero(model_std.dof_count); + VectorNd Tau = VectorNd::Zero(model_std.dof_count); + + VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count); + VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count); + + assert (model_std.q_size == model_2.q_size); + + ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std); + ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2); + + CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC); +} + +TEST ( ModelFixedJointQueryBodyId ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + + CHECK_EQUAL (fixed_body_id, model.GetBodyId("fixed_body")); +} + +/* + * Makes sure that when appending a body to a fixed joint the parent of the + * newly added parent is actually the moving body that the fixed body is + * attached to. + */ +TEST ( ModelAppendToFixedBody ) { + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + // unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (movable_body + 1, appended_body_id); + CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); +} + +// Adds a fixed body to another fixed body. +TEST ( ModelAppendFixedToFixedBody ) { + Body null_body; + + double movable_mass = 1.1; + Vector3d movable_com (1., 0.4, 0.4); + + double fixed_mass = 1.2; + Vector3d fixed_com (1.1, 0.5, 0.5); + + Vector3d fixed_displacement (0., 1., 0.); + + Body body(movable_mass, movable_com, Vector3d (1., 1., 1.)); + Body fixed_body(fixed_mass, fixed_com, Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int fixed_body_2_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body_2"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (movable_body + 1, appended_body_id); + CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); + CHECK_EQUAL (movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass); + + CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent); + CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent); + + double new_mass = 3.5; + Vector3d new_com = (1. / new_mass) * (movable_mass * movable_com + fixed_mass * (fixed_com + fixed_displacement) + fixed_mass * (fixed_com + fixed_displacement * 2.)); + + CHECK_ARRAY_CLOSE (new_com.data(), model.mBodies[movable_body].mCenterOfMass.data(), 3, TEST_PREC); +} + +// Ensures that the transformations of the movable parent and fixed joint +// frame is in proper order +TEST ( ModelFixedJointRotationOrderTranslationRotation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.)); + SpatialTransform rot_z = Xrotz (45. * M_PI / 180.); + + model.AddBody (0, trans_x, joint_rot_z, body); + model.AppendBody (rot_z, Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + Q[0] = 45 * M_PI / 180.; + Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point.data(), 3, TEST_PREC); +} + +// Ensures that the transformations of the movable parent and fixed joint +// frame is in proper order +TEST ( ModelFixedJointRotationOrderRotationTranslation ) { + // the standard modeling using a null body + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + SpatialTransform rot_z = Xrotz (45. * M_PI / 180.); + SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.)); + + model.AddBody (0, rot_z, joint_rot_z, body); + model.AppendBody (trans_x, Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body); + + VectorNd Q (VectorNd::Zero(model.dof_count)); + Q[0] = 45 * M_PI / 180.; + Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (Vector3d (-1., 2., 0.).data(), point.data(), 3, TEST_PREC); +} + +TEST ( ModelGetBodyName ) { + Body null_body; + Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.)); + + Model model; + + Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.)); + + model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body); + unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body"); + unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body"); + + CHECK_EQUAL (string("fixed_body"), model.GetBodyName(fixed_body_id)); + CHECK_EQUAL (string("appended_body"), model.GetBodyName(appended_body_id)); + CHECK_EQUAL (string(""), model.GetBodyName(123)); +} + +TEST_FIXTURE ( RotZRotZYXFixed, ModelGetParentBodyId ) { + CHECK_EQUAL (0u, model->GetParentBodyId(0)); + CHECK_EQUAL (0u, model->GetParentBodyId(body_a_id)); + CHECK_EQUAL (body_a_id, model->GetParentBodyId(body_b_id)); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetParentIdFixed) { + CHECK_EQUAL (body_b_id, model->GetParentBodyId(body_fixed_id)); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrame) { + SpatialTransform transform_a = model->GetJointFrame (body_a_id); + SpatialTransform transform_b = model->GetJointFrame (body_b_id); + SpatialTransform transform_root = model->GetJointFrame (0); + + CHECK_ARRAY_EQUAL (fixture_transform_a.r.data(), transform_a.r.data(), 3); + CHECK_ARRAY_EQUAL (fixture_transform_b.r.data(), transform_b.r.data(), 3); + CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrameFixed) { + SpatialTransform transform_fixed = model->GetJointFrame (body_fixed_id); + + CHECK_ARRAY_EQUAL (fixture_transform_fixed.r.data(), transform_fixed.r.data(), 3); +} + +TEST_FIXTURE(RotZRotZYXFixed, ModelSetJointFrame) { + SpatialTransform new_transform_a = Xtrans (Vector3d(-1., -2., -3.)); + SpatialTransform new_transform_b = Xtrans (Vector3d(-4., -5., -6.)); + SpatialTransform new_transform_root = Xtrans (Vector3d(-99, -99., -99.)); + + model->SetJointFrame (body_a_id, new_transform_a); + model->SetJointFrame (body_b_id, new_transform_b); + model->SetJointFrame (0, new_transform_root); + + SpatialTransform transform_a = model->GetJointFrame (body_a_id); + SpatialTransform transform_b = model->GetJointFrame (body_b_id); + SpatialTransform transform_root = model->GetJointFrame (0); + + CHECK_ARRAY_EQUAL (new_transform_a.r.data(), transform_a.r.data(), 3); + CHECK_ARRAY_EQUAL (new_transform_b.r.data(), transform_b.r.data(), 3); + CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); +} + +TEST (CalcBodyWorldOrientationFixedJoint) { + Model model_fixed; + Model model_movable; + + Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.)); + Joint joint_fixed (JointTypeFixed); + Joint joint_rot_x = (SpatialVector (1., 0., 0., 0., 0., 0.)); + + model_fixed.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body); + unsigned int body_id_fixed = model_fixed.AppendBody (Xroty (45 * M_PI / 180), joint_fixed, body); + + model_movable.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body); + unsigned int body_id_movable = model_movable.AppendBody (Xroty (45 * M_PI / 180), joint_rot_x, body); + + VectorNd q_fixed (VectorNd::Zero (model_fixed.q_size)); + VectorNd q_movable (VectorNd::Zero (model_movable.q_size)); + + Matrix3d E_fixed = CalcBodyWorldOrientation (model_fixed, q_fixed, body_id_fixed); + Matrix3d E_movable = CalcBodyWorldOrientation (model_movable, q_movable, body_id_movable); + + CHECK_ARRAY_CLOSE (E_movable.data(), E_fixed.data(), 9, TEST_PREC); +} + +TEST (TestAddFixedBodyToRoot) { + Model model; + + Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.)); + Joint joint_fixed (JointTypeFixed); + + // Add a fixed body + unsigned int body_id_fixed = model.AppendBody (Xtrans(Vector3d (1., 0., 0.)), joint_fixed, body, "FixedBody"); + + // Add a second fixed body + unsigned int body_id_fixed2 = model.AppendBody (Xtrans(Vector3d (1., 0., 0.)), joint_fixed, body, "FixedBody2"); + + // Add a mobile boby + unsigned int movable_body = model.AppendBody (Xrotx (45 * M_PI / 180), JointTypeRevoluteX, body, "MovableBody"); + + CHECK_EQUAL ((unsigned int) 2, model.mBodies.size()); + CHECK_EQUAL ((unsigned int) 2, model.mFixedBodies.size()); + + VectorNd q = VectorNd::Zero(model.q_size); + + Vector3d base_coords = CalcBodyToBaseCoordinates(model, q, body_id_fixed, Vector3d::Zero()); + CHECK_ARRAY_EQUAL(Vector3d (1., 0., 0.).data(), base_coords.data(), 3); + + base_coords = CalcBodyToBaseCoordinates(model, q, body_id_fixed2, Vector3d::Zero()); + CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3); + + base_coords = CalcBodyToBaseCoordinates(model, q, movable_body, Vector3d::Zero()); + CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3); + +} + diff --git a/3rdparty/rbdl/tests/MultiDofTests.cc b/3rdparty/rbdl/tests/MultiDofTests.cc new file mode 100644 index 0000000..5937564 --- /dev/null +++ b/3rdparty/rbdl/tests/MultiDofTests.cc @@ -0,0 +1,1118 @@ +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +struct SphericalJoint { + SphericalJoint () { + ClearLogOutput(); + + emulated_model.gravity = Vector3d (0., 0., -9.81); + multdof3_model.gravity = Vector3d (0., 0., -9.81); + eulerzyx_model.gravity = Vector3d (0., 0., -9.81); + + body = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.)); + + joint_rot_zyx = Joint ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.) + ); + joint_spherical = Joint (JointTypeSpherical); + joint_eulerzyx = Joint (JointTypeEulerZYX); + + joint_rot_y = Joint (SpatialVector (0., 1., 0., 0., 0., 0.)); + + emulated_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + emu_body_id = emulated_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_zyx, body); + emu_child_id = emulated_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + multdof3_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + sph_body_id = multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + sph_child_id = multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + eulerzyx_model.AppendBody (Xtrans(Vector3d (0., 0., 0.)), joint_rot_y, body); + eulerzyx_body_id = eulerzyx_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_eulerzyx, body); + eulerzyx_child_id = eulerzyx_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_rot_y, body); + + emuQ = VectorNd::Zero ((size_t) emulated_model.q_size); + emuQDot = VectorNd::Zero ((size_t) emulated_model.qdot_size); + emuQDDot = VectorNd::Zero ((size_t) emulated_model.qdot_size); + emuTau = VectorNd::Zero ((size_t) emulated_model.qdot_size); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + eulerzyxQ = VectorNd::Zero ((size_t) eulerzyx_model.q_size); + eulerzyxQDot = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + eulerzyxQDDot = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + eulerzyxTau = VectorNd::Zero ((size_t) eulerzyx_model.qdot_size); + } + + Joint joint_rot_zyx; + Joint joint_spherical; + Joint joint_eulerzyx; + Joint joint_rot_y; + Body body; + + unsigned int emu_body_id, emu_child_id, + sph_body_id, sph_child_id, + eulerzyx_body_id, eulerzyx_child_id; + + Model emulated_model; + Model multdof3_model; + Model eulerzyx_model; + + VectorNd emuQ; + VectorNd emuQDot; + VectorNd emuQDDot; + VectorNd emuTau; + + VectorNd sphQ; + VectorNd sphQDot; + VectorNd sphQDDot; + VectorNd sphTau; + + VectorNd eulerzyxQ; + VectorNd eulerzyxQDot; + VectorNd eulerzyxQDDot; + VectorNd eulerzyxTau; +}; + +void ConvertQAndQDotFromEmulated ( + const Model &UNUSED(emulated_model), const VectorNd &q_emulated, const VectorNd &qdot_emulated, + const Model &multdof3_model, VectorNd *q_spherical, VectorNd *qdot_spherical) { + for (unsigned int i = 1; i < multdof3_model.mJoints.size(); i++) { + unsigned int q_index = multdof3_model.mJoints[i].q_index; + + if (multdof3_model.mJoints[i].mJointType == JointTypeSpherical) { + Quaternion quat = Quaternion::fromZYXAngles ( Vector3d ( + q_emulated[q_index + 0], q_emulated[q_index + 1], q_emulated[q_index + 2])); + multdof3_model.SetQuaternion (i, quat, (*q_spherical)); + + Vector3d omega = angular_velocity_from_angle_rates ( + Vector3d (q_emulated[q_index], q_emulated[q_index + 1], q_emulated[q_index + 2]), + Vector3d (qdot_emulated[q_index], qdot_emulated[q_index + 1], qdot_emulated[q_index + 2]) + ); + + (*qdot_spherical)[q_index] = omega[0]; + (*qdot_spherical)[q_index + 1] = omega[1]; + (*qdot_spherical)[q_index + 2] = omega[2]; + } else { + (*q_spherical)[q_index] = q_emulated[q_index]; + (*qdot_spherical)[q_index] = qdot_emulated[q_index]; + } + } +} + +TEST(TestQuaternionIntegration ) { + double timestep = 0.001; + + Vector3d zyx_angles_t0 (0.1, 0.2, 0.3); + Vector3d zyx_rates (3., 5., 2.); + Vector3d zyx_angles_t1 = zyx_angles_t0 + timestep * zyx_rates; + Quaternion q_zyx_t1 = Quaternion::fromZYXAngles (zyx_angles_t1); + + Quaternion q_t0 = Quaternion::fromZYXAngles (zyx_angles_t0); + Vector3d w_base = global_angular_velocity_from_rates (zyx_angles_t0, zyx_rates); + Quaternion q_t1 = q_t0.timeStep (w_base, timestep); + + // Note: we test with a rather crude precision. My guess for the error is + // that we compare two different things: + // A) integration under the assumption that the euler rates are + // constant + // B) integration under the assumption that the angular velocity is + // constant + // However I am not entirely sure about this... + CHECK_ARRAY_CLOSE (q_zyx_t1.data(), q_t1.data(), 4, 1.0e-5); +} + +TEST_FIXTURE(SphericalJoint, TestQIndices) { + CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); + CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); + CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); + + CHECK_EQUAL (5u, emulated_model.q_size); + CHECK_EQUAL (5u, emulated_model.qdot_size); + + CHECK_EQUAL (6u, multdof3_model.q_size); + CHECK_EQUAL (5u, multdof3_model.qdot_size); + CHECK_EQUAL (5u, multdof3_model.multdof3_w_index[2]); +} + +TEST_FIXTURE(SphericalJoint, TestGetQuaternion) { + multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + CHECK_EQUAL (10u, multdof3_model.q_size); + CHECK_EQUAL (8u, multdof3_model.qdot_size); + + CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); + CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); + CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); + CHECK_EQUAL (5u, multdof3_model.mJoints[4].q_index); + + CHECK_EQUAL (8u, multdof3_model.multdof3_w_index[2]); + CHECK_EQUAL (9u, multdof3_model.multdof3_w_index[4]); + + sphQ[0] = 100.; + sphQ[1] = 0.; + sphQ[2] = 1.; + sphQ[3] = 2.; + sphQ[4] = 100.; + sphQ[5] = -6.; + sphQ[6] = -7.; + sphQ[7] = -8; + sphQ[8] = 4.; + sphQ[9] = -9.; + + Quaternion reference_1 (0., 1., 2., 4.); + Quaternion quat_1 = multdof3_model.GetQuaternion (2, sphQ); + CHECK_ARRAY_EQUAL (reference_1.data(), quat_1.data(), 4); + + Quaternion reference_3 (-6., -7., -8., -9.); + Quaternion quat_3 = multdof3_model.GetQuaternion (4, sphQ); + CHECK_ARRAY_EQUAL (reference_3.data(), quat_3.data(), 4); +} + +TEST_FIXTURE(SphericalJoint, TestSetQuaternion) { + multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body); + + sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size); + sphQDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size); + + Quaternion reference_1 (0., 1., 2., 3.); + multdof3_model.SetQuaternion (2, reference_1, sphQ); + Quaternion test = multdof3_model.GetQuaternion (2, sphQ); + CHECK_ARRAY_EQUAL (reference_1.data(), test.data(), 4); + + Quaternion reference_2 (11., 22., 33., 44.); + multdof3_model.SetQuaternion (4, reference_2, sphQ); + test = multdof3_model.GetQuaternion (4, sphQ); + CHECK_ARRAY_EQUAL (reference_2.data(), test.data(), 4); +} + +TEST_FIXTURE(SphericalJoint, TestOrientation) { + emuQ[0] = 1.1; + emuQ[1] = 1.1; + emuQ[2] = 1.1; + emuQ[3] = 1.1; + + for (unsigned int i = 0; i < emuQ.size(); i++) { + sphQ[i] = emuQ[i]; + } + + Quaternion quat = Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), emuQ[0]) + * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), emuQ[1]) + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), emuQ[2]); + multdof3_model.SetQuaternion (2, quat, sphQ); + + Matrix3d emu_orientation = CalcBodyWorldOrientation (emulated_model, emuQ, emu_child_id); + Matrix3d sph_orientation = CalcBodyWorldOrientation (multdof3_model, sphQ, sph_child_id); + + CHECK_ARRAY_CLOSE (emu_orientation.data(), sph_orientation.data(), 9, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestUpdateKinematics) { + emuQ[0] = 1.; + emuQ[1] = 1.; + emuQ[2] = 1.; + emuQ[3] = 1.; + emuQ[4] = 1.; + + emuQDot[0] = 1.; + emuQDot[1] = 1.; + emuQDot[2] = 1.; + emuQDot[3] = 1.; + emuQDot[4] = 1.; + + emuQDDot[0] = 1.; + emuQDDot[1] = 1.; + emuQDDot[2] = 1.; + emuQDDot[3] = 1.; + emuQDDot[4] = 1.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDDot, multdof3_model, &sphQ, &sphQDDot); + + Vector3d a = angular_acceleration_from_angle_rates ( + Vector3d (emuQ[3], emuQ[2], emuQ[1]), + Vector3d (emuQDot[3], emuQDot[2], emuQDot[1]), + Vector3d (emuQDDot[3], emuQDDot[2], emuQDDot[1]) + ); + + sphQDDot[0] = emuQDDot[0]; + sphQDDot[1] = a[0]; + sphQDDot[2] = a[1]; + sphQDDot[3] = a[2]; + sphQDDot[4] = emuQDDot[4]; + + UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, &emuQDDot); + UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, &sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_body_id].data(), multdof3_model.v[sph_body_id].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (emulated_model.a[emu_body_id].data(), multdof3_model.a[sph_body_id].data(), 6, TEST_PREC); + + UpdateKinematics (multdof3_model, sphQ, sphQDot, sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestSpatialVelocities) { + emuQ[0] = 1.; + emuQ[1] = 2.; + emuQ[2] = 3.; + emuQ[3] = 4.; + + emuQDot[0] = 4.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 6.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, NULL); + UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, NULL); + + CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestForwardDynamicsQAndQDot) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + + emuQDot[0] = 2.2; + emuQDot[1] = 2.3; + emuQDot[2] = 2.4; + emuQDot[3] = 2.5; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, emuQDDot); + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, sphQDDot); + + CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestDynamicsConsistencyRNEA_ABA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, sphQDDot); + + VectorNd tau_id (VectorNd::Zero (multdof3_model.qdot_size)); + InverseDynamics (multdof3_model, sphQ, sphQDot, sphQDDot, tau_id); + + CHECK_ARRAY_CLOSE (sphTau.data(), tau_id.data(), tau_id.size(), TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestCRBA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + MatrixNd H_crba (MatrixNd::Zero (multdof3_model.qdot_size, multdof3_model.qdot_size)); + + UpdateKinematicsCustom (multdof3_model, &sphQ, NULL, NULL); + CompositeRigidBodyAlgorithm (multdof3_model, sphQ, H_crba, false); + + MatrixNd H_id (MatrixNd::Zero (multdof3_model.qdot_size, multdof3_model.qdot_size)); + VectorNd H_col = VectorNd::Zero (multdof3_model.qdot_size); + VectorNd QDDot_zero = VectorNd::Zero (multdof3_model.qdot_size); + + for (unsigned int i = 0; i < multdof3_model.qdot_size; i++) { + // compute each column + VectorNd delta_a = VectorNd::Zero (multdof3_model.qdot_size); + delta_a[i] = 1.; + + // compute ID (model, q, qdot, delta_a) + VectorNd id_delta = VectorNd::Zero (multdof3_model.qdot_size); + InverseDynamics (multdof3_model, sphQ, sphQDot, delta_a, id_delta); + + // compute ID (model, q, qdot, zero) + VectorNd id_zero = VectorNd::Zero (multdof3_model.qdot_size); + InverseDynamics (multdof3_model, sphQ, sphQDot, QDDot_zero, id_zero); + + H_col = id_delta - id_zero; + H_id.block(0, i, multdof3_model.qdot_size, 1) = H_col; + } + + CHECK_ARRAY_CLOSE (H_id.data(), H_crba.data(), multdof3_model.qdot_size * multdof3_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestForwardDynamicsLagrangianVsABA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + sphTau[0] = 5.; + sphTau[1] = 4.; + sphTau[2] = 7.; + sphTau[3] = 3.; + sphTau[4] = 2.; + + ConvertQAndQDotFromEmulated (emulated_model, emuQ, emuQDot, multdof3_model, &sphQ, &sphQDot); + + VectorNd QDDot_aba = VectorNd::Zero (multdof3_model.qdot_size); + VectorNd QDDot_lag = VectorNd::Zero (multdof3_model.qdot_size); + + ForwardDynamicsLagrangian (multdof3_model, sphQ, sphQDot, sphTau, QDDot_lag); + ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, QDDot_aba); + + CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), multdof3_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestContactsLagrangian) { + ConstraintSet constraint_set_emu; + + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_emu.Bind(emulated_model); + + ConstraintSet constraint_set_sph; + + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_sph.Bind(multdof3_model); + + ForwardDynamicsConstraintsDirect (emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot); + VectorNd emu_force_lagrangian = constraint_set_emu.force; + ForwardDynamicsConstraintsDirect (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot); + VectorNd sph_force_lagrangian = constraint_set_sph.force; + + CHECK_ARRAY_CLOSE (emu_force_lagrangian.data(), sph_force_lagrangian.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestContacts) { + ConstraintSet constraint_set_emu; + + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_emu.Bind(emulated_model); + + ConstraintSet constraint_set_sph; + + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 1., 0.)); + constraint_set_sph.AddContactConstraint (sph_child_id, Vector3d (0., 0., -1.), Vector3d (0., 0., 1.)); + + constraint_set_sph.Bind(multdof3_model); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, constraint_set_emu, emuQDDot); + VectorNd emu_force_kokkevis = constraint_set_emu.force; + ForwardDynamicsContactsKokkevis (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot); + VectorNd sph_force_kokkevis = constraint_set_sph.force; + + CHECK_ARRAY_CLOSE (emu_force_kokkevis.data(), sph_force_kokkevis.data(), 3, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedLagrangian ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedArticulatedBodyAlgorithm ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamics (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedContacts ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ConstraintSet CS_euler; + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (1., 1., 1.), Vector3d (1., 0., 0.)); + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.)); + CS_euler.AddContactConstraint (eulerzyx_child_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.)); + CS_euler.Bind (eulerzyx_model); + + ConstraintSet CS_emulated; + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (1., 1., 1.), Vector3d (1., 0., 0.)); + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (0., 0., 0.), Vector3d (0., 1., 0.)); + CS_emulated.AddContactConstraint (emu_child_id, Vector3d (0., 0., 0.), Vector3d (0., 0., 1.)); + CS_emulated.Bind (emulated_model); + + ForwardDynamicsConstraintsDirect (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); + + ClearLogOutput(); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsContactsKokkevis (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm()); + + ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu); + ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm()); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedCRBA ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + MatrixNd H_emulated (MatrixNd::Zero (emulated_model.q_size, emulated_model.q_size)); + MatrixNd H_eulerzyx (MatrixNd::Zero (eulerzyx_model.q_size, eulerzyx_model.q_size)); + + CompositeRigidBodyAlgorithm (emulated_model, emuQ, H_emulated); + CompositeRigidBodyAlgorithm (eulerzyx_model, emuQ, H_eulerzyx); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_eulerzyx.data(), emulated_model.q_size * emulated_model.q_size, TEST_PREC); +} + +TEST ( TestJointTypeTranslationZYX ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + Joint joint_3dof (JointTypeTranslationXYZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.1 * (static_cast(i + 1)); + qddot_3dof[i] = 0.21 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + qddot_emulated = qddot_3dof; + VectorNd id_emulated (tau); + VectorNd id_3dof(tau); + InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated); + InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof); + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm()); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_EQUAL (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size()); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST ( TestJointTypeEulerXYZ ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerXYZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated); + UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated); + + CHECK_ARRAY_EQUAL (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9); + CHECK_ARRAY_EQUAL (model_emulated.v[3].data(), model_3dof.v[1].data(), 6); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST ( TestJointTypeEulerYXZ ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot_3dof[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + qddot_emulated = qddot_3dof; + + UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated); + UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated); + + CHECK_ARRAY_CLOSE (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC); + + VectorNd id_emulated (tau); + VectorNd id_3dof(tau); + InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated); + InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof); + + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST ( TestJointTypeEulerZXY ) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 0., 1., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerZXY); + + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_emulated.AppendBody (SpatialTransform (), joint_emulated, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + model_3dof.AppendBody (SpatialTransform (), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot_3dof[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + qddot_emulated = qddot_3dof; + + UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated); + UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated); + + CHECK_ARRAY_CLOSE (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC); + + VectorNd id_emulated (tau); + VectorNd id_3dof(tau); + InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated); + InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof); + + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC); + + ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated); + ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof); + + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC); + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); +} + +TEST_FIXTURE (Human36, TestUpdateKinematics) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd id_emulated (tau); + VectorNd id_3dof (tau); + + UpdateKinematics (*model_emulated, q, qdot, qddot); + UpdateKinematics (*model_3dof, q, qdot, qddot); + + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyPelvis]].data(), model_3dof->v[body_id_3dof[BodyPelvis]].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyThighRight]].data(), model_3dof->v[body_id_3dof[BodyThighRight]].data(), 6, TEST_PREC); + CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyShankRight]].data(), model_3dof->v[body_id_3dof[BodyShankRight]].data(), 6, TEST_PREC); +} + +TEST_FIXTURE (Human36, TestInverseDynamics) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd id_emulated (tau); + VectorNd id_3dof (tau); + + ClearLogOutput(); + InverseDynamics (*model_emulated, q, qdot, qddot, id_emulated); + InverseDynamics (*model_3dof, q, qdot, qddot, id_3dof); + + CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestNonlinearEffects) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd nle_emulated (tau); + VectorNd nle_3dof (tau); + + ClearLogOutput(); + NonlinearEffects (*model_emulated, q, qdot, nle_emulated); + NonlinearEffects (*model_3dof, q, qdot, nle_3dof); + + CHECK_ARRAY_CLOSE (nle_emulated.data(), nle_3dof.data(), nle_emulated.size(), TEST_PREC * nle_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_kokkevis (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_kokkevis); + + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = -0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_sparse (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_sparse); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = -0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_lagrangian (qddot_emulated); + VectorNd qddot_nullspace (qddot_emulated); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian); + ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace); + CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qddot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + + for (unsigned int i = 0; i < q.size(); i++) { + CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated); + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof); + CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated); + + for (unsigned int i = 0; i < q.size(); i++) { + CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]); + } + + VectorNd qddot_sparse (qddot_emulated); + VectorNd qddot_kokkevis (qddot_emulated); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); + + ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_sparse); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis); + CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm()); +} + +TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) { + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + tau[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(RAND_MAX); + } + + VectorNd qddot_kokkevis (qddot_emulated); + VectorNd qddot_kokkevis_2 (qddot_emulated); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); + + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis); + ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis_2); + CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm()); +} + +TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) { + emuQ[0] = 1.1; + emuQ[1] = 1.2; + emuQ[2] = 1.3; + emuQ[3] = 1.4; + emuQ[4] = 1.5; + + emuQDot[0] = 1.; + emuQDot[1] = 2.; + emuQDot[2] = 3.; + emuQDot[3] = 4.; + emuQDot[4] = 5.; + + emuTau[0] = 5.; + emuTau[1] = 4.; + emuTau[2] = 7.; + emuTau[3] = 3.; + emuTau[4] = 2.; + + VectorNd QDDot_emu = VectorNd::Zero (emulated_model.qdot_size); + VectorNd QDDot_eulerzyx = VectorNd::Zero (eulerzyx_model.qdot_size); + + ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu); + ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx); + + CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC); +} + +TEST_FIXTURE ( Human36, SolveMInvTimesTau) { + for (unsigned int i = 0; i < model->dof_count; i++) { + q[i] = rand() / static_cast(RAND_MAX); + tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, q, M); + + VectorNd qddot_solve_llt = M.llt().solve(tau); + + VectorNd qddot_minv (q); + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_minv.norm()); +} + +TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) { + for (unsigned int i = 0; i < model->dof_count; i++) { + q[i] = rand() / static_cast(RAND_MAX); + tau[i] = rand() / static_cast(RAND_MAX); + } + + MatrixNd M (MatrixNd::Zero(model->dof_count, model->dof_count)); + CompositeRigidBodyAlgorithm (*model, q, M); + + VectorNd qddot_solve_llt = M.llt().solve(tau); + + VectorNd qddot_minv (q); + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + for (unsigned int j = 0; j < 1; j++) { + for (unsigned int i = 0; i < model->dof_count; i++) { + tau[i] = rand() / static_cast(RAND_MAX); + } + + CompositeRigidBodyAlgorithm (*model, q, M); + qddot_solve_llt = M.llt().solve(tau); + + CalcMInvTimesTau (*model, q, tau, qddot_minv); + + CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_solve_llt.norm()); + } +} diff --git a/3rdparty/rbdl/tests/PendulumModels.h b/3rdparty/rbdl/tests/PendulumModels.h new file mode 100644 index 0000000..a3071a2 --- /dev/null +++ b/3rdparty/rbdl/tests/PendulumModels.h @@ -0,0 +1,183 @@ +#include "rbdl/rbdl.h" + +struct DoublePerpendicularPendulumJointCoordinates { + DoublePerpendicularPendulumJointCoordinates() + : model() + , q() + , qd() + , qdd() + , tau() + , l1(1.) + , l2(1.) + , m1(1.) + , m2(1.) + , idB1(0) + , idB2(0){ + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + model.gravity = Vector3d(0.,-9.81,0.); + /* + The perpendicular pendulum pictured with joint angles of 0,0. + The first joint rotates about the x axis, while the second + joint rotates about the local y axis of link 1 + + y + | + |___ x + z / | + | | + / | | link1 + | | + / | | +axis1:z0| |__________ + (_____________) link 2 + | | + | + + | + + | axis2:y1 + */ + + Body link1 = Body(m1, Vector3d( 0., -l1*0.5, 0.), + Matrix3d( m1*l1*l1/3., 0., 0., + 0., m1*l1*l1/30., 0., + 0., 0., m1*l1*l1/3.)); + + + Body link2 = Body(m2, Vector3d( l2*0.5, 0., 0.), + Matrix3d( m2*l2*l2/30., 0., 0., + 0., m2*l2*l2/3., 0., + 0., 0., m2*l2*l2/3.)); + + + Joint joint_rev_z = Joint(SpatialVector(0.,0.,1.,0.,0.,0.)); + Joint joint_rev_y = Joint(SpatialVector(0.,1.,0.,0.,0.,0.)); + + idB1 = model.AddBody( 0, Xtrans(Vector3d(0., 0, 0. )), + joint_rev_z, link1, "body1"); + idB2 = model.AddBody(idB1, Xtrans(Vector3d(0.,-l1, 0.)), + joint_rev_y, link2, "body2"); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + } + + RigidBodyDynamics::Model model; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qd; + RigidBodyDynamics::Math::VectorNd qdd; + RigidBodyDynamics::Math::VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB1; + unsigned int idB2; +}; + +struct DoublePerpendicularPendulumAbsoluteCoordinates { + DoublePerpendicularPendulumAbsoluteCoordinates() + : model() + , cs() + , q() + , qd() + , qdd() + , tau() + , l1(1.) + , l2(1.) + , m1(1.) + , m2(1.) + , idB1(0) + , idB2(0) + , X_p1(RigidBodyDynamics::Math::Xtrans( + RigidBodyDynamics::Math::Vector3d(0., 0., 0.))) + , X_s1(RigidBodyDynamics::Math::Xtrans( + RigidBodyDynamics::Math::Vector3d(0., 0., 0.))) + , X_p2(RigidBodyDynamics::Math::Xtrans( + RigidBodyDynamics::Math::Vector3d(0.,-l1, 0.))) + , X_s2(RigidBodyDynamics::Math::Xtrans( + RigidBodyDynamics::Math::Vector3d(0., 0., 0.))){ + + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + model.gravity = Vector3d(0.,-9.81,0.); + //Planar pendulum is at 0 when it is hanging down. + // x: points to the right + // y: points up + // z: out of the page + Body link1 = Body(m1, Vector3d( 0., -l1*0.5, 0.), + Matrix3d( m1*l1*l1/3., 0. , 0., + 0., m1*l1*l1/30., 0., + 0., 0. , m1*l1*l1/3.)); + + + Body link2 = Body(m2, Vector3d( l2*0.5, 0., 0.), + Matrix3d( m2*l2*l2/30., 0., 0., + 0., m2*l2*l2/3., 0., + 0., 0., m2*l2*l2/3.)); + + + Body nullbody = Body(0, Vector3d( 0.,0.,0.), + Vector3d( 0.,0.,0.)); + Joint jointT123 (JointTypeTranslationXYZ); + Joint jointA123 (JointTypeEulerZYX); + + model.AddBody(0, SpatialTransform(), jointT123, nullbody); + idB1 = model.AppendBody(SpatialTransform(), jointA123, link1); + + model.AddBody(0, SpatialTransform(), jointT123, nullbody); + idB2 = model.AppendBody(SpatialTransform(), jointA123, link2); + + //Make the revolute joints about the y axis using 5 constraints + //between the end points + cs.AddLoopConstraint(0, idB1, X_p1, X_s1, SpatialVector(0,0,0,1,0,0)); + cs.AddLoopConstraint(0, idB1, X_p1, X_s1, SpatialVector(0,0,0,0,1,0)); + cs.AddLoopConstraint(0, idB1, X_p1, X_s1, SpatialVector(0,0,0,0,0,1)); + cs.AddLoopConstraint(0, idB1, X_p1, X_s1, SpatialVector(1,0,0,0,0,0)); + cs.AddLoopConstraint(0, idB1, X_p1, X_s1, SpatialVector(0,1,0,0,0,0)); + + + cs.AddLoopConstraint(idB1, idB2, X_p2, X_s2, SpatialVector(0,0,0,1,0,0)); + cs.AddLoopConstraint(idB1, idB2, X_p2, X_s2, SpatialVector(0,0,0,0,1,0)); + cs.AddLoopConstraint(idB1, idB2, X_p2, X_s2, SpatialVector(0,0,0,0,0,1)); + cs.AddLoopConstraint(idB1, idB2, X_p2, X_s2, SpatialVector(1,0,0,0,0,0)); + cs.AddLoopConstraint(idB1, idB2, X_p2, X_s2, SpatialVector(0,0,1,0,0,0)); + + cs.Bind(model); + + q = VectorNd::Zero(model.dof_count); + qd = VectorNd::Zero(model.dof_count); + qdd = VectorNd::Zero(model.dof_count); + tau = VectorNd::Zero(model.dof_count); + } + + + RigidBodyDynamics::Model model; + RigidBodyDynamics::ConstraintSet cs; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qd; + RigidBodyDynamics::Math::VectorNd qdd; + RigidBodyDynamics::Math::VectorNd tau; + + double l1; + double l2; + double m1; + double m2; + + unsigned int idB1; + unsigned int idB2; + + RigidBodyDynamics::Math::SpatialTransform X_p1; + RigidBodyDynamics::Math::SpatialTransform X_s1; + RigidBodyDynamics::Math::SpatialTransform X_p2; + RigidBodyDynamics::Math::SpatialTransform X_s2; +}; diff --git a/3rdparty/rbdl/tests/ScrewJointTests.cc b/3rdparty/rbdl/tests/ScrewJointTests.cc new file mode 100644 index 0000000..c5966b8 --- /dev/null +++ b/3rdparty/rbdl/tests/ScrewJointTests.cc @@ -0,0 +1,111 @@ +#include + +#include +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Constraints.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +struct ScrewJoint1DofFixedBase { + ScrewJoint1DofFixedBase() { + using namespace RigidBodyDynamics; + using namespace RigidBodyDynamics::Math; + + ClearLogOutput(); + model = new Model; + + /* Single screw joint with a fixed base. Rotation about Z, translation along X. + * A rolling log. + */ + + Body body = Body (1., Vector3d (0., 0, 0.), Vector3d (1., 1., 1.)); + + Joint joint = Joint (SpatialVector (0., 0., 1., 1., 0., 0.)); + + roller = model->AppendBody (Xtrans (Vector3d (0., 0., 0.)), joint, body, "Roller"); + + q = VectorNd::Constant ((size_t) model->dof_count, 0.); + qdot = VectorNd::Constant ((size_t) model->dof_count, 0.); + qddot = VectorNd::Constant ((size_t) model->dof_count, 0.); + tau = VectorNd::Constant ((size_t) model->dof_count, 0.); + + epsilon = 1e-8; + + ClearLogOutput(); + } + ~ScrewJoint1DofFixedBase() { + delete model; + } + + RigidBodyDynamics::Model *model; + + RigidBodyDynamics::Math::VectorNd q; + RigidBodyDynamics::Math::VectorNd qdot; + RigidBodyDynamics::Math::VectorNd qddot; + RigidBodyDynamics::Math::VectorNd tau; + + unsigned int roller; + double epsilon; +}; + + +TEST_FIXTURE ( ScrewJoint1DofFixedBase, UpdateKinematics ) { + q[0] = 1; + qdot[0] = 2; + qddot[0] = 0; + + UpdateKinematics (*model, q, qdot, qddot); + + CHECK_ARRAY_EQUAL (Xrot(1,Vector3d(0,0,1)).E.data(), model->X_base[roller].E.data(), 9); + CHECK_ARRAY_EQUAL (Vector3d(1.,0.,0.).data(), model->X_base[roller].r.data(), 3); + CHECK_ARRAY_EQUAL (SpatialVector(0.,0.,2.,cos(q[0])*2,-sin(q[0])*2.,0.).data(), model->v[roller].data(), 6); + + SpatialVector a0(model->a[roller]); + SpatialVector v0(model->v[roller]); + + q[0] = 1+2*epsilon; + qdot[0] = 2; + qddot[0] = 0; + + UpdateKinematics (*model, q, qdot, qddot); + + v0 = model->v[roller] - v0; + v0 /= epsilon; + + CHECK_ARRAY_CLOSE (a0.data(),v0.data(), 6, 1e-5); //finite diff vs. analytical derivative + +} + +TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) { + q[0] = 1; + qdot[0] = 0; + qddot[0] = 9; + + Vector3d refPt = Vector3d(1,0,3); + MatrixNd GrefPt = MatrixNd::Constant(3,1,0.); + MatrixNd Gexpected = MatrixNd::Constant(3,1,0.); + Vector3d refPtBaseCoord = Vector3d(); + + refPtBaseCoord = CalcBodyToBaseCoordinates(*model, q, roller, refPt); + + CHECK_ARRAY_EQUAL (Vector3d(1+cos(1), sin(1), 3).data(), refPtBaseCoord.data(), 3); + + CalcPointJacobian(*model, q, roller, refPt, GrefPt); + + Gexpected(0,0) = 1 - sin(1); + Gexpected(1,0) = cos(1); + Gexpected(2,0) = 0; + + CHECK_ARRAY_EQUAL (Gexpected.data(), GrefPt.data(), 3); +} diff --git a/3rdparty/rbdl/tests/SparseFactorizationTests.cc b/3rdparty/rbdl/tests/SparseFactorizationTests.cc new file mode 100644 index 0000000..75a60dd --- /dev/null +++ b/3rdparty/rbdl/tests/SparseFactorizationTests.cc @@ -0,0 +1,268 @@ +#include + +#include + +#include "Fixtures.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-12; + +TEST_FIXTURE (FloatingBase12DoF, TestSparseFactorizationLTL) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + MatrixNd LTL = L.transpose() * L; + + CHECK_ARRAY_CLOSE (H.data(), LTL.data(), model->qdot_size * model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLx) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + VectorNd x = L * Q; + + SparseSolveLx (*model, L, x); + + CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLTx) { + for (unsigned int i = 0; i < model->q_size; i++) { + Q[i] = static_cast (i + 1) * 0.1; + } + + MatrixNd H (MatrixNd::Zero (model->qdot_size, model->qdot_size)); + + CompositeRigidBodyAlgorithm (*model, Q, H); + + MatrixNd L (H); + SparseFactorizeLTL (*model, L); + VectorNd x = L.transpose() * Q; + + SparseSolveLTx (*model, L, x); + + CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC); +} + +TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsSparse) { + ConstraintSet constraint_set_var1; + + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.)); + constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (0., 1., 0.)); + constraint_set.AddContactConstraint (child_2_id, contact_point, Vector3d (0., 1., 0.)); + + constraint_set_var1 = constraint_set.Copy(); + constraint_set_var1.Bind (*model); + constraint_set.Bind (*model); + + VectorNd QDDot_var1 = VectorNd::Constant (model->dof_count, 0.); + + Q[0] = 0.1; + Q[1] = -0.3; + Q[2] = 0.15; + Q[3] = -0.21; + Q[4] = -0.81; + Q[5] = 0.11; + Q[6] = 0.31; + Q[7] = -0.91; + Q[8] = 0.61; + + QDot[0] = 1.3; + QDot[1] = -1.7; + QDot[2] = 3; + QDot[3] = -2.5; + QDot[4] = 1.5; + QDot[5] = -5.5; + QDot[6] = 2.5; + QDot[7] = -1.5; + QDot[8] = -3.5; + + ClearLogOutput(); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set, QDDot); + + ClearLogOutput(); + ForwardDynamicsConstraintsRangeSpaceSparse (*model, Q, QDot, Tau, constraint_set_var1, QDDot_var1); + + CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_var1.data(), QDDot.size(), TEST_PREC); +} + +TEST ( TestSparseFactorizationMultiDof) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + Joint joint_rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_emulated = model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + model_emulated.AddBody (multdof_body_id_emulated, SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + model_emulated.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body); + + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_3dof = model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + model_3dof.AddBody (multdof_body_id_3dof, SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + model_3dof.AppendBody (SpatialTransform (Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + VectorNd b (VectorNd::Zero (q.size())); + VectorNd x_emulated (VectorNd::Zero (q.size())); + VectorNd x_3dof (VectorNd::Zero (q.size())); + + for (unsigned int i = 0; i < b.size(); i++) { + b[i] = static_cast (i + 1) * 2.152; + } + b = H_emulated * b; + + SparseFactorizeLTL (model_emulated, H_emulated); + SparseFactorizeLTL (model_3dof, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); + + x_emulated = b; + SparseSolveLx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); + + x_emulated = b; + SparseSolveLTx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLTx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); +} + +TEST ( TestSparseFactorizationMultiDofAndFixed) { + Model model_emulated; + Model model_3dof; + + Body body (1., Vector3d (1., 2., 1.), Matrix3d (1., 0., 0, 0., 1., 0., 0., 0., 1.)); + Joint joint_emulated ( + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + Joint joint_3dof (JointTypeEulerYXZ); + + Joint joint_rot_y ( + SpatialVector (0., 1., 0., 0., 0., 0.) + ); + + SpatialTransform translate_x (Matrix3d::Identity(), Vector3d (1., 0., 0.)); + + model_emulated.AppendBody (SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_emulated = model_emulated.AppendBody (translate_x, joint_emulated, body); + model_emulated.AppendBody (translate_x, joint_emulated, body); + model_emulated.AddBody(multdof_body_id_emulated, translate_x, Joint(JointTypeFixed), body); + model_emulated.AppendBody (translate_x, joint_emulated, body); + + model_3dof.AppendBody (SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body); + unsigned int multdof_body_id_3dof = model_3dof.AppendBody (translate_x, joint_3dof, body); + model_3dof.AppendBody (translate_x, joint_3dof, body); + model_3dof.AddBody (multdof_body_id_3dof, translate_x, Joint(JointTypeFixed), body); + model_3dof.AppendBody (translate_x, joint_3dof, body); + + VectorNd q (VectorNd::Zero (model_emulated.q_size)); + VectorNd qdot (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_emulated (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd qddot_3dof (VectorNd::Zero (model_emulated.qdot_size)); + VectorNd tau (VectorNd::Zero (model_emulated.qdot_size)); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 1.1 * (static_cast(i + 1)); + qdot[i] = 0.55* (static_cast(i + 1)); + qddot_emulated[i] = 0.23 * (static_cast(i + 1)); + qddot_3dof[i] = 0.22 * (static_cast(i + 1)); + tau[i] = 2.1 * (static_cast(i + 1)); + } + + MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size())); + MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size())); + + CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated); + CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof); + + VectorNd b (VectorNd::Zero (q.size())); + VectorNd x_emulated (VectorNd::Zero (q.size())); + VectorNd x_3dof (VectorNd::Zero (q.size())); + + for (unsigned int i = 0; i < b.size(); i++) { + b[i] = static_cast (i + 1) * 2.152; + } + b = H_emulated * b; + + SparseFactorizeLTL (model_emulated, H_emulated); + SparseFactorizeLTL (model_3dof, H_3dof); + + CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC); + + x_emulated = b; + SparseSolveLx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); + + x_emulated = b; + SparseSolveLTx (model_emulated, H_emulated, x_emulated); + x_3dof = b; + SparseSolveLTx (model_3dof, H_3dof, x_3dof); + + CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9); +} diff --git a/3rdparty/rbdl/tests/SpatialAlgebraTests.cc b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc new file mode 100644 index 0000000..6fed310 --- /dev/null +++ b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc @@ -0,0 +1,586 @@ +#include + +#include +#include + +#include "rbdl/Body.h" +#include "rbdl/rbdl_math.h" +#include "rbdl/rbdl_mathutils.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-14; + +SpatialMatrix spatial_adjoint(const SpatialMatrix &m) { + SpatialMatrix res (m); + res.block<3,3>(3,0) = m.block<3,3>(0,3); + res.block<3,3>(0,3) = m.block<3,3>(3,0); + return res; +} + +SpatialMatrix spatial_inverse(const SpatialMatrix &m) { + SpatialMatrix res(m); + res.block<3,3>(0,0) = m.block<3,3>(0,0).transpose(); + res.block<3,3>(3,0) = m.block<3,3>(3,0).transpose(); + res.block<3,3>(0,3) = m.block<3,3>(0,3).transpose(); + res.block<3,3>(3,3) = m.block<3,3>(3,3).transpose(); + return res; +} + +Matrix3d get_rotation (const SpatialMatrix &m) { + return m.block<3,3>(0,0); +} + +Vector3d get_translation (const SpatialMatrix &m) { + return Vector3d (-m(4,2), m(3,2), -m(3,1)); +} + +/// \brief Checks the multiplication of a SpatialMatrix with a SpatialVector +TEST(TestSpatialMatrixTimesSpatialVector) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + SpatialVector s_vector ( + 1., 2., 3., 4., 5., 6. + ); + + SpatialVector result; + result = s_matrix * s_vector; + + SpatialVector test_result ( + 43., 44., 45., 34., 35., 40. + ); + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialVector +TEST(TestScalarTimesSpatialVector) { + SpatialVector s_vector ( + 1., 2., 3., 4., 5., 6. + ); + + SpatialVector result; + result = 3. * s_vector; + + SpatialVector test_result(3., 6., 9., 12., 15., 18.); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialMatrix +TEST(TestScalarTimesSpatialMatrix) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + + SpatialMatrix result; + result = 3. * s_matrix; + + SpatialMatrix test_result( + 3., 0., 0., 0., 0., 21., + 0., 6., 0., 0., 24., 0., + 0., 0., 9., 27., 0., 0., + 0., 0., 18., 12., 0., 0., + 0., 15., 0., 0., 15., 0., + 12., 0., 0., 0., 0., 18. + ); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the multiplication of a scalar with a SpatialMatrix +TEST(TestSpatialMatrixTimesSpatialMatrix) { + SpatialMatrix s_matrix ( + 1., 0., 0., 0., 0., 7., + 0., 2., 0., 0., 8., 0., + 0., 0., 3., 9., 0., 0., + 0., 0., 6., 4., 0., 0., + 0., 5., 0., 0., 5., 0., + 4., 0., 0., 0., 0., 6. + ); + + SpatialMatrix result; + result = s_matrix * s_matrix; + + SpatialMatrix test_result( + 29., 0., 0., 0., 0., 49., + 0., 44., 0., 0., 56., 0., + 0., 0., 63., 63., 0., 0., + 0., 0., 42., 70., 0., 0., + 0., 35., 0., 0., 65., 0., + 28., 0., 0., 0., 0., 64. + ); + + CHECK_EQUAL (test_result, result); +} + +/// \brief Checks the adjoint method +// +// This method computes a spatial force transformation from a spatial +// motion transformation and vice versa +TEST(TestSpatialMatrixTransformAdjoint) { + SpatialMatrix s_matrix ( + 1., 2., 3., 4., 5., 6., + 7., 8., 9., 10., 11., 12., + 13., 14., 15., 16., 17., 18., + 19., 20., 21., 22., 23., 24., + 25., 26., 27., 28., 29., 30., + 31., 32., 33., 34., 35., 36. + ); + + SpatialMatrix result = spatial_adjoint(s_matrix); + + SpatialMatrix test_result_matrix ( + 1., 2., 3., 19., 20., 21., + 7., 8., 9., 25., 26., 27., + 13., 14., 15., 31., 32., 33., + 4., 5., 6., 22., 23., 24., + 10., 11., 12., 28., 29., 30., + 16., 17., 18., 34., 35., 36.); + + CHECK_EQUAL (test_result_matrix, result); +} + +TEST(TestSpatialMatrixInverse) { + SpatialMatrix s_matrix ( + 0, 1, 2, 0, 1, 2, + 3, 4, 5, 3, 4, 5, + 6, 7, 8, 6, 7, 8, + 0, 1, 2, 0, 1, 2, + 3, 4, 5, 3, 4, 5, + 6, 7, 8, 6, 7, 8 + ); + + SpatialMatrix test_inv ( + 0, 3, 6, 0, 3, 6, + 1, 4, 7, 1, 4, 7, + 2, 5, 8, 2, 5, 8, + 0, 3, 6, 0, 3, 6, + 1, 4, 7, 1, 4, 7, + 2, 5, 8, 2, 5, 8 + ); + + CHECK_EQUAL (test_inv, spatial_inverse(s_matrix)); +} + +TEST(TestSpatialMatrixGetRotation) { + SpatialMatrix spatial_transform ( + 1., 2., 3., 0., 0., 0., + 4., 5., 6., 0., 0., 0., + 7., 8., 9., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0. + ); + + // Matrix3d rotation = spatial_transform.block<3,3>(0,0); + Matrix3d rotation = get_rotation (spatial_transform); + Matrix3d test_result ( + 1., 2., 3., + 4., 5., 6., + 7., 8., 9. + ); + + CHECK_EQUAL(test_result, rotation); +} + +TEST(TestSpatialMatrixGetTranslation) { + SpatialMatrix spatial_transform ( + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., + 0., -3., 2., 0., 0., 0., + 0., 0., -1., 0., 0., 0., + 0., 0., 0., 0., 0., 0. + ); + + Vector3d translation = get_translation(spatial_transform); + Vector3d test_result ( + 1., 2., 3. + ); + + CHECK_EQUAL( test_result, translation); +} + +TEST(TestSpatialVectorCross) { + SpatialVector s_vec (1., 2., 3., 4., 5., 6.); + + SpatialMatrix test_cross ( + 0., -3., 2., 0., 0., 0., + 3., 0., -1., 0., 0., 0., + -2., 1., 0., 0., 0., 0., + 0., -6., 5., 0., -3., 2., + 6., 0., -4., 3., 0., -1., + -5., 4., 0., -2., 1., 0. + ); + + SpatialMatrix s_vec_cross (crossm(s_vec)); + CHECK_EQUAL (test_cross, s_vec_cross); + + SpatialMatrix s_vec_crossf (crossf(s_vec)); + SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose(); + + CHECK_EQUAL (test_crossf, s_vec_crossf); +} + +TEST(TestSpatialVectorCrossmCrossf) { + SpatialVector s_vec (1., 2., 3., 4., 5., 6.); + SpatialVector t_vec (9., 8., 7., 6., 5., 4.); + + // by explicitly building the matrices (crossm/f with only one vector) + SpatialVector crossm_s_x_t = crossm(s_vec) * t_vec; + SpatialVector crossf_s_x_t = crossf(s_vec) * t_vec; + + // by using direct computation that avoids building of the matrix + SpatialVector crossm_s_t = crossm(s_vec, t_vec); + SpatialVector crossf_s_t = crossf(s_vec, t_vec); + + /* + cout << crossm_s_x_t << endl; + cout << "---" << endl; + cout << crossf_s_x_t << endl; + cout << "---" << endl; + cout << crossf_s_t << endl; + */ + + CHECK_EQUAL (crossm_s_x_t, crossm_s_t); + CHECK_EQUAL (crossf_s_x_t, crossf_s_t); +} + +TEST(TestSpatialTransformApply) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialTransform X_st; + X_st.r = trans; + + SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6)); + X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_st.E = X_66_matrix.block<3,3>(0,0); + + // cout << X_66_matrix << endl; + // cout << X_st.E << endl; + // cout << X_st.r.transpose() << endl; + + SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1); + SpatialVector v_66_res = X_66_matrix * v; + SpatialVector v_st_res = X_st.apply(v); + + // cout << (v_66_res - v_st_res).transpose() << endl; + + CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformApplyTranspose) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialTransform X_st; + X_st.r = trans; + + SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6)); + X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_st.E = X_66_matrix.block<3,3>(0,0); + + // cout << X_66_matrix << endl; + // cout << X_st.E << endl; + // cout << X_st.r.transpose() << endl; + + SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1); + SpatialVector v_66_res = X_66_matrix.transpose() * v; + SpatialVector v_st_res = X_st.applyTranspose(v); + + // cout << (v_66_res - v_st_res).transpose() << endl; + + CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformApplyAdjoint) { + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialMatrix X_adjoint = X.toMatrixAdjoint(); + + SpatialVector f (1.1, 2.1, 4.1, 9.2, 3.3, 0.8); + SpatialVector f_apply = X.applyAdjoint(f); + SpatialVector f_matrix = X_adjoint * f; + + CHECK_ARRAY_CLOSE (f_matrix.data(), f_apply.data(), 6, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrix) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // SpatialMatrix X_diff = X_st.toMatrix() - X_matrix; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix.data(), X_st.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrixAdjoint) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // SpatialMatrix X_diff = X_st.toMatrixAdjoint() - spatial_adjoint(X_matrix); + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (spatial_adjoint(X_matrix).data(), X_st.toMatrixAdjoint().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformToMatrixTranspose) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6)); + X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st; + X_st.E = X_matrix.block<3,3>(0,0); + X_st.r = trans; + + // we have to copy the matrix as it is only transposed via a flag and + // thus data() does not return the proper data. + SpatialMatrix X_matrix_transposed = X_matrix.transpose(); + // SpatialMatrix X_diff = X_st.toMatrixTranspose() - X_matrix_transposed; + // cout << "Error: " << endl << X_diff << endl; + // cout << "X_st: " << endl << X_st.toMatrixTranspose() << endl; + // cout << "X: " << endl << X_matrix_transposed() << endl; + + CHECK_ARRAY_CLOSE (X_matrix_transposed.data(), X_st.toMatrixTranspose().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformMultiply) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6)); + SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6)); + + X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st_1; + SpatialTransform X_st_2; + + X_st_1.E = X_matrix_1.block<3,3>(0,0); + X_st_1.r = trans; + X_st_2.E = X_matrix_2.block<3,3>(0,0); + X_st_2.r = trans; + + SpatialTransform X_st_res = X_st_1 * X_st_2; + SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2; + + // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformMultiplyEqual) { + Vector3d rot (1.1, 1.2, 1.3); + Vector3d trans (1.1, 1.2, 1.3); + + SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6)); + SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6)); + + X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans); + + SpatialTransform X_st_1; + SpatialTransform X_st_2; + + X_st_1.E = X_matrix_1.block<3,3>(0,0); + X_st_1.r = trans; + X_st_2.E = X_matrix_2.block<3,3>(0,0); + X_st_2.r = trans; + + SpatialTransform X_st_res = X_st_1; + X_st_res *= X_st_2; + SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2; + + // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res; + // cout << "Error: " << endl << X_diff << endl; + + CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestXrotAxis) { + SpatialTransform X_rotX = Xrotx (M_PI * 0.15); + SpatialTransform X_rotX_axis = Xrot (M_PI * 0.15, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 36, TEST_PREC); + + // all the other axes + SpatialTransform X_rotX_90 = Xrotx (M_PI * 0.5); + SpatialTransform X_rotX_90_axis = Xrot (M_PI * 0.5, Vector3d (1., 0., 0.)); + + CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 36, TEST_PREC); + + SpatialTransform X_rotY_90 = Xroty (M_PI * 0.5); + SpatialTransform X_rotY_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 1., 0.)); + + CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 36, TEST_PREC); + + SpatialTransform X_rotZ_90 = Xrotz (M_PI * 0.5); + SpatialTransform X_rotZ_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 0., 1.)); + + CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 36, TEST_PREC); +} + +TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialMatrix rbi_matrix_added = rbi.toMatrix() + rbi.toMatrix(); + SpatialRigidBodyInertia rbi_added = rbi + rbi; + + // cout << "rbi = " << endl << rbi.toMatrix() << endl; + // cout << "rbi_added = " << endl << rbi_added.toMatrix() << endl; + // cout << "rbi_matrix_added = " << endl << rbi_matrix_added << endl; + // cout << "diff = " << endl << + // rbi_added.toMatrix() - rbi_matrix_added << endl; + + CHECK_ARRAY_CLOSE ( + rbi_matrix_added.data(), + rbi_added.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialRigidBodyInertia rbi_transformed = X.apply (rbi); + SpatialMatrix rbi_matrix_transformed = X.toMatrixAdjoint () * rbi.toMatrix() * X.inverse().toMatrix(); + + CHECK_ARRAY_CLOSE ( + rbi_matrix_transformed.data(), + rbi_transformed.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull) { + SpatialRigidBodyInertia rbi ( + 1.1, + Vector3d (1.2, 1.3, 1.4), + Matrix3d ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + )); + + SpatialTransform X ( + Xrotz (0.5) * + Xroty (0.9) * + Xrotx (0.2) * + Xtrans (Vector3d (1.1, 1.2, 1.3)) + ); + + SpatialRigidBodyInertia rbi_transformed = X.applyTranspose (rbi); + SpatialMatrix rbi_matrix_transformed = X.toMatrixTranspose() * rbi.toMatrix() * X.toMatrix(); + + CHECK_ARRAY_CLOSE ( + rbi_matrix_transformed.data(), + rbi_transformed.toMatrix().data(), + 36, + TEST_PREC + ); +} + +TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) { + double mass = 1.1; + Vector3d com (0., 0., 0.); + Matrix3d inertia ( + 1.1, 0.5, 0.3, + 0.5, 1.2, 0.4, + 0.3, 0.4, 1.3 + ); + SpatialRigidBodyInertia body_rbi(mass, com , inertia); + + SpatialMatrix spatial_inertia = body_rbi.toMatrix(); + + SpatialRigidBodyInertia rbi; + rbi.createFromMatrix (spatial_inertia); + + CHECK_EQUAL (mass, rbi.m); + CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3); + Matrix3d rbi_I_matrix ( + rbi.Ixx, rbi.Iyx, rbi.Izx, + rbi.Iyx, rbi.Iyy, rbi.Izy, + rbi.Izx, rbi.Izy, rbi.Izz + ); + CHECK_ARRAY_EQUAL (inertia.data(), rbi_I_matrix.data(), 9); +} + +#ifdef USE_SLOW_SPATIAL_ALGEBRA +TEST(TestSpatialLinSolve) { + SpatialVector b (1, 2, 0, 1, 1, 1); + SpatialMatrix A ( + 1., 2., 3., 0., 0., 0., + 3., 4., 5., 0., 0., 0., + 6., 7., 7., 0., 0., 0., + 0., 0., 0., 1., 0., 0., + 0., 0., 0., 0., 1., 0., + 0., 0., 0., 0., 0., 1. + ); + + SpatialVector x = SpatialLinSolve (A, b); + SpatialVector x_test (3.5, -6.5, 3.5, 1, 1, 1); + + CHECK_ARRAY_CLOSE (x_test.data(), x.data(), 6, TEST_PREC); +} +#endif diff --git a/3rdparty/rbdl/tests/TwolegModelTests.cc b/3rdparty/rbdl/tests/TwolegModelTests.cc new file mode 100644 index 0000000..ca5c379 --- /dev/null +++ b/3rdparty/rbdl/tests/TwolegModelTests.cc @@ -0,0 +1,404 @@ +#include + +#include + +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Constraints.h" +#include "rbdl/Dynamics.h" +#include "rbdl/Kinematics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +const double TEST_PREC = 1.0e-13; + +unsigned int hip_id, + upper_leg_right_id, + lower_leg_right_id, + foot_right_id, + upper_leg_left_id, + lower_leg_left_id, + foot_left_id; + +Body hip_body, + upper_leg_right_body, + lower_leg_right_body, + foot_right_body, + upper_leg_left_body, + lower_leg_left_body, + foot_left_body; + +Joint joint_txtyrz ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); +Joint joint_rot_z (SpatialVector (0., 0., 1., 0., 0., 0.)); + +VectorNd Q; +VectorNd QDot; +VectorNd QDDot; +VectorNd Tau; + +ConstraintSet constraint_set_right; +ConstraintSet constraint_set_left; +ConstraintSet constraint_set_left_flat; +ConstraintSet constraint_set_both; + +enum ParamNames { + ParamSteplength = 0, + ParamNameLast +}; + +enum PosNames { + PosHipPosX, + PosHipPosY, + PosHipRotZ, + PosRightThighRotZ, + PosRightShankRotZ, + PosRightAnkleRotZ, + PosLeftThighRotZ, + PosLeftShankRotZ, + PosLeftAnkleRotZ, + PosNameLast +}; + +enum StateNames { + StateHipPosX, + StateHipPosY, + StateHipRotZ, + StateRightThighRotZ, + StateRightShankRotZ, + StateRightAnkleRotZ, + StateLeftThighRotZ, + StateLeftShankRotZ, + StateLeftAnkleRotZ, + StateHipVelX, + StateHipVelY, + StateHipRotVelZ, + StateRightThighRotVelZ, + StateRightShankRotVelZ, + StateRightAnkleRotVelZ, + StateLeftThighRotVelZ, + StateLeftShankRotVelZ, + StateLeftAnkleRotVelZ, + StateNameLast +}; + +enum ControlNames { + ControlRightThighRotZ, + ControlRightKneeRotZ, + ControlRightAnkleRotZ, + ControlLeftThighRotZ, + ControlLeftKneeRotZ, + ControlLeftAnkleRotZ, + ControlNameLast +}; + +enum SegmentLengthsNames { + SegmentLengthsHip = 0, + SegmentLengthsThigh, + SegmentLengthsShank, + SegmentLengthsFootHeight, + SegmentLengthsFoot, + SegmentLengthsNameLast +}; + +const double ModelMass = 73.; +const double ModelHeight = 1.741; + +// absolute lengths! +double segment_lengths[SegmentLengthsNameLast] = { + 0.4346, + 0.4222, + 0.4340, + 0.0317, + 0.2581 +}; + +enum JointLocations { + JointLocationHip = 0, + JointLocationKnee, + JointLocationAnkle, + JointLocationLast +}; + +Vector3d joint_location[JointLocationLast] = { + Vector3d (0., 0., 0.), + Vector3d (0., - 0.2425 * ModelHeight, 0.), + Vector3d (0., - 0.2529 * ModelHeight, 0.) +}; + +enum SegmentMassNames { + SegmentMassHip, + SegmentMassThigh, + SegmentMassShank, + SegmentMassFoot, + SegmentMassLast +}; + +double segment_mass[SegmentMassLast] = { + 0.4346 * ModelMass, + 0.1416 * ModelMass, + 0.0433 * ModelMass, + 0.0137 * ModelMass +}; + +enum COMNames { + COMHip, + COMThigh, + COMShank, + COMFoot, + COMNameLast +}; + +Vector3d com_position[COMNameLast] = { + Vector3d (0., 0.3469 * ModelHeight, 0.), + Vector3d (0., 0.2425 * ModelHeight, 0.), + Vector3d (0., 0.2529 * ModelHeight, 0.), + Vector3d (0.0182 * ModelHeight, 0., 0.) +}; + +enum RGyrationNames { + RGyrationHip, + RGyrationThigh, + RGyrationShank, + RGyrationFoot, + RGyrationLast +}; + +Vector3d rgyration[RGyrationLast] = { + Vector3d (0.1981, 0.1021, 0.1848), + Vector3d (0.1389, 0.0629, 0.1389), + Vector3d (0.1123, 0.0454, 0.1096), + Vector3d (0.0081, 0.0039, 0.0078) +}; + +Vector3d heel_point (0., 0., 0.); +Vector3d medial_point (0., 0., 0.); + +void init_model (Model* model) { + assert (model); + + constraint_set_right = ConstraintSet(); + constraint_set_left = ConstraintSet(); + constraint_set_left_flat = ConstraintSet(); + constraint_set_both = ConstraintSet(); + + model->gravity = Vector3d (0., -9.81, 0.); + + // hip + hip_body = Body (segment_mass[SegmentMassHip], com_position[COMHip], rgyration[RGyrationHip]); + + // lateral right + upper_leg_right_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]); + lower_leg_right_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]); + foot_right_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]); + + // lateral left + upper_leg_left_body = Body (segment_mass[SegmentMassThigh], com_position[COMThigh], rgyration[RGyrationThigh]); + lower_leg_left_body = Body (segment_mass[SegmentMassShank], com_position[COMShank], rgyration[RGyrationShank]); + foot_left_body = Body (segment_mass[SegmentMassFoot], com_position[COMFoot], rgyration[RGyrationFoot]); + + // add hip to the model (planar, 3 DOF) + hip_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), joint_txtyrz, hip_body); + + // + // right leg + // + + unsigned int temp_id = 0; + + // add right upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_right_body); + upper_leg_right_id = temp_id; + + // add the right lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_right_body); + lower_leg_right_id = temp_id; + + // add the right foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_right_body); + foot_right_id = temp_id; + + // + // left leg + // + + // add left upper leg + temp_id = model->AddBody (hip_id, Xtrans (Vector3d(0., 0., 0.)), joint_rot_z, upper_leg_left_body); + upper_leg_left_id = temp_id; + + // add the left lower leg (only one DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationKnee]), joint_rot_z, lower_leg_left_body); + lower_leg_left_id = temp_id; + + // add the left foot (1 DOF) + temp_id = model->AddBody (temp_id, Xtrans (joint_location[JointLocationAnkle]), joint_rot_z, foot_left_body); + foot_left_id = temp_id; + + // cerr << "--- model created (" << model->dof_count << " DOF) ---" << endl; + + // contact data + + // the contact points for heel and toe + heel_point.set (-0.05, -0.0317, 0.); + medial_point.set (-0.05, -0.0317 + segment_lengths[SegmentLengthsFoot], 0.); + + constraint_set_right.AddContactConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x"); + constraint_set_right.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y"); + + constraint_set_left.AddContactConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x"); + constraint_set_left.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y"); + + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (1., 0., 0.), "right_heel_x"); + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 1., 0.), "right_heel_y"); + constraint_set_both.AddContactConstraint(foot_right_id, heel_point, Vector3d (0., 0., 1.), "right_heel_z"); + + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (1., 0., 0.), "left_heel_x"); + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 1., 0.), "left_heel_y"); + constraint_set_both.AddContactConstraint(foot_left_id, heel_point, Vector3d (0., 0., 1.), "left_heel_z"); + + constraint_set_right.Bind (*model); + constraint_set_left.Bind (*model); + constraint_set_both.Bind (*model); +} + +template +void copy_values (T *dest, const T *src, size_t count) { + memcpy (dest, src, count * sizeof (T)); +} + +TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) { + Model* model = new Model; + + init_model(model); + + Q.resize(model->dof_count); + QDot.resize(model->dof_count); + QDDot.resize(model->dof_count); + Tau.resize(model->dof_count); + + Q[0] = -0.2; + Q[1] = 0.9; + Q[2] = 0; + Q[3] = -0.15; + Q[4] = -0.15; + Q[5] = 0.1; + Q[6] = 0.15; + Q[7] = -0.15; + Q[8] = 0; + + QDot.setZero(); + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 1; + Tau[4] = 1; + Tau[5] = 1; + Tau[6] = 1; + Tau[7] = 1; + Tau[8] = 1; + + Vector3d contact_accel_left; + Vector3d contact_vel_left; + Vector3d contact_force = Vector3d::Zero(); + + VectorNd QDDot_aba (QDDot); + VectorNd QDDot_lag (QDDot); + ForwardDynamics (*model, Q, QDot, Tau, QDDot_aba); + ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot_lag); + + // cout << "QDDot_aba = " << QDDot_aba.transpose() << endl; + // cout << "QDDot_lag = " << QDDot_lag.transpose() << endl; + + unsigned int body_id = constraint_set_left.body[0]; + Vector3d contact_point = constraint_set_left.point[0]; + + MatrixNd G (3, Q.size()); + CalcPointJacobian (*model, Q, body_id, contact_point, G, true); + + // cout << G << endl; + + ClearLogOutput(); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_left, QDDot); + + // cout << "C0: " << contact_data_left[0].body_id << ", " << contact_data_left[0].point.transpose() << endl; + // cout << "C1: " << contact_data_left[1].body_id << ", " << contact_data_left[1].point.transpose() << endl; + // cout << "td: " << foot_left_id << ", " << heel_point.transpose() << endl; + + contact_force[0] = constraint_set_left.force[0]; + contact_force[1] = constraint_set_left.force[1]; + + CHECK_EQUAL (body_id, foot_left_id); + CHECK_EQUAL (contact_point, heel_point); + + // cout << LogOutput.str() << endl; + contact_accel_left = CalcPointAcceleration (*model, Q, QDot, QDDot, foot_left_id, heel_point); + contact_vel_left = CalcPointVelocity (*model, Q, QDot, foot_left_id, heel_point); + // cout << contact_force << endl; + // cout << contact_accel_left << endl; + + CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), contact_accel_left.data(), 3, TEST_PREC); + + delete model; +} + +TEST ( TestClearContactsInertiaMatrix ) { + Model* model = new Model; + + init_model(model); + + Q.resize(model->dof_count); + QDot.resize(model->dof_count); + QDDot.resize(model->dof_count); + Tau.resize(model->dof_count); + + Q[0] = -0.2; + Q[1] = 0.9; + Q[2] = 0; + Q[3] = -0.15; + Q[4] = -0.15; + Q[5] = 0.1; + Q[6] = 0.15; + Q[7] = -0.15; + Q[8] = 0; + + QDot.setZero(); + + Tau[0] = 0; + Tau[1] = 0; + Tau[2] = 0; + Tau[3] = 1; + Tau[4] = 1; + Tau[5] = 1; + Tau[6] = 1; + Tau[7] = 1; + Tau[8] = 1; + + VectorNd QDDot_aba (QDDot); + VectorNd QDDot_lag (QDDot); + + // initialize matrix with erroneous values + constraint_set_right.bound = false; + constraint_set_right.H = MatrixNd::Zero (model->dof_count, model->dof_count); + for (unsigned int i = 0; i < model->dof_count; i++) { + for (unsigned int j = 0; j < model->dof_count; j++) { + constraint_set_right.H(i,j) = 1.234; + } + } + + constraint_set_right.Bind (*model); + + ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_right, QDDot_lag); + ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set_right, QDDot_aba); + + CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), QDDot.size(), TEST_PREC * QDDot_lag.norm()); + + delete model; +} diff --git a/3rdparty/rbdl/tests/UtilsTests.cc b/3rdparty/rbdl/tests/UtilsTests.cc new file mode 100644 index 0000000..4a97a9d --- /dev/null +++ b/3rdparty/rbdl/tests/UtilsTests.cc @@ -0,0 +1,378 @@ +#include + +#include + +#include "Fixtures.h" +#include "Human36Fixture.h" +#include "rbdl/rbdl_mathutils.h" +#include "rbdl/rbdl_utils.h" +#include "rbdl/Logging.h" + +#include "rbdl/Model.h" +#include "rbdl/Kinematics.h" +#include "rbdl/Dynamics.h" + +using namespace std; +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) { + VectorNd q = VectorNd::Zero(model->q_size); + VectorNd qdot = VectorNd::Zero(model->q_size); + + for (unsigned int i = 0; i < q.size(); i++) { + q[i] = 0.1 * i; + qdot[i] = 0.3 * i; + } + + MatrixNd H = MatrixNd::Zero (model->q_size, model->q_size); + CompositeRigidBodyAlgorithm (*model, q, H, true); + + double kinetic_energy_ref = 0.5 * qdot.transpose() * H * qdot; + double kinetic_energy = Utils::CalcKineticEnergy (*model, q, qdot); + + CHECK_EQUAL (kinetic_energy_ref, kinetic_energy); +} + +TEST(TestPotentialEnergy) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + Body body (0.5, Vector3d (0., 0., 0.), inertia); + Joint joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + + model.AppendBody (Xtrans (Vector3d::Zero()), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + double potential_energy_zero = Utils::CalcPotentialEnergy (model, q); + CHECK_EQUAL (0., potential_energy_zero); + + q[1] = 1.; + double potential_energy_lifted = Utils::CalcPotentialEnergy (model, q); + CHECK_EQUAL (4.905, potential_energy_lifted); +} + +TEST(TestCOMSimple) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + Body body (123., Vector3d (0., 0., 0.), inertia); + Joint joint ( + SpatialVector (0., 0., 0., 1., 0., 0.), + SpatialVector (0., 0., 0., 0., 1., 0.), + SpatialVector (0., 0., 0., 0., 0., 1.) + ); + + model.AppendBody (Xtrans (Vector3d::Zero()), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + VectorNd qdot = VectorNd::Zero(model.qdot_size); + + double mass; + Vector3d com; + Vector3d com_velocity; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity); + + CHECK_EQUAL (123., mass); + CHECK_EQUAL (Vector3d (0., 0., 0.), com); + CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + + q[1] = 1.; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity); + CHECK_EQUAL (Vector3d (0., 1., 0.), com); + CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + + qdot[1] = 1.; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity); + CHECK_EQUAL (Vector3d (0., 1., 0.), com); + CHECK_EQUAL (Vector3d (0., 1., 0.), com_velocity); +} + +TEST(TestAngularMomentumSimple) { + Model model; + Matrix3d inertia = Matrix3d::Zero(3,3); + inertia(0,0) = 1.1; + inertia(1,1) = 2.2; + inertia(2,2) = 3.3; + + Body body (0.5, Vector3d (1., 0., 0.), inertia); + Joint joint ( + SpatialVector (1., 0., 0., 0., 0., 0.), + SpatialVector (0., 1., 0., 0., 0., 0.), + SpatialVector (0., 0., 1., 0., 0., 0.) + ); + + model.AppendBody (Xtrans (Vector3d(0., 0., 0.)), joint, body); + + VectorNd q = VectorNd::Zero(model.q_size); + VectorNd qdot = VectorNd::Zero(model.qdot_size); + + double mass; + Vector3d com; + Vector3d angular_momentum; + + qdot << 1., 0., 0.; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (1.1, 0., 0.), angular_momentum); + + qdot << 0., 1., 0.; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (0., 2.2, 0.), angular_momentum); + + qdot << 0., 0., 1.; + Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + CHECK_EQUAL (Vector3d (0., 0., 3.3), angular_momentum); +} + +TEST_FIXTURE (TwoArms12DoF, TestAngularMomentumSimple) { + double mass; + Vector3d com; + Vector3d angular_momentum; + + Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + + CHECK_EQUAL (Vector3d (0., 0., 0.), angular_momentum); + + qdot[0] = 1.; + qdot[1] = 2.; + qdot[2] = 3.; + + Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + + // only a rough guess from test calculation + CHECK_ARRAY_CLOSE (Vector3d (3.3, 2.54, 1.5).data(), angular_momentum.data(), 3, 1.0e-1); + + qdot[3] = -qdot[0]; + qdot[4] = -qdot[1]; + qdot[5] = -qdot[2]; + + ClearLogOutput(); + Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum); + + CHECK (angular_momentum[0] == 0); + CHECK (angular_momentum[1] < 0); + CHECK (angular_momentum[2] == 0.); +} + +template +void TestCoMComputation ( + T &obj +) { + + VectorNd Q = VectorNd::Random (obj.model->dof_count); + VectorNd QDot = VectorNd::Random (obj.model->dof_count); + VectorNd QDDot = VectorNd::Random (obj.model->dof_count); + + // compute quantities directly from model + double mass_expected = 0.0; + + UpdateKinematicsCustom(*obj.model, &Q, NULL, NULL); + for (unsigned int i = 1; i < obj.model->mBodies.size(); i++) { + // mass_expected += obj.model->I[i].m; + mass_expected += obj.model->mBodies[i].mMass; + } + + double mass_actual = 0.0; + Vector3d com = Vector3d::Zero(); + Utils::CalcCenterOfMass ( + *obj.model, + Q, QDot, NULL, + mass_actual, com, + NULL, NULL + ); + + CHECK_CLOSE (mass_expected, mass_actual, 1e-7); + + return; +} + +TEST_FIXTURE( + LinearInvertedPendulumModel, + TestCoMComputationLinearInvertedPendulumModel +) { + TestCoMComputation (*this); +} + +TEST_FIXTURE( + FixedJoint2DoF, + TestCoMComputationFixedJoint2DoF +) { + TestCoMComputation (*this); +} + +TEST_FIXTURE( + FixedBase6DoF12DoFFloatingBase, + TestCoMComputationFixedBase6DoF12DoFFloatingBase +) { + TestCoMComputation (*this); +} + +TEST_FIXTURE( + Human36, + TestCoMComputationHuman36 +) { + TestCoMComputation (*this); +} + +template +void TestCoMAccelerationUsingFD ( + T & obj, + const double TOL = 1e-7 +) { + const double EPS = 1e-7; + + obj.Q = VectorNd::Random (obj.model->dof_count); + obj.QDot = VectorNd::Random (obj.model->dof_count); + obj.QDDot = VectorNd::Random (obj.model->dof_count); + + double mass = 0.0; + Vector3d com (Vector3d::Zero()); + Vector3d com_vec (Vector3d::Zero()); + Vector3d ang_mom (Vector3d::Zero()); + + Vector3d com_acc_nom (Vector3d::Zero()); + Vector3d com_acc_fd (Vector3d::Zero()); + + Vector3d ch_ang_mom_nom (Vector3d::Zero()); + Vector3d ch_ang_mom_fd (Vector3d::Zero()); + + // compute com acceleration nominal + Utils::CalcCenterOfMass ( + *obj.model, + obj.Q, obj.QDot, &obj.QDDot, + mass, + com, &com_vec, &com_acc_nom, + &ang_mom, &ch_ang_mom_nom + ); + + // compute com acceleration using finite differences from velocity + Utils::CalcCenterOfMass ( + *obj.model, + obj.Q + EPS*obj.QDot, + obj.QDot + EPS*obj.QDDot, + NULL, + mass, com, &com_acc_fd, NULL, + &ch_ang_mom_fd + ); + + com_acc_fd = (com_acc_fd - com_vec) / EPS; + ch_ang_mom_fd = (ch_ang_mom_fd - ang_mom) / EPS; + + // check CoM acceleration + CHECK_ARRAY_CLOSE (com_acc_nom.data(), com_acc_fd.data(), 3, TOL); + CHECK_ARRAY_CLOSE (ch_ang_mom_nom.data(), ch_ang_mom_fd.data(), 3, TOL); + + return; +} + +TEST_FIXTURE( + LinearInvertedPendulumModel, + TestCoMAccelerationUsingFDLinearInvertedPendulumModel +) { + TestCoMAccelerationUsingFD (*this, 1e-8); +} + +TEST_FIXTURE( + FixedJoint2DoF, + TestCoMAccelerationUsingFDFixedJoint2DoF +) { + TestCoMAccelerationUsingFD (*this, 1e-7); +} + +TEST_FIXTURE( + FixedBase6DoF12DoFFloatingBase, + TestCoMAccelerationUsingFDFixedBase6DoF12DoFFloatingBase +) { + TestCoMAccelerationUsingFD (*this, 1e-6); +} + +template +void TestZMPComputationForNotMovingSystem( + T & obj, + const double TOL = 1e-8 +) { + // Test ZMP against CoM projection for non-moving system (qdot, qddot = 0) + // for this configurations CoM and ZMP coincide + + obj.Q = VectorNd::Random (obj.model->dof_count); + obj.QDot = VectorNd::Zero (obj.model->dof_count); + obj.QDDot = VectorNd::Zero (obj.model->dof_count); + + Vector3d zmp (Vector3d::Zero()); + Utils::CalcZeroMomentPoint ( + *obj.model, + obj.Q, obj.QDot, obj.QDDot, + &zmp, + obj.contact_normal, obj.contact_point + ); + + double mass = 0.0; + Vector3d com (Vector3d::Zero()); + Utils::CalcCenterOfMass ( + *obj.model, + obj.Q, obj.QDot, NULL, + mass, com, NULL, NULL + ); + + // project CoM onto surface + double distance = (com - obj.contact_point).dot(obj.contact_normal); + com = com - distance * obj.contact_normal; + + // check ZMP against CoM + CHECK_ARRAY_CLOSE (com.data(), zmp.data(), 3, TOL); + + return; +} + +TEST_FIXTURE( + LinearInvertedPendulumModel, + TestZMPComputationForNotMovingSystemLinearInvertedPendulumModel +) { + TestZMPComputationForNotMovingSystem (*this, 1e-8); +} + +template +void TestZMPComputationAgainstTableCartModel( + T & obj, + const double TOL = 1e-8 +) { + obj.Q = VectorNd::Random (obj.model->dof_count); + obj.QDot = VectorNd::Random (obj.model->dof_count); + obj.QDDot = VectorNd::Random (obj.model->dof_count); + + Vector3d zmp (Vector3d::Zero()); + Utils::CalcZeroMomentPoint ( + *obj.model, + obj.Q, obj.QDot, obj.QDDot, + &zmp, + obj.contact_normal, obj.contact_point + ); + + double mass = 0.0; + Vector3d com (Vector3d::Zero()); + Utils::CalcCenterOfMass ( + *obj.model, + obj.Q, obj.QDot, NULL, + mass, com, NULL, NULL + ); + + com.set( + obj.Q[0] - com[2]/obj.model->gravity.norm()*obj.QDDot[0], + obj.Q[1] - com[2]/obj.model->gravity.norm()*obj.QDDot[1], + 0. + ); + + // check ZMP against CoM + CHECK_ARRAY_CLOSE (com.data(), zmp.data(), 3, TOL); + + return; +} + +TEST_FIXTURE( + LinearInvertedPendulumModel, + TestZMPComputationAgainstTableCartModelLinearInvertedPendulumModel +) { + TestZMPComputationAgainstTableCartModel (*this, 1e-8); +} diff --git a/3rdparty/rbdl/tests/main.cc b/3rdparty/rbdl/tests/main.cc new file mode 100644 index 0000000..4608c22 --- /dev/null +++ b/3rdparty/rbdl/tests/main.cc @@ -0,0 +1,19 @@ +#include +#include +#include + +#include + +int main (int argc, char *argv[]) +{ + rbdl_check_api_version (RBDL_API_VERSION); + + if (argc > 1) { + std::string arg (argv[1]); + + if (arg == "-v" || arg == "--version") + rbdl_print_version(); + } + + return UnitTest::RunAllTests (); +} diff --git a/3rdparty/rbdl/utils/matlab/FrameTranslation.m b/3rdparty/rbdl/utils/matlab/FrameTranslation.m new file mode 100755 index 0000000..c95b7cf --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/FrameTranslation.m @@ -0,0 +1,30 @@ +function [F] = FrameTranslation (translation, euler_angles) + +% Here we store the result +Result = zeros (4,4); +Result(4,4) = 1; + +% Set the translation part: +Result (1:3, 4) = -translation; + +% Calculate the rotations +Rotation = eye (3,3); + +% Z Rotation +RotZ = eye (3,3); + +if (euler_angles(1) != 0.) + s = sin (euler_angles(1)); + c = cos (euler_angles(1)); + RotZ = [c, s, 0; + -s, c, 0; + 0, 0, 1]; +end + +Rotation = RotZ; + +Result (1:3, 1:3) = Rotation; + +F = Result; + +end; diff --git a/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m b/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m new file mode 100644 index 0000000..399c8c6 --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/VectorCrossMatrix.m @@ -0,0 +1,5 @@ +function R = VectorCrossMatrix (r) + +R = [0, -r(3), r(2); + r(3), 0, -r(1); + -r(2), r(1), 0]; diff --git a/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m b/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m new file mode 100644 index 0000000..b7870fd --- /dev/null +++ b/3rdparty/rbdl/utils/matlab/ZYXEulerToMatrix.m @@ -0,0 +1,56 @@ +function [Rx, Ry, Rz, M] = ZYXEulerToMatrix (e) +% +% Calculates the rotation matrix from ZYX Euler Angles +% + +z = e(1); +y = e(2); +x = e(3); + +sz = sin (z); +cz = cos (z); +sy = sin (y); +cy = cos (y); +sx = sin (x); +cx = cos (x); + +Rx = [ +1, 0, 0; +0, cx, -sx; +0, sx, cx; +]; + +Ry = [ + cy, 0, sy; + 0, 1, 0; +-sy, 0, cy +]; + +Rz = [ +cz, -sz, 0; +sz, cz, 0; + 0, 0, 1; +]; + +% Rx * Ry * Rz +M = [ + cy * cz, - cy * sz, +sy; +sz*cx + sx*sy*cz, cz*cx - sx*sy*sz, -sx*cy; +sz*sx - cx*sy*cz, cz*sx + cx*sy*sz, cx*cy; +]; + +% Rz' * Ry' * Rx' +M = [ + cz * cy, -cz*sy*sx + sz* cx, cz*sy*cx + sz*sx; +-sz*cy, - sz*sy*sx + cz*cx, sz*sy*cx + cz*sx; +sy, -cy*sx, cy*cx; +]; + +% Rx' * Ry' * Rz' +M = [ +cy*cz, cy*sz, -sy; +-cx*sz+sx*sy*cz, cx*cz+sx*sy*sz, sx*cy; +sx*sz+cx*sy*cz, -sx*cz+cx*sy*sz, cx*cy; +]; + +M = Rz * Ry * Rx; diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f63d39d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required (VERSION 3.15) + +cmake_policy(SET CMP0077 NEW) + +project (rbdlsim + VERSION 0.0.1 + LANGUAGES CXX) + +set(ENABLE_DOUBLE_PRECISION On) +set(RBDL_USE_SIMPLE_MATH On) + +add_subdirectory (3rdparty/rbdl) +add_subdirectory (3rdparty/libccd) + +# Library +add_library(${PROJECT_NAME}) +target_include_directories( + ${PROJECT_NAME} + PUBLIC $ + PUBLIC $ + PUBLIC $) +target_link_libraries(${PROJECT_NAME} rbdl ccd) +target_sources(${PROJECT_NAME} PRIVATE src/rbdlsim.cc) + +# Simulator Executable +add_executable(runsim) +target_include_directories( + runsim + PUBLIC $ + PUBLIC $ + PUBLIC $) + +target_link_libraries(runsim ${PROJECT_NAME}) + +target_sources(runsim PRIVATE src/main.cc) + +# Tests +add_executable(runtests) + +target_sources(runtests PRIVATE tests/runtests.cc tests/CollisionTests.cc) +target_include_directories( + runtests + PUBLIC $ + PUBLIC $ + PUBLIC $) + +target_link_libraries(runtests ${PROJECT_NAME}) diff --git a/include/rbdlsim.h b/include/rbdlsim.h new file mode 100644 index 0000000..f38eae8 --- /dev/null +++ b/include/rbdlsim.h @@ -0,0 +1,75 @@ +#ifndef RBDLSIM_RBDLSIM_H +#define RBDLSIM_RBDLSIM_H + +#include +#include + +#include + +namespace RBDLSim { + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +struct SimShape { + enum ShapeType { + Box = 0, + Sphere = 1, + }; + ShapeType mType; + Vector3d pos; + Quaternion orientation; + Vector3d scale; +}; + +struct SimBody { + VectorNd q, qdot, qddot, tau; + Model mModel; + + typedef std::pair BodyCollisionInfo; + std::vector mCollisionShapes; + + void step(double ts); + void updateCollisionShapes(); +}; + +struct CollisionInfo { + const SimBody* mBodyA; + const SimBody* mBodyB; + const SimShape* mShapeA; + const SimShape* mShapeB; + Vector3d pos; + Vector3d dir; +}; + +struct World { + double mSimTime = 0.; + std::vector mBodies; + std::vector mStaticShapes; + std::vector mContactPoints; + + void updateCollisionShapes(); + void detectCollisions(); + bool step(double dt); +}; + +bool CheckPenetration( + const SimShape& shape_a, + const SimShape& shape_b, + CollisionInfo& cinfo); + +SimBody CreateSphereBody( + double mass, + double radius, + const Vector3d& pos, + const Vector3d& vel); + +SimBody CreateBoxBody( + double mass, + const Vector3d& size, + const Vector3d& pos, + const Vector3d& vel); + +} // namespace RBDLSim + +#endif //RBDLSIM_RBDLSIM_H diff --git a/src/main.cc b/src/main.cc new file mode 100644 index 0000000..59dfdb1 --- /dev/null +++ b/src/main.cc @@ -0,0 +1,39 @@ +#include + +#include "rbdlsim.h" + +using namespace std; +using namespace RBDLSim; + +void simplesim() { + World world; + SimBody sphere_body = + CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero()); + sphere_body.qdot[3] = 1 * M_PI; + world.mBodies.push_back(sphere_body); + + SimShape ground_shape; + ground_shape.mType = SimShape::Box; + ground_shape.pos.set(0., -5.0, 0.); + ground_shape.orientation.set(0., 0., 0., 1.); + ground_shape.scale.set(25.0, 10.0, 25.0); + + world.mStaticShapes.push_back(ground_shape); + + world.mSimTime = 0.; + cout << world.mBodies[0].q.transpose() << endl; + + do { + world.updateCollisionShapes(); + world.detectCollisions(); + world.step(1.0e-3); + } while (world.mContactPoints.size() == 0); + + cout << "Collision at t = " << world.mSimTime << endl + << "q = " << world.mBodies[0].q.transpose() << endl; +} + +int main(int argc, char* argv[]) { + simplesim(); + return 0; +} diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc new file mode 100644 index 0000000..693220b --- /dev/null +++ b/src/rbdlsim.cc @@ -0,0 +1,259 @@ +#include "rbdlsim.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; + +namespace RBDLSim { + +void SimBody::step(double dt) { + ForwardDynamics(mModel, q, qdot, tau, qddot); + + // semi-implicit eulers + qdot += dt * qddot; + + for (int i = 1; i < mModel.mJoints.size(); ++i) { + const Joint& joint = mModel.mJoints[i]; + if (joint.mJointType != JointTypeSpherical) { + q.block(joint.q_index, 0, joint.mDoFCount, 1) += + dt * qdot.block(joint.q_index, 0, joint.mDoFCount, 1); + continue; + } + + // Integrate the Quaternion + Quaternion q0 = mModel.GetQuaternion(i, q); + Vector3d omega(qdot.block(joint.q_index, 0, 3, 1)); + Quaternion qd = q0.omegaToQDot(omega); + Quaternion q1 = (q0 + qd * dt).normalize(); + mModel.SetQuaternion(i, q1, q); + } +} + +/** Support function for box */ +void SimShapeSupport( + const void* _shape, + const ccd_vec3_t* _dir, + ccd_vec3_t* v) { + SimShape* shape = (SimShape*)_shape; + + CCD_VEC3(pos, shape->pos[0], shape->pos[1], shape->pos[2]); + CCD_QUAT( + quat, + shape->orientation[0], + shape->orientation[1], + shape->orientation[2], + shape->orientation[3]); + ccd_vec3_t dir; + ccd_quat_t qinv; + + // apply rotation on direction vector + ccdVec3Copy(&dir, _dir); + ccdQuatInvert2(&qinv, &quat); + ccdQuatRotVec(&dir, &qinv); + + // compute support point in specified direction + if (shape->mType == SimShape::Box) { + ccdVec3Set( + v, + ccdSign(ccdVec3X(&dir)) * shape->scale[0] * CCD_REAL(0.5), + ccdSign(ccdVec3Y(&dir)) * shape->scale[1] * CCD_REAL(0.5), + ccdSign(ccdVec3Z(&dir)) * shape->scale[2] * CCD_REAL(0.5)); + + } else if (shape->mType == SimShape::Sphere) { + ccd_real_t len; + assert((shape->scale[0] - shape->scale[1]) < 1.0e-12); + assert((shape->scale[2] - shape->scale[1]) < 1.0e-12); + + len = ccdVec3Len2(&dir); + if (len - CCD_EPS > CCD_ZERO) { + ccdVec3Copy(v, &dir); + ccdVec3Scale(v, shape->scale[0] / CCD_SQRT(len)); + } else { + ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO); + } + } + + // transform support point according to position and rotation of object + ccdQuatRotVec(v, &quat); + ccdVec3Add(v, &pos); +} + +bool CheckPenetration( + const SimShape& shape_a, + const SimShape& shape_b, + CollisionInfo& cinfo) { + ccd_t ccd; + CCD_INIT(&ccd); + ccd.support1 = SimShapeSupport; + ccd.support2 = SimShapeSupport; + ccd.max_iterations = 100; + ccd.epa_tolerance = 0.0001; + + ccd_real_t depth; + ccd_vec3_t dir, pos; + int intersect = + ccdGJKPenetration(&shape_a, &shape_b, &ccd, &depth, &dir, &pos); + + if (intersect == 0) { + cinfo.pos.set(pos.v[0], pos.v[1], pos.v[2]); + cinfo.dir.set(dir.v[0], dir.v[1], dir.v[2]); + } + + return !intersect; +} + +void SimBody::updateCollisionShapes() { + UpdateKinematicsCustom(mModel, &q, nullptr, nullptr); + + for (SimBody::BodyCollisionInfo& body_col_info : mCollisionShapes) { + const unsigned int& body_id = body_col_info.first; + SimShape& shape = body_col_info.second; + + shape.pos = + CalcBodyToBaseCoordinates(mModel, q, body_id, Vector3d::Zero(), false); + shape.orientation.fromMatrix( + CalcBodyWorldOrientation(mModel, q, body_id, false)); + } +} + +void World::updateCollisionShapes() { + for (SimBody& body : mBodies) { + body.updateCollisionShapes(); + } +} + +void World::detectCollisions() { + mContactPoints.clear(); + + for (SimBody& body : mBodies) { + for (SimBody::BodyCollisionInfo& body_col_info : body.mCollisionShapes) { + SimShape& body_shape = body_col_info.second; + + // check against all static shapes + for (SimShape& static_shape : mStaticShapes) { + bool has_penetration = false; + CollisionInfo cinfo; + + has_penetration = CheckPenetration(static_shape, body_shape, cinfo); + + if (has_penetration) { + cinfo.mBodyA = nullptr; + cinfo.mBodyB = &body; + mContactPoints.push_back(cinfo); + } + } + } + } +} + +bool World::step(double dt) { + for (SimBody& body : mBodies) { + body.step(dt); + } + + mSimTime += dt; + return true; +} + +SimBody CreateSphereBody( + double mass, + double radius, + const Vector3d& pos, + const Vector3d& vel) { + SimBody result; + + Joint joint_trans_xyz(JointTypeTranslationXYZ); + Joint joint_rot_quat(JointTypeSpherical); + Body nullbody(0., Vector3d(0., 0., 0.), Matrix3d::Zero()); + Body body( + mass, + Vector3d(0., 0., 0.), + Matrix3d::Identity() * 2. / 5. * mass * radius * radius); + + result.mModel.AppendBody( + Xtrans(Vector3d(0., 0., 0.)), + joint_trans_xyz, + nullbody); + unsigned int sphere_body = result.mModel.AppendBody( + Xtrans(Vector3d(0., 0., 0.)), + joint_rot_quat, + body); + + result.q = VectorNd::Zero(result.mModel.q_size); + result.q.block(0, 0, 3, 1) = pos; + result.mModel.SetQuaternion( + sphere_body, + Quaternion(0., 0., 0., 1.), + result.q); + result.qdot = VectorNd::Zero(result.mModel.qdot_size); + result.qddot = VectorNd::Zero(result.mModel.qdot_size); + result.tau = VectorNd::Zero(result.mModel.qdot_size); + + SimShape shape; + shape.mType = SimShape::Sphere; + shape.pos = pos; + shape.orientation.set(0., 0., 0., 1.); + shape.scale.set(radius, radius, radius); + + result.mCollisionShapes.push_back( + SimBody::BodyCollisionInfo(sphere_body, shape)); + + return result; +} + +SimBody CreateBoxBody( + double mass, + const Vector3d& size, + const Vector3d& pos, + const Vector3d& vel) { + SimBody result; + + Joint joint_trans_xyz(JointTypeTranslationXYZ); + Joint joint_rot_quat(JointTypeSpherical); + Body nullbody(0., Vector3d(0., 0., 0.), Matrix3d::Zero()); + Matrix3d inertia(Matrix3d::Zero()); + inertia(0, 0) = (1. / 12.) * mass * (size[1] * size[1] + size[2] * size[2]); + inertia(1, 1) = (1. / 12.) * mass * (size[0] * size[0] + size[2] * size[2]); + inertia(2, 2) = (1. / 12.) * mass * (size[1] * size[1] + size[0] * size[0]); + Body body(mass, Vector3d(0., 0., 0.), inertia); + + result.mModel.AppendBody( + Xtrans(Vector3d(0., 0., 0.)), + joint_trans_xyz, + nullbody); + unsigned int sphere_body = result.mModel.AppendBody( + Xtrans(Vector3d(0., 0., 0.)), + joint_rot_quat, + body); + + result.q = VectorNd::Zero(result.mModel.q_size); + result.q.block(0, 0, 3, 1) = pos; + result.mModel.SetQuaternion( + sphere_body, + Quaternion(0., 0., 0., 1.), + result.q); + result.qdot = VectorNd::Zero(result.mModel.qdot_size); + result.qddot = VectorNd::Zero(result.mModel.qdot_size); + result.tau = VectorNd::Zero(result.mModel.qdot_size); + + SimShape shape; + shape.mType = SimShape::Sphere; + shape.pos = pos; + shape.orientation.set(0., 0., 0., 1.); + shape.scale.set(size[0], size[1], size[2]); + + result.mCollisionShapes.push_back( + SimBody::BodyCollisionInfo(sphere_body, shape)); + + return result; +} + +} // namespace RBDLSim \ No newline at end of file diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc new file mode 100644 index 0000000..dd6740b --- /dev/null +++ b/tests/CollisionTests.cc @@ -0,0 +1,46 @@ +#include + +#include "catch.hpp" +#include "rbdlsim.h" + +using namespace std; + +using namespace RBDLSim; + +TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") { + SimShape box; + box.mType = SimShape::Box; + box.pos.set(0.0, 0.5, 0.); + box.scale.set(1., 1., 1.); + box.orientation.set(0., 0., 0., 1.); + + SimShape sphere; + sphere.mType = SimShape::Sphere; + + sphere.scale.set(0.5, 0.5, 0.5); + sphere.orientation.set(0., 0., 0., 1.); + + bool cresult = false; + CollisionInfo cinfo; + + SECTION("Box and Sphere Touching") { + sphere.pos.set(0., 1.5, 0.); + cresult = CheckPenetration(box, sphere, cinfo); + + REQUIRE(cresult == true); + } + + SECTION("Box and Sphere Intersecting") { + sphere.pos.set(0., 1.4, 0.); + cresult = CheckPenetration(box, sphere, cinfo); + + REQUIRE(cresult == true); + } + + SECTION("Box and Sphere Separated") { + sphere.pos.set(0., 1.5001, 0.); + cresult = CheckPenetration(box, sphere, cinfo); + + REQUIRE(cresult == false); + } +} diff --git a/tests/catch.hpp b/tests/catch.hpp new file mode 100644 index 0000000..94e20c2 --- /dev/null +++ b/tests/catch.hpp @@ -0,0 +1,17802 @@ +/* + * Catch v2.13.1 + * Generated: 2020-09-07 12:12:38.090364 + * ---------------------------------------------------------- + * This file has been merged from multiple headers. Please don't edit it directly + * Copyright (c) 2020 Two Blue Cubes Ltd. All rights reserved. + * + * Distributed under the Boost Software License, Version 1.0. (See accompanying + * file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) + */ +#ifndef TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +#define TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +// start catch.hpp + + +#define CATCH_VERSION_MAJOR 2 +#define CATCH_VERSION_MINOR 13 +#define CATCH_VERSION_PATCH 1 + +#ifdef __clang__ +# pragma clang system_header +#elif defined __GNUC__ +# pragma GCC system_header +#endif + +// start catch_suppress_warnings.h + +#ifdef __clang__ +# ifdef __ICC // icpc defines the __clang__ macro +# pragma warning(push) +# pragma warning(disable: 161 1682) +# else // __ICC +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wpadded" +# pragma clang diagnostic ignored "-Wswitch-enum" +# pragma clang diagnostic ignored "-Wcovered-switch-default" +# endif +#elif defined __GNUC__ + // Because REQUIREs trigger GCC's -Wparentheses, and because still + // supported version of g++ have only buggy support for _Pragmas, + // Wparentheses have to be suppressed globally. +# pragma GCC diagnostic ignored "-Wparentheses" // See #674 for details + +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-variable" +# pragma GCC diagnostic ignored "-Wpadded" +#endif +// end catch_suppress_warnings.h +#if defined(CATCH_CONFIG_MAIN) || defined(CATCH_CONFIG_RUNNER) +# define CATCH_IMPL +# define CATCH_CONFIG_ALL_PARTS +#endif + +// In the impl file, we want to have access to all parts of the headers +// Can also be used to sanely support PCHs +#if defined(CATCH_CONFIG_ALL_PARTS) +# define CATCH_CONFIG_EXTERNAL_INTERFACES +# if defined(CATCH_CONFIG_DISABLE_MATCHERS) +# undef CATCH_CONFIG_DISABLE_MATCHERS +# endif +# if !defined(CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER) +# define CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER +# endif +#endif + +#if !defined(CATCH_CONFIG_IMPL_ONLY) +// start catch_platform.h + +#ifdef __APPLE__ +# include +# if TARGET_OS_OSX == 1 +# define CATCH_PLATFORM_MAC +# elif TARGET_OS_IPHONE == 1 +# define CATCH_PLATFORM_IPHONE +# endif + +#elif defined(linux) || defined(__linux) || defined(__linux__) +# define CATCH_PLATFORM_LINUX + +#elif defined(WIN32) || defined(__WIN32__) || defined(_WIN32) || defined(_MSC_VER) || defined(__MINGW32__) +# define CATCH_PLATFORM_WINDOWS +#endif + +// end catch_platform.h + +#ifdef CATCH_IMPL +# ifndef CLARA_CONFIG_MAIN +# define CLARA_CONFIG_MAIN_NOT_DEFINED +# define CLARA_CONFIG_MAIN +# endif +#endif + +// start catch_user_interfaces.h + +namespace Catch { + unsigned int rngSeed(); +} + +// end catch_user_interfaces.h +// start catch_tag_alias_autoregistrar.h + +// start catch_common.h + +// start catch_compiler_capabilities.h + +// Detect a number of compiler features - by compiler +// The following features are defined: +// +// CATCH_CONFIG_COUNTER : is the __COUNTER__ macro supported? +// CATCH_CONFIG_WINDOWS_SEH : is Windows SEH supported? +// CATCH_CONFIG_POSIX_SIGNALS : are POSIX signals supported? +// CATCH_CONFIG_DISABLE_EXCEPTIONS : Are exceptions enabled? +// **************** +// Note to maintainers: if new toggles are added please document them +// in configuration.md, too +// **************** + +// In general each macro has a _NO_ form +// (e.g. CATCH_CONFIG_NO_POSIX_SIGNALS) which disables the feature. +// Many features, at point of detection, define an _INTERNAL_ macro, so they +// can be combined, en-mass, with the _NO_ forms later. + +#ifdef __cplusplus + +# if (__cplusplus >= 201402L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201402L) +# define CATCH_CPP14_OR_GREATER +# endif + +# if (__cplusplus >= 201703L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +# define CATCH_CPP17_OR_GREATER +# endif + +#endif + +#if defined(__cpp_lib_uncaught_exceptions) +# define CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS +#endif + +// We have to avoid both ICC and Clang, because they try to mask themselves +// as gcc, and we want only GCC in this block +#if defined(__GNUC__) && !defined(__clang__) && !defined(__ICC) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic pop" ) + +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) + +#endif + +#if defined(__clang__) + +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic pop" ) + +// As of this writing, IBM XL's implementation of __builtin_constant_p has a bug +// which results in calls to destructors being emitted for each temporary, +// without a matching initialization. In practice, this can result in something +// like `std::string::~string` being called on an uninitialized value. +// +// For example, this code will likely segfault under IBM XL: +// ``` +// REQUIRE(std::string("12") + "34" == "1234") +// ``` +// +// Therefore, `CATCH_INTERNAL_IGNORE_BUT_WARN` is not implemented. +# if !defined(__ibmxl__) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) /* NOLINT(cppcoreguidelines-pro-type-vararg, hicpp-vararg) */ +# endif + +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wexit-time-destructors\"" ) \ + _Pragma( "clang diagnostic ignored \"-Wglobal-constructors\"") + +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wparentheses\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-variable\"" ) + +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wgnu-zero-variadic-macro-arguments\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-template\"" ) + +#endif // __clang__ + +//////////////////////////////////////////////////////////////////////////////// +// Assume that non-Windows platforms support posix signals by default +#if !defined(CATCH_PLATFORM_WINDOWS) + #define CATCH_INTERNAL_CONFIG_POSIX_SIGNALS +#endif + +//////////////////////////////////////////////////////////////////////////////// +// We know some environments not to support full POSIX signals +#if defined(__CYGWIN__) || defined(__QNX__) || defined(__EMSCRIPTEN__) || defined(__DJGPP__) + #define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +#endif + +#ifdef __OS400__ +# define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +# define CATCH_CONFIG_COLOUR_NONE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Android somehow still does not support std::to_string +#if defined(__ANDROID__) +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING +# define CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Not all Windows environments support SEH properly +#if defined(__MINGW32__) +# define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH +#endif + +//////////////////////////////////////////////////////////////////////////////// +// PS4 +#if defined(__ORBIS__) +# define CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Cygwin +#ifdef __CYGWIN__ + +// Required for some versions of Cygwin to declare gettimeofday +// see: http://stackoverflow.com/questions/36901803/gettimeofday-not-declared-in-this-scope-cygwin +# define _BSD_SOURCE +// some versions of cygwin (most) do not support std::to_string. Use the libstd check. +// https://gcc.gnu.org/onlinedocs/gcc-4.8.2/libstdc++/api/a01053_source.html line 2812-2813 +# if !((__cplusplus >= 201103L) && defined(_GLIBCXX_USE_C99) \ + && !defined(_GLIBCXX_HAVE_BROKEN_VSWPRINTF)) + +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING + +# endif +#endif // __CYGWIN__ + +//////////////////////////////////////////////////////////////////////////////// +// Visual C++ +#if defined(_MSC_VER) + +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION __pragma( warning(push) ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION __pragma( warning(pop) ) + +# if _MSC_VER >= 1900 // Visual Studio 2015 or newer +# define CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS +# endif + +// Universal Windows platform does not support SEH +// Or console colours (or console at all...) +# if defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_APP) +# define CATCH_CONFIG_COLOUR_NONE +# else +# define CATCH_INTERNAL_CONFIG_WINDOWS_SEH +# endif + +// MSVC traditional preprocessor needs some workaround for __VA_ARGS__ +// _MSVC_TRADITIONAL == 0 means new conformant preprocessor +// _MSVC_TRADITIONAL == 1 means old traditional non-conformant preprocessor +# if !defined(__clang__) // Handle Clang masquerading for msvc +# if !defined(_MSVC_TRADITIONAL) || (defined(_MSVC_TRADITIONAL) && _MSVC_TRADITIONAL) +# define CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +# endif // MSVC_TRADITIONAL +# endif // __clang__ + +#endif // _MSC_VER + +#if defined(_REENTRANT) || defined(_MSC_VER) +// Enable async processing, as -pthread is specified or no additional linking is required +# define CATCH_INTERNAL_CONFIG_USE_ASYNC +#endif // _MSC_VER + +//////////////////////////////////////////////////////////////////////////////// +// Check if we are compiled with -fno-exceptions or equivalent +#if defined(__EXCEPTIONS) || defined(__cpp_exceptions) || defined(_CPPUNWIND) +# define CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED +#endif + +//////////////////////////////////////////////////////////////////////////////// +// DJGPP +#ifdef __DJGPP__ +# define CATCH_INTERNAL_CONFIG_NO_WCHAR +#endif // __DJGPP__ + +//////////////////////////////////////////////////////////////////////////////// +// Embarcadero C++Build +#if defined(__BORLANDC__) + #define CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// Use of __COUNTER__ is suppressed during code analysis in +// CLion/AppCode 2017.2.x and former, because __COUNTER__ is not properly +// handled by it. +// Otherwise all supported compilers support COUNTER macro, +// but user still might want to turn it off +#if ( !defined(__JETBRAINS_IDE__) || __JETBRAINS_IDE__ >= 20170300L ) + #define CATCH_INTERNAL_CONFIG_COUNTER +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// RTX is a special version of Windows that is real time. +// This means that it is detected as Windows, but does not provide +// the same set of capabilities as real Windows does. +#if defined(UNDER_RTSS) || defined(RTX64_BUILD) + #define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH + #define CATCH_INTERNAL_CONFIG_NO_ASYNC + #define CATCH_CONFIG_COLOUR_NONE +#endif + +#if !defined(_GLIBCXX_USE_C99_MATH_TR1) +#define CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Various stdlib support checks that require __has_include +#if defined(__has_include) + // Check if string_view is available and usable + #if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW + #endif + + // Check if optional is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if byte is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # include + # if __cpp_lib_byte > 0 + # define CATCH_INTERNAL_CONFIG_CPP17_BYTE + # endif + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if variant is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # if defined(__clang__) && (__clang_major__ < 8) + // work around clang bug with libstdc++ https://bugs.llvm.org/show_bug.cgi?id=31852 + // fix should be in clang 8, workaround in libstdc++ 8.2 + # include + # if defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # define CATCH_CONFIG_NO_CPP17_VARIANT + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__clang__) && (__clang_major__ < 8) + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) +#endif // defined(__has_include) + +#if defined(CATCH_INTERNAL_CONFIG_COUNTER) && !defined(CATCH_CONFIG_NO_COUNTER) && !defined(CATCH_CONFIG_COUNTER) +# define CATCH_CONFIG_COUNTER +#endif +#if defined(CATCH_INTERNAL_CONFIG_WINDOWS_SEH) && !defined(CATCH_CONFIG_NO_WINDOWS_SEH) && !defined(CATCH_CONFIG_WINDOWS_SEH) && !defined(CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH) +# define CATCH_CONFIG_WINDOWS_SEH +#endif +// This is set by default, because we assume that unix compilers are posix-signal-compatible by default. +#if defined(CATCH_INTERNAL_CONFIG_POSIX_SIGNALS) && !defined(CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_POSIX_SIGNALS) +# define CATCH_CONFIG_POSIX_SIGNALS +#endif +// This is set by default, because we assume that compilers with no wchar_t support are just rare exceptions. +#if !defined(CATCH_INTERNAL_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_WCHAR) +# define CATCH_CONFIG_WCHAR +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_CPP11_TO_STRING) +# define CATCH_CONFIG_CPP11_TO_STRING +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_NO_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_CPP17_OPTIONAL) +# define CATCH_CONFIG_CPP17_OPTIONAL +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS) && !defined(CATCH_CONFIG_NO_CPP17_UNCAUGHT_EXCEPTIONS) && !defined(CATCH_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS) +# define CATCH_CONFIG_CPP17_UNCAUGHT_EXCEPTIONS +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_NO_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_CPP17_STRING_VIEW) +# define CATCH_CONFIG_CPP17_STRING_VIEW +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_VARIANT) && !defined(CATCH_CONFIG_NO_CPP17_VARIANT) && !defined(CATCH_CONFIG_CPP17_VARIANT) +# define CATCH_CONFIG_CPP17_VARIANT +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_BYTE) && !defined(CATCH_CONFIG_NO_CPP17_BYTE) && !defined(CATCH_CONFIG_CPP17_BYTE) +# define CATCH_CONFIG_CPP17_BYTE +#endif + +#if defined(CATCH_CONFIG_EXPERIMENTAL_REDIRECT) +# define CATCH_INTERNAL_CONFIG_NEW_CAPTURE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_NEW_CAPTURE) && !defined(CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NEW_CAPTURE) +# define CATCH_CONFIG_NEW_CAPTURE +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED) && !defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +# define CATCH_CONFIG_DISABLE_EXCEPTIONS +#endif + +#if defined(CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_NO_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_POLYFILL_ISNAN) +# define CATCH_CONFIG_POLYFILL_ISNAN +#endif + +#if defined(CATCH_INTERNAL_CONFIG_USE_ASYNC) && !defined(CATCH_INTERNAL_CONFIG_NO_ASYNC) && !defined(CATCH_CONFIG_NO_USE_ASYNC) && !defined(CATCH_CONFIG_USE_ASYNC) +# define CATCH_CONFIG_USE_ASYNC +#endif + +#if defined(CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_NO_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_ANDROID_LOGWRITE) +# define CATCH_CONFIG_ANDROID_LOGWRITE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_NO_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_GLOBAL_NEXTAFTER) +# define CATCH_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Even if we do not think the compiler has that warning, we still have +// to provide a macro that can be used by the code. +#if !defined(CATCH_INTERNAL_START_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS +#endif + +// The goal of this macro is to avoid evaluation of the arguments, but +// still have the compiler warn on problems inside... +#if !defined(CATCH_INTERNAL_IGNORE_BUT_WARN) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) +#endif + +#if defined(__APPLE__) && defined(__apple_build_version__) && (__clang_major__ < 10) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#elif defined(__clang__) && (__clang_major__ < 5) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +#define CATCH_TRY if ((true)) +#define CATCH_CATCH_ALL if ((false)) +#define CATCH_CATCH_ANON(type) if ((false)) +#else +#define CATCH_TRY try +#define CATCH_CATCH_ALL catch (...) +#define CATCH_CATCH_ANON(type) catch (type) +#endif + +#if defined(CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_NO_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) +#define CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#endif + +// end catch_compiler_capabilities.h +#define INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) name##line +#define INTERNAL_CATCH_UNIQUE_NAME_LINE( name, line ) INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) +#ifdef CATCH_CONFIG_COUNTER +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __COUNTER__ ) +#else +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __LINE__ ) +#endif + +#include +#include +#include + +// We need a dummy global operator<< so we can bring it into Catch namespace later +struct Catch_global_namespace_dummy {}; +std::ostream& operator<<(std::ostream&, Catch_global_namespace_dummy); + +namespace Catch { + + struct CaseSensitive { enum Choice { + Yes, + No + }; }; + + class NonCopyable { + NonCopyable( NonCopyable const& ) = delete; + NonCopyable( NonCopyable && ) = delete; + NonCopyable& operator = ( NonCopyable const& ) = delete; + NonCopyable& operator = ( NonCopyable && ) = delete; + + protected: + NonCopyable(); + virtual ~NonCopyable(); + }; + + struct SourceLineInfo { + + SourceLineInfo() = delete; + SourceLineInfo( char const* _file, std::size_t _line ) noexcept + : file( _file ), + line( _line ) + {} + + SourceLineInfo( SourceLineInfo const& other ) = default; + SourceLineInfo& operator = ( SourceLineInfo const& ) = default; + SourceLineInfo( SourceLineInfo&& ) noexcept = default; + SourceLineInfo& operator = ( SourceLineInfo&& ) noexcept = default; + + bool empty() const noexcept { return file[0] == '\0'; } + bool operator == ( SourceLineInfo const& other ) const noexcept; + bool operator < ( SourceLineInfo const& other ) const noexcept; + + char const* file; + std::size_t line; + }; + + std::ostream& operator << ( std::ostream& os, SourceLineInfo const& info ); + + // Bring in operator<< from global namespace into Catch namespace + // This is necessary because the overload of operator<< above makes + // lookup stop at namespace Catch + using ::operator<<; + + // Use this in variadic streaming macros to allow + // >> +StreamEndStop + // as well as + // >> stuff +StreamEndStop + struct StreamEndStop { + std::string operator+() const; + }; + template + T const& operator + ( T const& value, StreamEndStop ) { + return value; + } +} + +#define CATCH_INTERNAL_LINEINFO \ + ::Catch::SourceLineInfo( __FILE__, static_cast( __LINE__ ) ) + +// end catch_common.h +namespace Catch { + + struct RegistrarForTagAliases { + RegistrarForTagAliases( char const* alias, char const* tag, SourceLineInfo const& lineInfo ); + }; + +} // end namespace Catch + +#define CATCH_REGISTER_TAG_ALIAS( alias, spec ) \ + CATCH_INTERNAL_START_WARNINGS_SUPPRESSION \ + CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + namespace{ Catch::RegistrarForTagAliases INTERNAL_CATCH_UNIQUE_NAME( AutoRegisterTagAlias )( alias, spec, CATCH_INTERNAL_LINEINFO ); } \ + CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION + +// end catch_tag_alias_autoregistrar.h +// start catch_test_registry.h + +// start catch_interfaces_testcase.h + +#include + +namespace Catch { + + class TestSpec; + + struct ITestInvoker { + virtual void invoke () const = 0; + virtual ~ITestInvoker(); + }; + + class TestCase; + struct IConfig; + + struct ITestCaseRegistry { + virtual ~ITestCaseRegistry(); + virtual std::vector const& getAllTests() const = 0; + virtual std::vector const& getAllTestsSorted( IConfig const& config ) const = 0; + }; + + bool isThrowSafe( TestCase const& testCase, IConfig const& config ); + bool matchTest( TestCase const& testCase, TestSpec const& testSpec, IConfig const& config ); + std::vector filterTests( std::vector const& testCases, TestSpec const& testSpec, IConfig const& config ); + std::vector const& getAllTestCasesSorted( IConfig const& config ); + +} + +// end catch_interfaces_testcase.h +// start catch_stringref.h + +#include +#include +#include +#include + +namespace Catch { + + /// A non-owning string class (similar to the forthcoming std::string_view) + /// Note that, because a StringRef may be a substring of another string, + /// it may not be null terminated. + class StringRef { + public: + using size_type = std::size_t; + using const_iterator = const char*; + + private: + static constexpr char const* const s_empty = ""; + + char const* m_start = s_empty; + size_type m_size = 0; + + public: // construction + constexpr StringRef() noexcept = default; + + StringRef( char const* rawChars ) noexcept; + + constexpr StringRef( char const* rawChars, size_type size ) noexcept + : m_start( rawChars ), + m_size( size ) + {} + + StringRef( std::string const& stdString ) noexcept + : m_start( stdString.c_str() ), + m_size( stdString.size() ) + {} + + explicit operator std::string() const { + return std::string(m_start, m_size); + } + + public: // operators + auto operator == ( StringRef const& other ) const noexcept -> bool; + auto operator != (StringRef const& other) const noexcept -> bool { + return !(*this == other); + } + + auto operator[] ( size_type index ) const noexcept -> char { + assert(index < m_size); + return m_start[index]; + } + + public: // named queries + constexpr auto empty() const noexcept -> bool { + return m_size == 0; + } + constexpr auto size() const noexcept -> size_type { + return m_size; + } + + // Returns the current start pointer. If the StringRef is not + // null-terminated, throws std::domain_exception + auto c_str() const -> char const*; + + public: // substrings and searches + // Returns a substring of [start, start + length). + // If start + length > size(), then the substring is [start, size()). + // If start > size(), then the substring is empty. + auto substr( size_type start, size_type length ) const noexcept -> StringRef; + + // Returns the current start pointer. May not be null-terminated. + auto data() const noexcept -> char const*; + + constexpr auto isNullTerminated() const noexcept -> bool { + return m_start[m_size] == '\0'; + } + + public: // iterators + constexpr const_iterator begin() const { return m_start; } + constexpr const_iterator end() const { return m_start + m_size; } + }; + + auto operator += ( std::string& lhs, StringRef const& sr ) -> std::string&; + auto operator << ( std::ostream& os, StringRef const& sr ) -> std::ostream&; + + constexpr auto operator "" _sr( char const* rawChars, std::size_t size ) noexcept -> StringRef { + return StringRef( rawChars, size ); + } +} // namespace Catch + +constexpr auto operator "" _catch_sr( char const* rawChars, std::size_t size ) noexcept -> Catch::StringRef { + return Catch::StringRef( rawChars, size ); +} + +// end catch_stringref.h +// start catch_preprocessor.hpp + + +#define CATCH_RECURSION_LEVEL0(...) __VA_ARGS__ +#define CATCH_RECURSION_LEVEL1(...) CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL2(...) CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL3(...) CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL4(...) CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL5(...) CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(__VA_ARGS__))) + +#ifdef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_EXPAND_VARGS(...) __VA_ARGS__ +// MSVC needs more evaluations +#define CATCH_RECURSION_LEVEL6(...) CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(__VA_ARGS__))) +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL6(CATCH_RECURSION_LEVEL6(__VA_ARGS__)) +#else +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL5(__VA_ARGS__) +#endif + +#define CATCH_REC_END(...) +#define CATCH_REC_OUT + +#define CATCH_EMPTY() +#define CATCH_DEFER(id) id CATCH_EMPTY() + +#define CATCH_REC_GET_END2() 0, CATCH_REC_END +#define CATCH_REC_GET_END1(...) CATCH_REC_GET_END2 +#define CATCH_REC_GET_END(...) CATCH_REC_GET_END1 +#define CATCH_REC_NEXT0(test, next, ...) next CATCH_REC_OUT +#define CATCH_REC_NEXT1(test, next) CATCH_DEFER ( CATCH_REC_NEXT0 ) ( test, next, 0) +#define CATCH_REC_NEXT(test, next) CATCH_REC_NEXT1(CATCH_REC_GET_END test, next) + +#define CATCH_REC_LIST0(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2(f, x, peek, ...) f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) + +#define CATCH_REC_LIST0_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2_UD(f, userdata, x, peek, ...) f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) + +// Applies the function macro `f` to each of the remaining parameters, inserts commas between the results, +// and passes userdata as the first parameter to each invocation, +// e.g. CATCH_REC_LIST_UD(f, x, a, b, c) evaluates to f(x, a), f(x, b), f(x, c) +#define CATCH_REC_LIST_UD(f, userdata, ...) CATCH_RECURSE(CATCH_REC_LIST2_UD(f, userdata, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define CATCH_REC_LIST(f, ...) CATCH_RECURSE(CATCH_REC_LIST2(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define INTERNAL_CATCH_EXPAND1(param) INTERNAL_CATCH_EXPAND2(param) +#define INTERNAL_CATCH_EXPAND2(...) INTERNAL_CATCH_NO## __VA_ARGS__ +#define INTERNAL_CATCH_DEF(...) INTERNAL_CATCH_DEF __VA_ARGS__ +#define INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE(...) INTERNAL_CATCH_STRINGIZE2(__VA_ARGS__) +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_STRINGIZE2(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) +#else +// MSVC is adding extra space and needs another indirection to expand INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE2(...) INTERNAL_CATCH_STRINGIZE3(__VA_ARGS__) +#define INTERNAL_CATCH_STRINGIZE3(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) (INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) + 1) +#endif + +#define INTERNAL_CATCH_MAKE_NAMESPACE2(...) ns_##__VA_ARGS__ +#define INTERNAL_CATCH_MAKE_NAMESPACE(name) INTERNAL_CATCH_MAKE_NAMESPACE2(name) + +#define INTERNAL_CATCH_REMOVE_PARENS(...) INTERNAL_CATCH_EXPAND1(INTERNAL_CATCH_DEF __VA_ARGS__) + +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) decltype(get_wrapper()) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__)) +#else +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) INTERNAL_CATCH_EXPAND_VARGS(decltype(get_wrapper())) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_EXPAND_VARGS(INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__))) +#endif + +#define INTERNAL_CATCH_MAKE_TYPE_LISTS_FROM_TYPES(...)\ + CATCH_REC_LIST(INTERNAL_CATCH_MAKE_TYPE_LIST,__VA_ARGS__) + +#define INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_0) INTERNAL_CATCH_REMOVE_PARENS(_0) +#define INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_0, _1) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_1) +#define INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_0, _1, _2) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_1, _2) +#define INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_0, _1, _2, _3) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_1, _2, _3) +#define INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_0, _1, _2, _3, _4) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_1, _2, _3, _4) +#define INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_0, _1, _2, _3, _4, _5) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_1, _2, _3, _4, _5) +#define INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_0, _1, _2, _3, _4, _5, _6) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_1, _2, _3, _4, _5, _6) +#define INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_0, _1, _2, _3, _4, _5, _6, _7) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_1, _2, _3, _4, _5, _6, _7) +#define INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_1, _2, _3, _4, _5, _6, _7, _8) +#define INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9) +#define INTERNAL_CATCH_REMOVE_PARENS_11_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10) + +#define INTERNAL_CATCH_VA_NARGS_IMPL(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, N, ...) N + +#define INTERNAL_CATCH_TYPE_GEN\ + template struct TypeList {};\ + template\ + constexpr auto get_wrapper() noexcept -> TypeList { return {}; }\ + template class...> struct TemplateTypeList{};\ + template class...Cs>\ + constexpr auto get_wrapper() noexcept -> TemplateTypeList { return {}; }\ + template\ + struct append;\ + template\ + struct rewrap;\ + template class, typename...>\ + struct create;\ + template class, typename>\ + struct convert;\ + \ + template \ + struct append { using type = T; };\ + template< template class L1, typename...E1, template class L2, typename...E2, typename...Rest>\ + struct append, L2, Rest...> { using type = typename append, Rest...>::type; };\ + template< template class L1, typename...E1, typename...Rest>\ + struct append, TypeList, Rest...> { using type = L1; };\ + \ + template< template class Container, template class List, typename...elems>\ + struct rewrap, List> { using type = TypeList>; };\ + template< template class Container, template class List, class...Elems, typename...Elements>\ + struct rewrap, List, Elements...> { using type = typename append>, typename rewrap, Elements...>::type>::type; };\ + \ + template