From 4de3375232af021b5372ceb0711a44ffd00670fa Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 24 Oct 2015 23:49:35 -0400 Subject: [PATCH 01/79] deleting more copy constructors that should not exist --- dart/dynamics/DegreeOfFreedom.h | 2 ++ dart/dynamics/EulerJoint.h | 2 ++ dart/dynamics/FreeJoint.h | 2 ++ dart/dynamics/MultiDofJoint.h | 2 ++ dart/dynamics/PlanarJoint.h | 2 ++ dart/dynamics/PrismaticJoint.h | 2 ++ dart/dynamics/RevoluteJoint.h | 2 ++ dart/dynamics/ScrewJoint.h | 2 ++ dart/dynamics/TranslationalJoint.h | 2 ++ dart/dynamics/UniversalJoint.h | 2 ++ dart/dynamics/WeldJoint.h | 2 ++ dart/dynamics/ZeroDofJoint.h | 2 ++ 12 files changed, 24 insertions(+) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index d5479955761fe..a7df00b4f0dad 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -64,6 +64,8 @@ class DegreeOfFreedom : public virtual common::Subject template friend class MultiDofJoint; friend class Skeleton; + DegreeOfFreedom(const DegreeOfFreedom&) = delete; + /// Change the name of this DegreeOfFreedom /// /// The _preserveName argument will be passed to the preserveName(bool) diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index f2e3fae098523..e2686359f394d 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -81,6 +81,8 @@ class EulerJoint : public MultiDofJoint<3> virtual ~Properties() = default; }; + EulerJoint(const EulerJoint&) = delete; + /// Destructor virtual ~EulerJoint(); diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index ac393d51a8d86..8a70a26fa8d15 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -61,6 +61,8 @@ class FreeJoint : public MultiDofJoint<6> virtual ~Properties() = default; }; + FreeJoint(const FreeJoint&) = delete; + /// Destructor virtual ~FreeJoint(); diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index ad188d202bf77..5b1ff7dc8113e 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -62,6 +62,8 @@ class MultiDofJoint : public Joint typedef Eigen::Matrix Vector; + MultiDofJoint(const MultiDofJoint&) = delete; + struct UniqueProperties { /// Lower limit of position diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 56f0427fca1e6..db59468437cd0 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -121,6 +121,8 @@ class PlanarJoint : public MultiDofJoint<3> virtual ~Properties() = default; }; + PlanarJoint(const PlanarJoint&) = delete; + /// Destructor virtual ~PlanarJoint(); diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 3ce7a38723724..2ad7a9026fe9d 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -72,6 +72,8 @@ class PrismaticJoint : public SingleDofJoint virtual ~Properties() = default; }; + PrismaticJoint(const PrismaticJoint&) = delete; + /// Destructor virtual ~PrismaticJoint(); diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 4831b07303b2f..367cf50b79f84 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -74,6 +74,8 @@ class RevoluteJoint : public SingleDofJoint virtual ~Properties() = default; }; + RevoluteJoint(const RevoluteJoint&) = delete; + /// Destructor virtual ~RevoluteJoint(); diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index be80b97248c55..ba6910f9a244c 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -78,6 +78,8 @@ class ScrewJoint : public SingleDofJoint virtual ~Properties() = default; }; + ScrewJoint(const ScrewJoint&) = delete; + /// Destructor virtual ~ScrewJoint(); diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index e6e1ed8c832b0..7506f65774983 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/dart/dynamics/TranslationalJoint.h @@ -59,6 +59,8 @@ class TranslationalJoint : public MultiDofJoint<3> virtual ~Properties() = default; }; + TranslationalJoint(const TranslationalJoint&) = delete; + /// Destructor virtual ~TranslationalJoint(); diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index faf0cbe4afb5e..dccf8bbfdf1fb 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -73,6 +73,8 @@ class UniversalJoint : public MultiDofJoint<2> virtual ~Properties() = default; }; + UniversalJoint(const UniversalJoint&) = delete; + /// Destructor virtual ~UniversalJoint(); diff --git a/dart/dynamics/WeldJoint.h b/dart/dynamics/WeldJoint.h index a24dc8419d10e..6de5219490837 100644 --- a/dart/dynamics/WeldJoint.h +++ b/dart/dynamics/WeldJoint.h @@ -59,6 +59,8 @@ class WeldJoint : public ZeroDofJoint virtual ~Properties() = default; }; + WeldJoint(const WeldJoint&) = delete; + /// Destructor virtual ~WeldJoint(); diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index a2dcbf6414102..f9bd0d5ca5393 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -58,6 +58,8 @@ class ZeroDofJoint : public Joint virtual ~Properties() = default; }; + ZeroDofJoint(const ZeroDofJoint&) = delete; + /// Destructor virtual ~ZeroDofJoint(); From bc49d40c1306f5097bd58cf5256ddaa8201ab3f0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 25 Oct 2015 10:31:52 -0400 Subject: [PATCH 02/79] Update .travis.yml --- .travis.yml | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index ddd2af804a7fc..d878fe1427078 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,7 +1,7 @@ language: cpp os: - linux - - osx + #- osx compiler: - gcc - clang @@ -15,8 +15,25 @@ matrix: - os: osx compiler: gcc before_install: - - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/before_install_linux.sh' ; fi - - if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/before_install_osx.sh' ; fi + #- if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/before_install_linux.sh' ; fi + #- if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/before_install_osx.sh' ; fi + # Install boost 1.55 compatible with C++11 + - wget --quiet -O boost_1_55_0.tar.gz http://sourceforge.net/projects/boost/files/boost/1.55.0/boost_1_55_0.tar.gz/download + - tar -xzf boost_1_55_0.tar.gz + - cd boost_1_55_0/ + - sudo apt-get -qq update + - sudo apt-get -qq install build-essential g++ python-dev autotools-dev libicu-dev build-essential libbz2-dev + - ./bootstrap.sh --prefix=/usr/local &>/dev/null # mute the output + - n=`cat /proc/cpuinfo | grep "cpu cores" | uniq | awk '{print $NF}'` + - sudo ./b2 --with=all -j $n install &>/dev/null # mute the output + - sudo sh -c 'echo "/usr/local/lib" >> /etc/ld.so.conf.d/local.conf' + - sudo ldconfig + - sudo add-apt-repository --yes ppa:dartsim/ppa + - sudo apt-get -qq update + - APT_CORE='cmake libassimp-dev libccd-dev libfcl-dev' + - APT=$APT_CORE' freeglut3-dev libxi-dev libxmu-dev libflann-dev libnlopt-dev coinor-libipopt-dev libtinyxml-dev libtinyxml2-dev liburdfdom-dev liburdfdom-headers-dev libopenscenegraph-dev' + - if [ $BUILD_CORE_ONLY = OFF ]; then sudo apt-get -qq --yes --force-yes install $APT; else sudo apt-get -qq --yes --force-yes install $APT_CORE; fi + install: - mkdir build - cd build From 667ff1c2e7b3f96bfd84f65ca0e55a4de0680c96 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 25 Oct 2015 10:44:08 -0400 Subject: [PATCH 03/79] Revert last accidental commit (bc49d40c1306f5097bd58cf5256ddaa8201ab3f0) --- .travis.yml | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/.travis.yml b/.travis.yml index d878fe1427078..ddd2af804a7fc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,7 +1,7 @@ language: cpp os: - linux - #- osx + - osx compiler: - gcc - clang @@ -15,25 +15,8 @@ matrix: - os: osx compiler: gcc before_install: - #- if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/before_install_linux.sh' ; fi - #- if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/before_install_osx.sh' ; fi - # Install boost 1.55 compatible with C++11 - - wget --quiet -O boost_1_55_0.tar.gz http://sourceforge.net/projects/boost/files/boost/1.55.0/boost_1_55_0.tar.gz/download - - tar -xzf boost_1_55_0.tar.gz - - cd boost_1_55_0/ - - sudo apt-get -qq update - - sudo apt-get -qq install build-essential g++ python-dev autotools-dev libicu-dev build-essential libbz2-dev - - ./bootstrap.sh --prefix=/usr/local &>/dev/null # mute the output - - n=`cat /proc/cpuinfo | grep "cpu cores" | uniq | awk '{print $NF}'` - - sudo ./b2 --with=all -j $n install &>/dev/null # mute the output - - sudo sh -c 'echo "/usr/local/lib" >> /etc/ld.so.conf.d/local.conf' - - sudo ldconfig - - sudo add-apt-repository --yes ppa:dartsim/ppa - - sudo apt-get -qq update - - APT_CORE='cmake libassimp-dev libccd-dev libfcl-dev' - - APT=$APT_CORE' freeglut3-dev libxi-dev libxmu-dev libflann-dev libnlopt-dev coinor-libipopt-dev libtinyxml-dev libtinyxml2-dev liburdfdom-dev liburdfdom-headers-dev libopenscenegraph-dev' - - if [ $BUILD_CORE_ONLY = OFF ]; then sudo apt-get -qq --yes --force-yes install $APT; else sudo apt-get -qq --yes --force-yes install $APT_CORE; fi - + - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/before_install_linux.sh' ; fi + - if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/before_install_osx.sh' ; fi install: - mkdir build - cd build From 8e1034c523ccd1b706cd5698f2782473df2de7d9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 25 Oct 2015 12:49:52 -0400 Subject: [PATCH 04/79] Use trusty (beta) for travis-ci build tests --- .travis.yml | 3 +++ ci/before_install_linux.sh | 50 ++++++-------------------------------- 2 files changed, 11 insertions(+), 42 deletions(-) diff --git a/.travis.yml b/.travis.yml index ddd2af804a7fc..dcf0ecd839f7a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,6 +2,8 @@ language: cpp os: - linux - osx +sudo: required +dist: trusty compiler: - gcc - clang @@ -27,3 +29,4 @@ script: after_failure: - if [ "$TRAVIS_OS_NAME" = "linux" ]; then '../ci/after_failure_linux.sh' ; fi - if [ "$TRAVIS_OS_NAME" = "osx" ]; then '../ci/after_failure_osx.sh' ; fi + diff --git a/ci/before_install_linux.sh b/ci/before_install_linux.sh index 11eeed1b33f80..d98c922fb3d1b 100755 --- a/ci/before_install_linux.sh +++ b/ci/before_install_linux.sh @@ -1,61 +1,29 @@ -before_install() { - cd /tmp - - # Install nlopt from source since Ubuntu 12.04 does not provide debian package for nlopt - curl -o nlopt-2.4.1.tar.gz http://ab-initio.mit.edu/nlopt/nlopt-2.4.1.tar.gz - tar -xf nlopt-2.4.1.tar.gz - cd nlopt-2.4.1/ - sh autogen.sh &>/dev/null # mute the output - make CPPFLAGS='-fPIC' && sudo make install &>/dev/null # mute the output -} - -# Install gcc-4.8 and g++-4.8 for C++11 -sudo apt-get -qq --yes install python-software-properties -sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test -sudo apt-get -qq update -sudo apt-get -qq --yes install gcc-4.8 g++-4.8 -sudo apt-get -qq --yes autoremove -sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.8 50 -sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-4.8 50 - -# Install eigen-3.2.1 (for unsupported/Eigen/Splines) -wget --quiet -O libeigen3-dev_3.2.1-1~precise1_all.deb http://packages.yade-dem.org/precise/libeigen3-dev_3.2.1-1~precise1_all.deb -sudo dpkg -i libeigen3-dev_3.2.1-1~precise1_all.deb - -# Install boost 1.55 compatible with C++11 -wget --quiet -O boost_1_55_0.tar.gz http://sourceforge.net/projects/boost/files/boost/1.55.0/boost_1_55_0.tar.gz/download -tar -xzf boost_1_55_0.tar.gz -cd boost_1_55_0/ -sudo apt-get -qq update -sudo apt-get -qq install build-essential g++ python-dev autotools-dev libicu-dev build-essential libbz2-dev -./bootstrap.sh --prefix=/usr/local &>/dev/null # mute the output -n=`cat /proc/cpuinfo | grep "cpu cores" | uniq | awk '{print $NF}'` -sudo ./b2 --with=all -j $n install &>/dev/null # mute the output -sudo sh -c 'echo "/usr/local/lib" >> /etc/ld.so.conf.d/local.conf' -sudo ldconfig - -sudo add-apt-repository --yes ppa:libccd-debs/ppa -sudo add-apt-repository --yes ppa:fcl-debs/ppa -sudo add-apt-repository --yes ppa:dartsim/ppa +sudo apt-add-repository --yes ppa:libccd-debs/ppa +sudo apt-add-repository --yes ppa:fcl-debs/ppa +sudo apt-add-repository --yes ppa:dartsim/ppa sudo apt-get -qq update APT_CORE=' cmake libassimp-dev +libboost-all-dev libccd-dev +libeigen3-dev libfcl-dev ' -APT=$APT_CORE' +APT=$APT_CORE' freeglut3-dev libxi-dev libxmu-dev libflann-dev +libnlopt-dev coinor-libipopt-dev libtinyxml-dev libtinyxml2-dev liburdfdom-dev liburdfdom-headers-dev +libopenscenegraph-dev ' if [ $BUILD_CORE_ONLY = OFF ]; then @@ -64,5 +32,3 @@ else sudo apt-get -qq --yes --force-yes install $APT_CORE fi -(before_install) - From ebc0ad8891c419ae2e35062ccdaf07e8e91c399a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 25 Oct 2015 18:31:07 -0400 Subject: [PATCH 05/79] Update version number and changelogs --- CMakeLists.txt | 2 +- Changelog.md | 20 ++++++++++++++++++++ debian/changelog | 10 ++++++++++ package.xml | 2 +- 4 files changed, 32 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 50d8efdca191d..bbc629af3cc40 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,7 +41,7 @@ project(dart) set(DART_MAJOR_VERSION "5") set(DART_MINOR_VERSION "1") -set(DART_PATCH_VERSION "0") +set(DART_PATCH_VERSION "1") set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}") set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.") set(DART_PKG_EXTERNAL_DEPS "flann, ccd, fcl") diff --git a/Changelog.md b/Changelog.md index 5b4f589742a08..2625e946724c1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,23 @@ +### Version 5.1.1 (2015-11-02) + +1. Add bullet dependency to package.xml + * [Pull request #523](https://github.com/dartsim/dart/pull/523) + +1. Improved travis-ci build log for Mac + * [Pull request #529](https://github.com/dartsim/dart/pull/529) + +1. Fixed const qualification of ResourceRetriever + * [Pull request #534](https://github.com/dartsim/dart/pull/534) + * [Issue #532](https://github.com/dartsim/dart/issues/532) + +1. Fixed aligned memory allocation with Eigen objects + * [Pull request #527](https://github.com/dartsim/dart/pull/527) + +1. Fixed copy safety for various classes + * [Pull request #526](https://github.com/dartsim/dart/pull/526) + * [Pull request #539](https://github.com/dartsim/dart/pull/539) + * [Issue #524](https://github.com/dartsim/dart/issues/524) + ### Version 5.1.0 (2015-10-15) 1. Fixed incorrect rotational motion of BallJoint and FreeJoint diff --git a/debian/changelog b/debian/changelog index d5fa5af1de538..1a2ad08973ed7 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,13 @@ +dart (5.1.1) unstable; urgency=medium + + * Add bullet dependency to package.xml + * Improved travis-ci build log for Mac + * Fixed const qualification of ResourceRetriever + * Fixed aligned memory allocation with Eigen objects + * Fixed copy safety for various classes + + -- Jeongseok Lee Mon, 02 Nov 2015 00:00:00 -0500 + dart (5.1.0) unstable; urgency=medium * Fixed incorrect rotational motion of BallJoint and FreeJoint diff --git a/package.xml b/package.xml index 3fe1452ad66db..d96121eb79ff9 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ a Catkin workspace. Catkin is not required to build DART. For more information, see: http://ros.org/reps/rep-0136.html --> dart - 5.1.0 + 5.1.1 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics From b4b43935201fef012c7607379e3ee72ad256354b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 28 Oct 2015 13:36:56 -0400 Subject: [PATCH 06/79] Add option to build with MSVC default options (flags) --- CMakeLists.txt | 10 ++++++---- appveyor.yml | 3 ++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bbc629af3cc40..32339c7572192 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ option(BUILD_CORE_ONLY "Build only the core of DART" OFF) if(MSVC) set(DART_RUNTIME_LIBRARY "/MD" CACHE STRING "BaseName chosen by the user at CMake configure time") set_property(CACHE DART_RUNTIME_LIBRARY PROPERTY STRINGS /MD /MT) + option(DART_MSVC_DEFAULT_OPTIONS "Build DART with default Visual Studio options" OFF) else() option(BUILD_SHARED_LIBS "Build shared libraries" ON) endif() @@ -420,14 +421,15 @@ endif() # Compiler flags #=============================================================================== if(MSVC) - message(STATUS "Setup Visual Studio Specific Flags") # Visual Studio enables c++11 support by default if(NOT MSVC12) message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") endif() - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${DART_RUNTIME_LIBRARY}d /Zi /Gy /W1 /EHsc") - set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${DART_RUNTIME_LIBRARY} /Zi /GL /Gy /W1 /EHsc /arch:SSE2") - set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/LTCG /INCREMENTAL:NO") + if(NOT DART_MSVC_DEFAULT_OPTIONS) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${DART_RUNTIME_LIBRARY}d /Zi /Gy /W1 /EHsc") + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${DART_RUNTIME_LIBRARY} /Zi /GL /Gy /W1 /EHsc /arch:SSE2") + set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/LTCG /INCREMENTAL:NO") + endif(NOT DART_MSVC_DEFAULT_OPTIONS) elseif(CMAKE_COMPILER_IS_GNUCXX) set(CMAKE_CXX_FLAGS "-Wall -msse2 -fPIC") execute_process( diff --git a/appveyor.yml b/appveyor.yml index 8ada23ea80804..69e229bbc1dbd 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -11,6 +11,7 @@ platform: # specify custom environment variables environment: + MSVC_DEFAULT_OPTIONS: ON BOOST_ROOT: C:\Libraries\boost BOOST_LIBRARYDIR: C:\Libraries\boost\stage\lib BUILD_EXAMPLES: OFF # don't build examples to not exceed allowed build time (40 min) @@ -54,7 +55,7 @@ before_build: # We generate project files for Visual Studio 12 because the boost binaries installed on the test server are for Visual Studio 12. - cmd: if "%platform%"=="Win32" set CMAKE_GENERATOR_NAME=Visual Studio 12 - cmd: if "%platform%"=="x64" set CMAKE_GENERATOR_NAME=Visual Studio 12 Win64 - - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%configuration% -DBUILD_CORE_ONLY="%BUILD_CORE_ONLY%" -DDART_BUILD_EXAMPLES="%BUILD_EXAMPLES%" -DDART_BUILD_TUTORIALS="%BUILD_TUTORIALS%" -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" -DBoost_USE_STATIC_LIBS="ON" -Durdfdom_DIR="%urdfdom_DIR%" -Durdfdom_headers_DIR="%urdfdom_headers_DIR%" .. + - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%configuration% -DBUILD_CORE_ONLY="%BUILD_CORE_ONLY%" -DDART_BUILD_EXAMPLES="%BUILD_EXAMPLES%" -DDART_BUILD_TUTORIALS="%BUILD_TUTORIALS%" -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" -DBoost_USE_STATIC_LIBS="ON" -Durdfdom_DIR="%urdfdom_DIR%" -Durdfdom_headers_DIR="%urdfdom_headers_DIR%" -DDART_MSVC_DEFAULT_OPTIONS="%MSVC_DEFAULT_OPTIONS%" .. build: project: C:\projects\dart\build\dart.sln # path to Visual Studio solution or project From 0d2425ecec9181ecd7d6503a7dd8be4ad25d7734 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 28 Oct 2015 16:17:59 -0400 Subject: [PATCH 07/79] Enable parallel build for MSVC --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32339c7572192..7695c4d6b26b8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -425,6 +425,7 @@ if(MSVC) if(NOT MSVC12) message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") endif() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP4") if(NOT DART_MSVC_DEFAULT_OPTIONS) set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${DART_RUNTIME_LIBRARY}d /Zi /Gy /W1 /EHsc") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${DART_RUNTIME_LIBRARY} /Zi /GL /Gy /W1 /EHsc /arch:SSE2") From 207636224b9dd3f482c50af5d2907c929fe01299 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 28 Oct 2015 18:44:14 -0400 Subject: [PATCH 08/79] Update CMakeLists.txt --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7695c4d6b26b8..ffeaabe699a85 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -426,10 +426,10 @@ if(MSVC) message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP4") + set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/LTCG /INCREMENTAL:NO") if(NOT DART_MSVC_DEFAULT_OPTIONS) set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${DART_RUNTIME_LIBRARY}d /Zi /Gy /W1 /EHsc") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${DART_RUNTIME_LIBRARY} /Zi /GL /Gy /W1 /EHsc /arch:SSE2") - set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/LTCG /INCREMENTAL:NO") endif(NOT DART_MSVC_DEFAULT_OPTIONS) elseif(CMAKE_COMPILER_IS_GNUCXX) set(CMAKE_CXX_FLAGS "-Wall -msse2 -fPIC") From 4e8abc8a323646ba4bcff8631525333d59f0bd38 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 29 Oct 2015 18:54:27 -0400 Subject: [PATCH 09/79] Fix const correctness of BodyNode::getMomentOfInertia() --- dart/dynamics/BodyNode.cpp | 5 +++-- dart/dynamics/BodyNode.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index fc23985a8cbfb..acbce6ec78901 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -317,8 +317,9 @@ void BodyNode::setMomentOfInertia(double _Ixx, double _Iyy, double _Izz, } //============================================================================== -void BodyNode::getMomentOfInertia(double& _Ixx, double& _Iyy, double& _Izz, - double& _Ixy, double& _Ixz, double& _Iyz) +void BodyNode::getMomentOfInertia( + double& _Ixx, double& _Iyy, double& _Izz, + double& _Ixy, double& _Ixz, double& _Iyz) const { _Ixx = mBodyP.mInertia.getParameter(Inertia::I_XX); _Iyy = mBodyP.mInertia.getParameter(Inertia::I_YY); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 71d424326b6cb..357f15556d90e 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -204,7 +204,7 @@ class BodyNode : /// Return moment of inertia defined around the center of mass void getMomentOfInertia( double& _Ixx, double& _Iyy, double& _Izz, - double& _Ixy, double& _Ixz, double& _Iyz); + double& _Ixy, double& _Ixz, double& _Iyz) const; /// Return spatial inertia const Eigen::Matrix6d& getSpatialInertia() const; From 643f8188b584d15dc073c50b7ed54a9ba492a71a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 2 Nov 2015 13:23:54 -0500 Subject: [PATCH 10/79] Add check for missing symbols in assimp --- CMakeLists.txt | 47 ++++++++++++++++++++++++++++++++++++- dart/config.h.in | 4 ++++ dart/dynamics/MeshShape.cpp | 17 +++++++------- 3 files changed, 59 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bbc629af3cc40..238d5f5a0489a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,6 +150,51 @@ endif() find_package(ASSIMP 3.0.0 QUIET) if(ASSIMP_FOUND) message(STATUS "Looking for ASSIMP - ${ASSIMP_VERSION} found") + + # Check for missing symbols in ASSIMP (see #451) + include(CheckCXXSourceCompiles) + set(CMAKE_REQUIRED_INCLUDES ${ASSIMP_INCLUDE_DIRS}) + set(CMAKE_REQUIRED_LIBRARIES ${ASSIMP_LIBRARIES}) + + check_cxx_source_compiles( + " + #include + int main() + { + aiScene* scene = new aiScene; + delete scene; + return 1; + } + " + ASSIMP_AISCENE_CTOR_DTOR_DEFINED) + + if(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) + message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} has missing symbols " + "for constructor or destructor of aiScene. DART will use own " + "implementation. Please use ASSIMP that fixed this issue.") + endif(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) + + check_cxx_source_compiles( + " + #include + int main() + { + aiMaterial* material = new aiMaterial; + delete material; + return 1; + } + " + ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) + + if(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) + message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} has missing symbols " + "for constructor or destructor of aiMaterial. DART will use own " + "implementation. Please use ASSIMP that fixed this issue.") + endif(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) + + unset(CMAKE_REQUIRED_INCLUDES) + unset(CMAKE_REQUIRED_LIBRARIES) + else() if(ASSIMP_VERSION) message(SEND_ERROR "Looking for ASSIMP - ${ASSIMP_VERSION} found, ${PROJECT_NAME} requires 3.0.0 or greater.") @@ -410,7 +455,7 @@ endif() #=============================================================================== execute_process(COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/tools/case_sensitive_filesystem RESULT_VARIABLE FILESYSTEM_CASE_SENSITIVE_RETURN) -if (${FILESYSTEM_CASE_SENSITIVE_RETURN} EQUAL 0) +if(${FILESYSTEM_CASE_SENSITIVE_RETURN} EQUAL 0) set(FILESYSTEM_CASE_SENSITIVE TRUE) else() set(FILESYSTEM_CASE_SENSITIVE FALSE) diff --git a/dart/config.h.in b/dart/config.h.in index 0128e0e5613ae..2db6ccba4f347 100644 --- a/dart/config.h.in +++ b/dart/config.h.in @@ -41,4 +41,8 @@ #define DART_ROOT_PATH "@CMAKE_SOURCE_DIR@/" #define DART_DATA_PATH "@CMAKE_SOURCE_DIR@/data/" +// See #451 +#cmakedefine ASSIMP_AISCENE_CTOR_DTOR_DEFINED 1 +#cmakedefine ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED 1 + #endif // #ifndef DART_CONFIG_H_ diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index a78344ecc34b5..0621a1edbb586 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -44,14 +44,16 @@ #include #include +#include "dart/config.h" #include "dart/renderer/RenderInterface.h" #include "dart/common/Console.h" #include "dart/dynamics/AssimpInputResourceAdaptor.h" #include "dart/common/LocalResourceRetriever.h" #include "dart/common/Uri.h" -// We define our own constructor for aiScene, because it seems to be missing -// from the standard assimp library +#ifndef ASSIMP_AISCENE_CTOR_DTOR_DEFINED +// We define our own constructor and destructor for aiScene, because it seems to +// be missing from the standard assimp library (see #451) aiScene::aiScene() : mFlags(0), mRootNode(nullptr), @@ -70,8 +72,6 @@ aiScene::aiScene() } -// We define our own destructor for aiScene, because it seems to be missing -// from the standard assimp library aiScene::~aiScene() { delete mRootNode; @@ -106,9 +106,11 @@ aiScene::~aiScene() delete mCameras[a]; delete[] mCameras; } +#endif // #ifndef ASSIMP_AISCENE_CTOR_DTOR_DEFINED -// We define our own constructor for aiMaterial, because it seems to be missing -// from the standard assimp library +// We define our own constructor and destructor for aiMaterial, because it seems +// to be missing from the standard assimp library (see #451) +#ifndef ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED aiMaterial::aiMaterial() { mNumProperties = 0; @@ -118,8 +120,6 @@ aiMaterial::aiMaterial() mProperties[i] = nullptr; } -// We define our own destructor for aiMaterial, because it seems to be missing -// from the standard assimp library aiMaterial::~aiMaterial() { for(size_t i=0; i Date: Mon, 2 Nov 2015 21:59:19 -0500 Subject: [PATCH 11/79] Remove requirement of Assimp < 3.0~dfsg-4 - DART now is able to detect if installed Assimp is missing C++ symbols so we don't need to restrict DART to depend on the version of Assimp that is missing the symbols. --- debian/control | 2 -- 1 file changed, 2 deletions(-) diff --git a/debian/control b/debian/control index 6c8efec230e7c..3938bf469407d 100644 --- a/debian/control +++ b/debian/control @@ -6,7 +6,6 @@ Build-Depends: debhelper (>= 9), libeigen3-dev, libfcl-dev (>= 0.2.7), libassimp-dev (>= 3), - libassimp-dev (<< 3.0~dfsg-4), freeglut3-dev, libxi-dev, libxmu-dev, @@ -32,7 +31,6 @@ Depends: ${misc:Depends}, libdart-core5.1 (= ${binary:Version}), libeigen3-dev, libassimp-dev (>= 3), - libassimp-dev (<< 3.0~dfsg-4), libfcl-dev, libboost-dev (>= 1.54.0.1ubuntu1) Description: Dynamic Animation and Robotics Toolkit, core development files From 9adad4381a277430f14c53d19231ee9eac37849a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 3 Nov 2015 02:51:45 -0500 Subject: [PATCH 12/79] Relax error tolerance in signal test --- unittests/testSignal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/testSignal.cpp b/unittests/testSignal.cpp index 170f580c3bb16..3583de0b18c7d 100644 --- a/unittests/testSignal.cpp +++ b/unittests/testSignal.cpp @@ -241,7 +241,7 @@ struct signal_sum //============================================================================== TEST(Signal, ReturnValues) { - const float tol = 1e-6; + const float tol = 1.5e-6; const float a = 5.0f; const float b = 3.0f; From bdb6cdb2e3f94dfa733fd998796adb643ed320cd Mon Sep 17 00:00:00 2001 From: mklingen Date: Tue, 3 Nov 2015 15:09:00 -0500 Subject: [PATCH 13/79] new bounding box semantics --- dart/dynamics/BoxShape.cpp | 8 +++++--- dart/dynamics/CylinderShape.cpp | 5 ++--- dart/dynamics/EllipsoidShape.cpp | 5 +++-- dart/dynamics/MeshShape.cpp | 20 ++++++++------------ dart/dynamics/Shape.cpp | 12 +++++++++--- dart/dynamics/Shape.h | 13 +++++++++---- dart/math/Geometry.cpp | 12 ++++++++++++ dart/math/Geometry.h | 29 +++++++++++++++++++++++++++++ 8 files changed, 77 insertions(+), 27 deletions(-) diff --git a/dart/dynamics/BoxShape.cpp b/dart/dynamics/BoxShape.cpp index 11ef3fa0d0d03..8297f058ec262 100644 --- a/dart/dynamics/BoxShape.cpp +++ b/dart/dynamics/BoxShape.cpp @@ -48,7 +48,8 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size) assert(_size[0] > 0.0); assert(_size[1] > 0.0); assert(_size[2] > 0.0); - mBoundingBoxDim = _size; + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); initMeshes(); computeVolume(); } @@ -61,7 +62,8 @@ void BoxShape::setSize(const Eigen::Vector3d& _size) { assert(_size[1] > 0.0); assert(_size[2] > 0.0); mSize = _size; - mBoundingBoxDim = _size; + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); computeVolume(); } @@ -80,7 +82,7 @@ void BoxShape::draw(renderer::RenderInterface* _ri, _ri->setPenColor(mColor); _ri->pushMatrix(); _ri->transform(mTransform); - _ri->drawCube(mBoundingBoxDim); + _ri->drawCube(mBoundingBox.computeFullExtents()); _ri->popMatrix(); } diff --git a/dart/dynamics/CylinderShape.cpp b/dart/dynamics/CylinderShape.cpp index dc23d9419bb1c..8d68c4719eeb9 100644 --- a/dart/dynamics/CylinderShape.cpp +++ b/dart/dynamics/CylinderShape.cpp @@ -103,9 +103,8 @@ Eigen::Matrix3d CylinderShape::computeInertia(double _mass) const { } void CylinderShape::_updateBoundingBoxDim() { - mBoundingBoxDim[0] = mRadius * 2.0; - mBoundingBoxDim[1] = mRadius * 2.0; - mBoundingBoxDim[2] = mHeight; + mBoundingBox.setMin(Eigen::Vector3d(-mRadius, -mRadius, -mHeight * 0.5)); + mBoundingBox.setMax(Eigen::Vector3d(mRadius, mRadius, mHeight * 0.5)); } } // namespace dynamics diff --git a/dart/dynamics/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp index 4e7efb9231fdb..2fed38fa7c264 100644 --- a/dart/dynamics/EllipsoidShape.cpp +++ b/dart/dynamics/EllipsoidShape.cpp @@ -56,7 +56,8 @@ void EllipsoidShape::setSize(const Eigen::Vector3d& _size) { assert(_size[1] > 0.0); assert(_size[2] > 0.0); mSize = _size; - mBoundingBoxDim = _size; + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); computeVolume(); } @@ -77,7 +78,7 @@ void EllipsoidShape::draw(renderer::RenderInterface* _ri, _ri->setPenColor(mColor); _ri->pushMatrix(); _ri->transform(mTransform); - _ri->drawEllipsoid(mBoundingBoxDim); + _ri->drawEllipsoid(mBoundingBox.computeFullExtents()); _ri->popMatrix(); } diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index a78344ecc34b5..7d4d164982648 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -278,9 +278,10 @@ void MeshShape::draw(renderer::RenderInterface* _ri, Eigen::Matrix3d MeshShape::computeInertia(double _mass) const { // use bounding box to represent the mesh - double l = mScale[0] * mBoundingBoxDim[0]; - double h = mScale[1] * mBoundingBoxDim[1]; - double w = mScale[2] * mBoundingBoxDim[2]; + Eigen::Vector3d bounds = mBoundingBox.computeFullExtents(); + double l = bounds.x(); + double h = bounds.y(); + double w = bounds.z(); Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); inertia(0, 0) = _mass / 12.0 * (h * h + w * w); @@ -291,12 +292,8 @@ Eigen::Matrix3d MeshShape::computeInertia(double _mass) const { } void MeshShape::computeVolume() { - // Use bounding box to represent the mesh - double l = mScale[0] * mBoundingBoxDim[0]; - double h = mScale[1] * mBoundingBoxDim[1]; - double w = mScale[2] * mBoundingBoxDim[2]; - - mVolume = l * h * w; + Eigen::Vector3d bounds = mBoundingBox.computeFullExtents(); + mVolume = bounds.x() * bounds.y() * bounds.z(); } void MeshShape::_updateBoundingBoxDim() { @@ -323,9 +320,8 @@ void MeshShape::_updateBoundingBoxDim() { min_Z = mMesh->mMeshes[i]->mVertices[j].z; } } - mBoundingBoxDim[0] = max_X - min_X; - mBoundingBoxDim[1] = max_Y - min_Y; - mBoundingBoxDim[2] = max_Z - min_Z; + mBoundingBox.setMin(Eigen::Vector3d(min_X, min_Y, min_Z)); + mBoundingBox.setMax(Eigen::Vector3d(max_X, max_Y, max_Z)); } const aiScene* MeshShape::loadMesh( diff --git a/dart/dynamics/Shape.cpp b/dart/dynamics/Shape.cpp index b5ea3e1ef0a1c..55d62777b14bc 100644 --- a/dart/dynamics/Shape.cpp +++ b/dart/dynamics/Shape.cpp @@ -43,7 +43,7 @@ namespace dart { namespace dynamics { //============================================================================== Shape::Shape(ShapeType _type) - : mBoundingBoxDim(0, 0, 0), + : mBoundingBox(), mVolume(0.0), mID(mCounter++), mColor(0.5, 0.5, 1.0, 1.0), @@ -102,9 +102,15 @@ void Shape::setAlpha(double _alpha) { } //============================================================================== -const Eigen::Vector3d& Shape::getBoundingBoxDim() const +const math::BoundingBox& Shape::getBoundingBox() const { - return mBoundingBoxDim; + return mBoundingBox; +} + +//============================================================================== +Eigen::Vector3d Shape::getBoundingBoxDim() const +{ + return mBoundingBox.computeFullExtents(); } //============================================================================== diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 2ab0f08ce10ac..aa51a9ddd955e 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -54,7 +54,6 @@ class RenderInterface; namespace dart { namespace dynamics { - /// \brief class Shape : public virtual common::Subject { @@ -114,6 +113,12 @@ class Shape : public virtual common::Subject /// \brief Set the transparency of this Shape virtual void setAlpha(double _alpha); + + /// \brief Get the bounding box of the shape in its local coordinate frame. + /// The dimension will be automatically determined by the sub-classes + /// such as BoxShape, EllipsoidShape, CylinderShape, and MeshShape. + const math::BoundingBox& getBoundingBox() const; + /// \brief Get dimensions of bounding box. /// The dimension will be automatically determined by the sub-classes /// such as BoxShape, EllipsoidShape, CylinderShape, and MeshShape. @@ -121,7 +126,7 @@ class Shape : public virtual common::Subject // biased mesh shape. Two Vector3ds might be better; one is for // minimum verterx, and the other is for maximum verterx of the // bounding box. - const Eigen::Vector3d& getBoundingBoxDim() const; + Eigen::Vector3d getBoundingBoxDim() const; /// \brief Set local transformation of the shape w.r.t. parent frame. void setLocalTransform(const Eigen::Isometry3d& _Transform); @@ -195,8 +200,8 @@ class Shape : public virtual common::Subject /// \brief virtual void initMeshes() {} - /// \brief Dimensions for bounding box. - Eigen::Vector3d mBoundingBoxDim; + /// \brief The bounding box (in the local coordinate frame) of the shape. + math::BoundingBox mBoundingBox; /// \brief Volume enclosed by the geometry. double mVolume; diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 87210bc445fd4..4fbf6f65e5183 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1951,5 +1951,17 @@ Eigen::Vector2d computeClosestPointOnSupportPolygon(size_t& _index1, size_t& _in return result; } +BoundingBox::BoundingBox() : + mMin(0, 0, 0), mMax(0, 0, 0) +{ + +} +BoundingBox::BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max) : + mMin(min), mMax(max) +{ + +} + + } // namespace math } // namespace dart diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h index f60581cc22ce4..57ba44f6c36f2 100644 --- a/dart/math/Geometry.h +++ b/dart/math/Geometry.h @@ -533,6 +533,35 @@ Eigen::Vector2d computeClosestPointOnSupportPolygon( const Eigen::Vector2d& _p, const SupportPolygon& _support); + +// Represents a bounding box with minimum and maximum coordinates. +class BoundingBox { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + BoundingBox(); + BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max); + + inline const Eigen::Vector3d& getMin() const { return mMin; } + inline const Eigen::Vector3d& getMax() const { return mMax; } + + inline void setMin(const Eigen::Vector3d& min) { mMin = min; } + inline void setMax(const Eigen::Vector3d& max) { mMax = max; } + + // \brief Centroid of the bounding box (i.e average of min and max) + inline Eigen::Vector3d computeCenter() const { return (mMax + mMin) * 0.5; } + // \brief Coordinates of the maximum corner with respect to the centroid. + inline Eigen::Vector3d computeHalfExtents() const { return (mMax - mMin) * 0.5; } + // \brief Length of each of the sides of the bounding box. + inline Eigen::Vector3d computeFullExtents() const { return (mMax - mMin); } + + protected: + // \brief minimum coordinates of the bounding box + Eigen::Vector3d mMin; + // \brief maximum coordinates of the bounding box + Eigen::Vector3d mMax; +}; + } // namespace math } // namespace dart From 71bcedc49e95fd2d4b4122a54185aabe0e0f47ea Mon Sep 17 00:00:00 2001 From: mklingen Date: Tue, 3 Nov 2015 15:16:56 -0500 Subject: [PATCH 14/79] forgot mesh scale --- dart/dynamics/MeshShape.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 7d4d164982648..f4485212e0790 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -320,8 +320,8 @@ void MeshShape::_updateBoundingBoxDim() { min_Z = mMesh->mMeshes[i]->mVertices[j].z; } } - mBoundingBox.setMin(Eigen::Vector3d(min_X, min_Y, min_Z)); - mBoundingBox.setMax(Eigen::Vector3d(max_X, max_Y, max_Z)); + mBoundingBox.setMin(Eigen::Vector3d(min_X * mScale[0], min_Y * mScale[1], min_Z * mScale[2])); + mBoundingBox.setMax(Eigen::Vector3d(max_X * mScale[0], max_Y * mScale[1], max_Z * mScale[2])); } const aiScene* MeshShape::loadMesh( From 55e1266f6ebb5c38135b6a7b314d26be5bd746d6 Mon Sep 17 00:00:00 2001 From: mklingen Date: Tue, 3 Nov 2015 15:18:02 -0500 Subject: [PATCH 15/79] mesh bounding box must change with scale --- dart/dynamics/MeshShape.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index f4485212e0790..377e41d218b2e 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -222,6 +222,7 @@ void MeshShape::setScale(const Eigen::Vector3d& _scale) { assert(_scale[2] > 0.0); mScale = _scale; computeVolume(); + _updateBoundingBoxDim(); } const Eigen::Vector3d& MeshShape::getScale() const { From 3f4a9db35b9258752849915e90a5bb9255ca7283 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 4 Nov 2015 00:53:02 -0500 Subject: [PATCH 16/79] Suppress deprecated-warning in Function.cpp For backward compatibility, we have to call deprecated functions in Function.cpp, and compilers complain for these calls. This commit suppresses the warnings only for those calls. See also #544. Note that the suppression code couldn't be made as macros. It needs to use compiler dependent #pragma directives but directives is not allowed to be decleared in macros. --- dart/config.h.in | 9 +++++++ dart/optimizer/Function.cpp | 51 +++++++++++++++++++++++++++++++++++-- 2 files changed, 58 insertions(+), 2 deletions(-) diff --git a/dart/config.h.in b/dart/config.h.in index 0128e0e5613ae..dfd5fb2ed7996 100644 --- a/dart/config.h.in +++ b/dart/config.h.in @@ -28,6 +28,15 @@ (DART_MAJOR_VERSION < x || (DART_MAJOR_VERSION <= x && \ (DART_MINOR_VERSION < y || (DART_MINOR_VERSION <= y)))) +// Detect the compiler +#if defined(__clang__) + #define DART_COMPILER_CLANG +#elif defined(__GNUC__) || defined(__GNUG__) + #define DART_COMPILER_GCC +#elif defined(_MSC_VER) + #define DART_COMPILER_MSVC +#endif + #cmakedefine BUILD_TYPE_DEBUG 1 #cmakedefine BUILD_TYPE_RELEASE 1 #cmakedefine BUILD_TYPE_RELWITHDEBINFO 1 diff --git a/dart/optimizer/Function.cpp b/dart/optimizer/Function.cpp index 0f0c3eb2171c0..5d36b7801d830 100644 --- a/dart/optimizer/Function.cpp +++ b/dart/optimizer/Function.cpp @@ -37,6 +37,7 @@ #include "dart/optimizer/Function.h" +#include "dart/config.h" #include "dart/common/Console.h" namespace dart { @@ -81,9 +82,32 @@ double Function::eval(Eigen::Map& _x) double Function::eval(const Eigen::VectorXd& _x) { // TODO(MXG): This is for backwards compatibility. This function should be - // made pure abstract with the next major version-up + // made pure abstract with the next major version-up. We suppress the + // deprecated-warnings until then (see #544). Eigen::Map temp(_x.data(), _x.size()); + +#if defined (DART_COMPILER_GCC) + + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return eval(temp); + #pragma GCC diagnostic pop + +#elif defined (DART_COMPILER_CLANG) + + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wdeprecated-declarations" return eval(temp); + #pragma clang diagnostic pop + +#elif defined (DART_COMPILER_MSVC) + + #pragma warning( push ) + #pragma warning( disable : 4996 ) + return eval(temp); + #pragma warning( pop ) + +#endif } //============================================================================== @@ -101,9 +125,32 @@ void Function::evalGradient(Eigen::Map& _x, void Function::evalGradient(const Eigen::VectorXd& _x, Eigen::Map _grad) { - // TODO(MXG): This is for backwards compatibility + // TODO(MXG): This is for backwards compatibility. We suppress the + // deprecated-warnings until then (see #544). Eigen::Map temp(_x.data(), _x.size()); + +#if defined (DART_COMPILER_GCC) + + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + evalGradient(temp, _grad); + #pragma GCC diagnostic pop + +#elif defined (DART_COMPILER_CLANG) + + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wdeprecated-declarations" evalGradient(temp, _grad); + #pragma clang diagnostic pop + +#elif defined (DART_COMPILER_MSVC) + + #pragma warning( push ) + #pragma warning( disable : 4996 ) + evalGradient(temp, _grad); + #pragma warning( pop ) + +#endif } //============================================================================== From c629eddacced5bac035ea89ac0b2190b499993de Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 4 Nov 2015 12:39:19 -0500 Subject: [PATCH 17/79] Add deprecated-warning suppress macros for reusability --- dart/common/Deprecated.h | 44 ++++++++++++++++++++++++++++++++++ dart/optimizer/Function.cpp | 47 ++++--------------------------------- 2 files changed, 48 insertions(+), 43 deletions(-) diff --git a/dart/common/Deprecated.h b/dart/common/Deprecated.h index cc433979d78c1..263103485b6d2 100644 --- a/dart/common/Deprecated.h +++ b/dart/common/Deprecated.h @@ -34,6 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "dart/config.h" + #ifndef DART_COMMON_DEPRECATED_H_ #define DART_COMMON_DEPRECATED_H_ @@ -54,4 +56,46 @@ #define FORCEINLINE #endif +// We define two convenient macros that can be used to suppress +// deprecated-warnings for a specific code block rather than a whole project. +// This macros would be useful when you need to call deprecated function for +// some reason (e.g., for backward compatibility) but don't want warnings. +// +// Example code: +// +// deprecated_function() // warning +// +// DART_SUPPRESS_DEPRECATED_BEGIN +// deprecated_function() // okay, no warning +// DART_SUPPRESS_DEPRECATED_END +// +#if defined (DART_COMPILER_GCC) + + #define DART_SUPPRESS_DEPRECATED_BEGIN \ + _Pragma("GCC diagnostic push") \ + _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") + + #define DART_SUPPRESS_DEPRECATED_END \ + _Pragma("GCC diagnostic pop") + +#elif defined (DART_COMPILER_CLANG) + + #define DART_SUPPRESS_DEPRECATED_BEGIN \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"-Wdeprecated-declarations\"") + + #define DART_SUPPRESS_DEPRECATED_END \ + _Pragma("clang diagnostic pop") + +#elif defined (DART_COMPILER_MSVC) + + #define DART_SUPPRESS_DEPRECATED_BEGIN \ + __pragma(warning(push)) \ + __pragma(warning(disable:4996)) + + #define DART_SUPPRESS_DEPRECATED_END \ + __pragma(warning(pop)) + +#endif + #endif // DART_COMMON_DEPRECATED_H_ diff --git a/dart/optimizer/Function.cpp b/dart/optimizer/Function.cpp index 5d36b7801d830..83e8530f65585 100644 --- a/dart/optimizer/Function.cpp +++ b/dart/optimizer/Function.cpp @@ -37,7 +37,6 @@ #include "dart/optimizer/Function.h" -#include "dart/config.h" #include "dart/common/Console.h" namespace dart { @@ -86,28 +85,9 @@ double Function::eval(const Eigen::VectorXd& _x) // deprecated-warnings until then (see #544). Eigen::Map temp(_x.data(), _x.size()); -#if defined (DART_COMPILER_GCC) - - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return eval(temp); - #pragma GCC diagnostic pop - -#elif defined (DART_COMPILER_CLANG) - - #pragma clang diagnostic push - #pragma clang diagnostic ignored "-Wdeprecated-declarations" + DART_SUPPRESS_DEPRECATED_BEGIN return eval(temp); - #pragma clang diagnostic pop - -#elif defined (DART_COMPILER_MSVC) - - #pragma warning( push ) - #pragma warning( disable : 4996 ) - return eval(temp); - #pragma warning( pop ) - -#endif + DART_SUPPRESS_DEPRECATED_END } //============================================================================== @@ -129,28 +109,9 @@ void Function::evalGradient(const Eigen::VectorXd& _x, // deprecated-warnings until then (see #544). Eigen::Map temp(_x.data(), _x.size()); -#if defined (DART_COMPILER_GCC) - - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wdeprecated-declarations" - evalGradient(temp, _grad); - #pragma GCC diagnostic pop - -#elif defined (DART_COMPILER_CLANG) - - #pragma clang diagnostic push - #pragma clang diagnostic ignored "-Wdeprecated-declarations" + DART_SUPPRESS_DEPRECATED_BEGIN evalGradient(temp, _grad); - #pragma clang diagnostic pop - -#elif defined (DART_COMPILER_MSVC) - - #pragma warning( push ) - #pragma warning( disable : 4996 ) - evalGradient(temp, _grad); - #pragma warning( pop ) - -#endif + DART_SUPPRESS_DEPRECATED_END } //============================================================================== From 827a4492703615c062d358f17bbb936dc5534a2e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 4 Nov 2015 13:02:27 -0500 Subject: [PATCH 18/79] Update style and comment --- CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 238d5f5a0489a..773967400cdab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,9 +169,9 @@ if(ASSIMP_FOUND) ASSIMP_AISCENE_CTOR_DTOR_DEFINED) if(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) - message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} has missing symbols " - "for constructor or destructor of aiScene. DART will use own " - "implementation. Please use ASSIMP that fixed this issue.") + message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} is missing symbols " + "for constructor or destructor of aiScene. DART will use own " + "implementation. Please use ASSIMP that fixed this issue.") endif(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) check_cxx_source_compiles( @@ -187,9 +187,9 @@ if(ASSIMP_FOUND) ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) if(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) - message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} has missing symbols " - "for constructor or destructor of aiMaterial. DART will use own " - "implementation. Please use ASSIMP that fixed this issue.") + message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} is missing symbols " + "for constructor or destructor of aiMaterial. DART will use own " + "implementation. Please use ASSIMP that fixed this issue.") endif(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) unset(CMAKE_REQUIRED_INCLUDES) From 7969f2d4825826244427379cad3a502893aefa0a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 5 Nov 2015 12:34:51 -0500 Subject: [PATCH 19/79] Update boost paths --- appveyor.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 69e229bbc1dbd..f1fad0c2eac35 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -12,8 +12,8 @@ platform: # specify custom environment variables environment: MSVC_DEFAULT_OPTIONS: ON - BOOST_ROOT: C:\Libraries\boost - BOOST_LIBRARYDIR: C:\Libraries\boost\stage\lib + BOOST_ROOT: "C:\\Libraries\\boost" + BOOST_LIBRARYDIR: "C:\\Libraries\\boost\\lib32-msvc-12.0" BUILD_EXAMPLES: OFF # don't build examples to not exceed allowed build time (40 min) BUILD_TUTORIALS: OFF # don't build tutorials to not exceed allowed build time (40 min) matrix: From 49b8d7bc42e8c0309a08168d3f534780ebc6e90b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 5 Nov 2015 15:08:21 -0500 Subject: [PATCH 20/79] Use consistent path style --- appveyor.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index f1fad0c2eac35..38f6ebdbe3460 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -12,8 +12,8 @@ platform: # specify custom environment variables environment: MSVC_DEFAULT_OPTIONS: ON - BOOST_ROOT: "C:\\Libraries\\boost" - BOOST_LIBRARYDIR: "C:\\Libraries\\boost\\lib32-msvc-12.0" + BOOST_ROOT: C:\Libraries\boost + BOOST_LIBRARYDIR: C:\Libraries\boost\lib32-msvc-12.0 BUILD_EXAMPLES: OFF # don't build examples to not exceed allowed build time (40 min) BUILD_TUTORIALS: OFF # don't build tutorials to not exceed allowed build time (40 min) matrix: @@ -44,7 +44,7 @@ branches: # scripts that run after cloning repository install: - - ps: cd "C:\projects\dart\ci" + - ps: cd C:\projects\dart\ci - ps: .\appveyor_install.ps1 # scripts to run before build From 5c5c5afb603e07a0e173a6e231c9a9183eed4c3f Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 5 Nov 2015 17:24:14 -0500 Subject: [PATCH 21/79] removed unused variable --- osgDart/examples/osgHuboPuppet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/osgDart/examples/osgHuboPuppet.cpp b/osgDart/examples/osgHuboPuppet.cpp index abd67f8dd0b10..1bbd795bb4901 100644 --- a/osgDart/examples/osgHuboPuppet.cpp +++ b/osgDart/examples/osgHuboPuppet.cpp @@ -804,7 +804,7 @@ class TeleoperationWorld : public osgDart::WorldNode mHubo->getJoint(0)->setPositions(FreeJoint::convertToPositions(new_tf)); } - bool solved = mHubo->getIK(true)->solve(); + mHubo->getIK(true)->solve(); } bool mAmplifyMovement; From 0ab5d773d4fd824cf7c5f6ee3fec5c5cbb0175e5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 5 Nov 2015 22:30:46 -0500 Subject: [PATCH 22/79] Update changelogs for 5.1.1 --- Changelog.md | 11 ++++++++++- debian/changelog | 5 ++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/Changelog.md b/Changelog.md index 2625e946724c1..c0f6952c8c7aa 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,11 +1,20 @@ -### Version 5.1.1 (2015-11-02) +### Version 5.1.1 (2015-11-06) 1. Add bullet dependency to package.xml * [Pull request #523](https://github.com/dartsim/dart/pull/523) +1. Improved handling of missing symbols of Assimp package + * [Pull request #542](https://github.com/dartsim/dart/pull/542) + 1. Improved travis-ci build log for Mac * [Pull request #529](https://github.com/dartsim/dart/pull/529) +1. Fixed warnings in Function.cpp + * [Pull request #550](https://github.com/dartsim/dart/pull/550) + +1. Fixed build failures on AppVeyor + * [Pull request #543](https://github.com/dartsim/dart/pull/543) + 1. Fixed const qualification of ResourceRetriever * [Pull request #534](https://github.com/dartsim/dart/pull/534) * [Issue #532](https://github.com/dartsim/dart/issues/532) diff --git a/debian/changelog b/debian/changelog index 1a2ad08973ed7..02b9cfe984ccf 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,12 +1,15 @@ dart (5.1.1) unstable; urgency=medium * Add bullet dependency to package.xml + * Improved handling of missing symbols of Assimp package * Improved travis-ci build log for Mac + * Fixed warnings in Function.cpp + * Fixed build failures on AppVeyor * Fixed const qualification of ResourceRetriever * Fixed aligned memory allocation with Eigen objects * Fixed copy safety for various classes - -- Jeongseok Lee Mon, 02 Nov 2015 00:00:00 -0500 + -- Jeongseok Lee Mon, 06 Nov 2015 00:00:00 -0500 dart (5.1.0) unstable; urgency=medium From b961d29cbce2bf7231c0cd2612146a47885215fe Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 5 Nov 2015 22:34:30 -0500 Subject: [PATCH 23/79] Update control file for Debian packaging --- debian/control | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/debian/control b/debian/control index 3938bf469407d..5f94f1a88134c 100644 --- a/debian/control +++ b/debian/control @@ -15,7 +15,9 @@ Build-Depends: debhelper (>= 9), liburdfdom-dev, libboost-dev (>= 1.54.0.1ubuntu1), libboost-system-dev (>= 1.54.0-4ubuntu3), - libboost-regex-dev (>= 1.54.0-4ubuntu3) + libboost-regex-dev (>= 1.54.0-4ubuntu3), + libopenthreads-dev, + libopenscenegraph-dev Standards-Version: 3.9.6 Section: libs Homepage: http://dartsim.github.io/ @@ -25,14 +27,14 @@ Vcs-Browser: https://github.com/dartsim/dart Package: libdart-core5-dev Section: libdevel Architecture: any -Pre-Depends: multiarch-support +Pre-Depends: ${misc:Pre-Depends} Conflicts: libdart-core3-dev, libdart-core4-dev Depends: ${misc:Depends}, libdart-core5.1 (= ${binary:Version}), libeigen3-dev, libassimp-dev (>= 3), libfcl-dev, - libboost-dev (>= 1.54.0.1ubuntu1) + libboost-all-dev (>= 1.54.0.1ubuntu1) Description: Dynamic Animation and Robotics Toolkit, core development files DART is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data @@ -60,14 +62,16 @@ Description: Dynamic Animation and Robotics Toolkit, core development files Package: libdart5-dev Section: libdevel Architecture: any -Pre-Depends: multiarch-support +Pre-Depends: ${misc:Pre-Depends} Depends: ${misc:Depends}, libdart-core5-dev, libdart5.1 (= ${binary:Version}), freeglut3-dev, libxi-dev, libxmu-dev, - libtinyxml2-dev + libtinyxml2-dev, + libopenthreads-dev, + libopenscenegraph-dev Description: Dynamic Animation and Robotics Toolkit, development files DART is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data @@ -96,7 +100,7 @@ Description: Dynamic Animation and Robotics Toolkit, development files Package: libdart-core5.1 Section: libs Architecture: any -Pre-Depends: multiarch-support +Pre-Depends: ${misc:Pre-Depends} Depends: ${misc:Depends}, ${shlibs:Depends} Description: Dynamic Animation and Robotics Toolkit, core library files @@ -126,7 +130,7 @@ Description: Dynamic Animation and Robotics Toolkit, core library files Package: libdart5.1 Section: libs Architecture: any -Pre-Depends: multiarch-support +Pre-Depends: ${misc:Pre-Depends} Depends: ${misc:Depends}, ${shlibs:Depends} Description: Dynamic Animation and Robotics Toolkit, library files From a5eda7ea4188337041c8329a1d019a1a0a5f618e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 10 Nov 2015 12:06:48 -0500 Subject: [PATCH 24/79] use std::fmod instead of our own implementation --- dart/math/Geometry.h | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h index bd292e9c079d8..0025514c6b352 100644 --- a/dart/math/Geometry.h +++ b/dart/math/Geometry.h @@ -428,20 +428,11 @@ bool verifyRotation(const Eigen::Matrix3d& _R); /// all the elements are not NaN values. bool verifyTransform(const Eigen::Isometry3d& _T); -/// Get the remainder of dividing x by y -inline double mod(double x, double y) -{ - if( 0.0 == y ) - return x; - - return x - y * floor(x/y); -} - /// Compute the angle (in the range of -pi to +pi) which ignores any full /// rotations inline double wrapToPi(double angle) { - return mod(angle+M_PI, 2*M_PI) - M_PI; + return std::fmod(angle+M_PI, 2*M_PI) - M_PI; } template From 1048a0657ca5a60272edc40c96037741e8a920fb Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 10 Nov 2015 12:22:50 -0500 Subject: [PATCH 25/79] improved comments --- dart/dynamics/InverseKinematics.h | 12 ++++++++---- dart/dynamics/detail/InverseKinematics.h | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index 707efc0a95eb8..f9c28bb55ce6a 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -581,7 +581,11 @@ class InverseKinematics : public common::Subject /// Creating an Analytical solver will have the side effect of removing the /// error clamp and error weights from your ErrorMethod. If you still want /// your error computations to be clamped and weighted, you should set it - /// again after creating the Analytical solver. + /// again after creating the Analytical solver. Clamping and weighting the + /// error vector often helps iterative methods to converge smoothly, but it is + /// counter-productive for analytical methods which do not typically rely on + /// convergence; analytical methods can usually solve the entire error vector + /// directly. class Analytical : public GradientMethod { public: @@ -590,9 +594,9 @@ class InverseKinematics : public common::Subject /// solution produced by the analytical IK. enum Validity_t { - VALID = 0, - OUT_OF_REACH = 1 << 0, - LIMIT_VIOLATED = 1 << 1 + VALID = 0, ///< The solution is completely valid and reaches the target + OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target + LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions that violate the joint limits }; /// If there are extra DOFs in the IK module which your Analytical solver diff --git a/dart/dynamics/detail/InverseKinematics.h b/dart/dynamics/detail/InverseKinematics.h index abd2fd87980ca..273b19c3d31b6 100644 --- a/dart/dynamics/detail/InverseKinematics.h +++ b/dart/dynamics/detail/InverseKinematics.h @@ -79,7 +79,7 @@ void InverseKinematics::setDofs(const std::vector& _dofs) setDofs(indices); } -} // namespace dart } // namespace dynamics +} // namespace dart #endif // DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_H_ From b66b6794865783ae703fb2d1f0e2462750b6186f Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 10 Nov 2015 12:24:37 -0500 Subject: [PATCH 26/79] use .empty() since it may perform better --- dart/dynamics/InverseKinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 2bb3122e6766c..03481d013985d 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -1056,7 +1056,7 @@ void InverseKinematics::Analytical::computeGradient( getSolutions(desiredTf); - if(mSolutions.size() == 0) + if(mSolutions.empty()) return; const Eigen::VectorXd& bestSolution = mSolutions[0].mConfig; From e65a82a167e21ab450ac5337b1e7350d494305d6 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 10 Nov 2015 12:29:41 -0500 Subject: [PATCH 27/79] added clarifying comment --- dart/dynamics/InverseKinematics.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index f9c28bb55ce6a..a0a41ad97324e 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -780,7 +780,9 @@ class InverseKinematics : public common::Subject /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if /// any components of their configuration are outside of their position - /// limits. + /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of + /// mSolutions which are already tagged with it, even if they do not violate + /// any limits. void checkSolutionJointLimits(); /// Vector of solutions From 1a750fd6665c95566a6fd1392594c63d8bbce061 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 10 Nov 2015 12:59:36 -0500 Subject: [PATCH 28/79] improved wording of the cmake warning messages for ASSIMP --- CMakeLists.txt | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b48ef4c546f74..632b026481199 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -170,9 +170,11 @@ if(ASSIMP_FOUND) ASSIMP_AISCENE_CTOR_DTOR_DEFINED) if(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) - message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} is missing symbols " - "for constructor or destructor of aiScene. DART will use own " - "implementation. Please use ASSIMP that fixed this issue.") + message(WARNING "The installed version of ASSIMP (${ASSIMP_VERSION}) is " + "missing symbols for the constructor and/or destructor of " + "aiScene. DART will use its own implementations of these " + "functions. We recommend using a version of ASSIMP that " + "does not have this issue.") endif(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) check_cxx_source_compiles( @@ -188,9 +190,11 @@ if(ASSIMP_FOUND) ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) if(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) - message(WARNING "Installed ASSIMP ${ASSIMP_VERSION} is missing symbols " - "for constructor or destructor of aiMaterial. DART will use own " - "implementation. Please use ASSIMP that fixed this issue.") + message(WARNING "The installed version of ASSIMP (${ASSIMP_VERSION}) is " + "missing symbols for the constructor and/or destructor of " + "aiMaterial. DART will use its own implementations of " + "these functions. We recommend using a version of ASSIMP " + "that does not have this issue.") endif(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) unset(CMAKE_REQUIRED_INCLUDES) From 71b04561554f3ddfe8241d8e269759db12965490 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 11 Nov 2015 14:34:08 -0500 Subject: [PATCH 29/79] fixing missing definitions --- dart/constraint/BalanceConstraint.cpp | 6 ++++++ dart/constraint/BalanceConstraint.h | 2 +- dart/constraint/ConstraintSolver.h | 3 --- dart/renderer/Camera.h | 18 +++++++++--------- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/dart/constraint/BalanceConstraint.cpp b/dart/constraint/BalanceConstraint.cpp index 8b80326748887..30f4f5bb5a8bc 100644 --- a/dart/constraint/BalanceConstraint.cpp +++ b/dart/constraint/BalanceConstraint.cpp @@ -381,6 +381,12 @@ void BalanceConstraint::setPseudoInverseDamping(double _damping) clearCaches(); } +//============================================================================== +double BalanceConstraint::getPseudoInverseDamping() const +{ + return mDamping; +} + //============================================================================== const Eigen::Vector3d& BalanceConstraint::getLastError() const { diff --git a/dart/constraint/BalanceConstraint.h b/dart/constraint/BalanceConstraint.h index ca4ee4af81a70..49b6afe81239f 100644 --- a/dart/constraint/BalanceConstraint.h +++ b/dart/constraint/BalanceConstraint.h @@ -134,7 +134,7 @@ class BalanceConstraint : public optimizer::Function, void setPseudoInverseDamping(double _damping); /// Get the damping factor that will be used when computing the pseudoinverse - double getPseudoInverseDamping(); + double getPseudoInverseDamping() const; /// Get the last error vector that was computed by this BalanceConstraint const Eigen::Vector3d& getLastError() const; diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index b6acac0bd2d3d..e1ae909797c0d 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -98,9 +98,6 @@ class ConstraintSolver /// Add a constraint void addConstraint(ConstraintBase* _constraint); - /// Return the number of constraints - size_t getNumConstraints() const; - /// Remove a constraint void removeConstraint(ConstraintBase* _constraint); diff --git a/dart/renderer/Camera.h b/dart/renderer/Camera.h index e173c6e9cbd06..f826a2e87d416 100644 --- a/dart/renderer/Camera.h +++ b/dart/renderer/Camera.h @@ -55,17 +55,17 @@ class Camera { public: Camera() {} virtual ~Camera() {} - virtual void set(const Eigen::Vector3d& _eye, const Eigen::Vector3d& _look, const Eigen::Vector3d& _up); - virtual void slide(double _delX, double _delY, double _delZ, bool _bLocal = false); - virtual void setFrustum(float _vAng, float _asp, float _nearD, float _farD); - virtual void setOrtho(float _width, float _height, float _nearD, float _farD); + virtual void set(const Eigen::Vector3d& _eye, const Eigen::Vector3d& _look, const Eigen::Vector3d& _up) = 0; + virtual void slide(double _delX, double _delY, double _delZ, bool _bLocal = false) = 0; + virtual void setFrustum(float _vAng, float _asp, float _nearD, float _farD) = 0; + virtual void setOrtho(float _width, float _height, float _nearD, float _farD) = 0; - virtual void roll(float _angle); - virtual void pitch(float _angle); - virtual void yaw(float _angle); - virtual void localRotate(float _angle, AXIS _axis); - virtual void globalRotate(float _angle, AXIS _axis); + virtual void roll(float _angle) = 0; + virtual void pitch(float _angle) = 0; + virtual void yaw(float _angle) = 0; + virtual void localRotate(float _angle, AXIS _axis) = 0; + virtual void globalRotate(float _angle, AXIS _axis) = 0; virtual Eigen::Vector3d getEye(void) const { return Eigen::Vector3d(0.0f, 0.0f, 1.0f); From 105a0735aeb5b2609d960d38ab3df7cef6fd5df6 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 13:45:34 -0500 Subject: [PATCH 30/79] allow BodyNodes and DOFs to be uncoupled in Groups --- dart/dynamics/Group.cpp | 2 +- dart/dynamics/Linkage.cpp | 4 +- dart/dynamics/ReferentialSkeleton.cpp | 139 +++++++++++++++++++------- dart/dynamics/ReferentialSkeleton.h | 35 +++++-- unittests/testSkeleton.cpp | 17 ++++ 5 files changed, 146 insertions(+), 51 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index f9c551cf2ea30..c9b0e56bef78b 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -209,7 +209,7 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) return false; } - unregisterBodyNode(_bn); + unregisterBodyNode(_bn, false); return true; } diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 8da04a5797cf7..18b2f83781a54 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -517,11 +517,11 @@ void Linkage::satisfyCriteria() { std::vector bns = mCriteria.satisfy(); while(getNumBodyNodes() > 0) - unregisterBodyNode(mBodyNodes.back()); + unregisterComponent(mBodyNodes.back()); for(BodyNode* bn : bns) { - registerBodyNode(bn); + registerComponent(bn); } update(); diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index b5f8666631195..718b546972701 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -955,13 +955,44 @@ math::LinearJacobian ReferentialSkeleton::getCOMLinearJacobianDeriv( } //============================================================================== -void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) +void ReferentialSkeleton::registerComponent(BodyNode* _bn) { + registerBodyNode(_bn); + size_t nDofs = _bn->getParentJoint()->getNumDofs(); for(size_t i=0; i < nDofs; ++i) - { registerDegreeOfFreedom(_bn->getParentJoint()->getDof(i)); +} + +//============================================================================== +void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) +{ + std::unordered_map::iterator it = + mIndexMap.find(_bn); + + if( it == mIndexMap.end() ) + { + // Create an index map entry for this BodyNode, and only add the BodyNode's + // index to it. + IndexMap indexing; + + mBodyNodes.push_back(_bn); + indexing.mBodyNodeIndex = mBodyNodes.size()-1; + + mIndexMap[_bn] = indexing; } + else + { + IndexMap& indexing = it->second; + + if(INVALID_INDEX == indexing.mBodyNodeIndex) + { + mBodyNodes.push_back(_bn); + indexing.mBodyNodeIndex = mBodyNodes.size()-1; + } + } + + updateCaches(); } //============================================================================== @@ -975,9 +1006,9 @@ void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) if( it == mIndexMap.end() ) { + // Create an index map entry for this DegreeOfFreedom, and only add the + // DegreeOfFreedom's index to it IndexMap indexing; - mBodyNodes.push_back(bn); - indexing.mBodyNodeIndex = mBodyNodes.size()-1; indexing.mDofIndices.resize(localIndex+1, INVALID_INDEX); mDofs.push_back(_dof); @@ -988,17 +1019,29 @@ void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) else { IndexMap& indexing = it->second; + if(indexing.mDofIndices.size() < localIndex+1) indexing.mDofIndices.resize(localIndex+1, INVALID_INDEX); - mDofs.push_back(_dof); - indexing.mDofIndices[localIndex] = mDofs.size()-1; + + if(INVALID_INDEX == indexing.mDofIndices[localIndex]) + { + mDofs.push_back(_dof); + indexing.mDofIndices[localIndex] = mDofs.size()-1; + } } updateCaches(); } //============================================================================== -void ReferentialSkeleton::unregisterBodyNode(BodyNode* _bn) +void ReferentialSkeleton::unregisterComponent(BodyNode* _bn) +{ + unregisterBodyNode(_bn, true); +} + +//============================================================================== +void ReferentialSkeleton::unregisterBodyNode( + BodyNode* _bn, bool _unregisterDofs) { if(nullptr == _bn) { @@ -1022,27 +1065,37 @@ void ReferentialSkeleton::unregisterBodyNode(BodyNode* _bn) return; } - const IndexMap& indexing = it->second; + IndexMap& indexing = it->second; + size_t bnIndex = indexing.mBodyNodeIndex; + mBodyNodes.erase(mBodyNodes.begin() + bnIndex); + indexing.mBodyNodeIndex = INVALID_INDEX; - for(size_t i=0; isecond.mDofIndices[_localIndex] == INVALID_INDEX) { dterr << "[ReferentialSkeleton::unregisterDegreeOfFreedom] Attempting to " - << "unregister a DegreeOfFreedom from a BodyNode named [" - << _bn->getName() << "] (" << _bn << ") that is not currently in the " - << "ReferentialSkeleton! This is most likely a bug. Please report " - << "this!\n"; + << "unregister DegreeOfFreedom #" << _localIndex << " of a BodyNode " + << "named [" << _bn->getName() << "] (" << _bn << "), but it is not " + << "currently in the ReferentialSkeleton! This is most likely a bug. " + << "Please report this!\n"; assert(false); return; } @@ -1075,27 +1128,15 @@ void ReferentialSkeleton::unregisterDegreeOfFreedom( for(size_t i = dofIndex; i < mDofs.size(); ++i) { + // Re-index all the DOFs in this ReferentialSkeleton which came after the + // DOF that was removed. DegreeOfFreedomPtr dof = mDofs[i]; IndexMap& indexing = mIndexMap[dof.getBodyNodePtr()]; indexing.mDofIndices[dof.getLocalIndex()] = i; } - if(removeBnIfEmpty) - { - const std::vector& dofIndices = it->second.mDofIndices; - bool removeBn = true; - for(size_t i=0; isecond.isExpired()) + mIndexMap.erase(it); updateCaches(); } @@ -1142,5 +1183,27 @@ void ReferentialSkeleton::updateCaches() mFc = Eigen::VectorXd::Zero(nDofs); } +//============================================================================== +ReferentialSkeleton::IndexMap::IndexMap() + : mBodyNodeIndex(INVALID_INDEX) +{ + // Do nothing +} + +//============================================================================== +bool ReferentialSkeleton::IndexMap::isExpired() const +{ + if(mBodyNodeIndex != INVALID_INDEX) + return false; + + for(size_t i=0; i < mDofIndices.size(); ++i) + { + if(mDofIndices[i] != INVALID_INDEX) + return false; + } + + return true; +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index 1d06219474d9c..3f141cf02d0d7 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -327,21 +327,29 @@ class ReferentialSkeleton : public MetaSkeleton /// of ReferentialSkeleton. ReferentialSkeleton() = default; - /// Add a BodyNode to this ReferentialSkeleton. Only usable by derived classes + /// Add a BodyNode, along with its parent Joint and parent DegreesOfFreedom + /// to this ReferentialSkeleton. This can only be used by derived classes. + void registerComponent(BodyNode* _bn); + + /// Add a BodyNode to this ReferentialSkeleton, ignoring its DegreesOfFreedom. + /// This can only be used by derived classes. void registerBodyNode(BodyNode* _bn); - /// Add a DegreeOfFreedom to this ReferentialSkeleton. Only usable by - /// derived classes. + /// Add a DegreeOfFreedom to this ReferentialSkeleton. This can only be used + /// by derived classes. void registerDegreeOfFreedom(DegreeOfFreedom* _dof); - /// Completely remove a BodyNode from this ReferentialSkeleton. Only usable - /// by derived classes - void unregisterBodyNode(BodyNode* _bn); + /// Completely remove a BodyNode and its parent DegreesOfFreedom from this + /// ReferentialSkeleton. This can only be used by derived classes. + void unregisterComponent(BodyNode* _bn); + + /// Remove a BodyNode from this ReferentialSkeleton, ignoring its parent + /// DegreesOfFreedom. This can only be used by derived classes. + void unregisterBodyNode(BodyNode* _bn, bool _unregisterDofs); - /// Remove a DegreeOfFreedom from this ReferentialSkeleton. Only usable by - /// derived classes. - void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex, - bool removeBnIfEmpty = true); + /// Remove a DegreeOfFreedom from this ReferentialSkeleton. This can only be + /// used by derived classes. + void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex); /// Update the caches to match any changes to the structure of this /// ReferentialSkeleton @@ -359,6 +367,13 @@ class ReferentialSkeleton : public MetaSkeleton /// Indices of the DegreesOfFreedom std::vector mDofIndices; + + /// Default constructor. Initializes mBodyNodeIndex to INVALID_INDEX + IndexMap(); + + /// Returns true if nothing in this entry is mapping to a valid index any + /// longer. + bool isExpired() const; }; /// Name of this ReferentialSkeleton diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index 29cff58d6235b..b42afe63f03b0 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -623,6 +623,23 @@ TEST(Skeleton, NodePersistence) } } +TEST(Skeleton, ZeroDofJointInReferential) +{ + // This is a regression test which makes sure that the BodyNodes of + // ZeroDofJoints will be correctly included in linkages. + SkeletonPtr skel = Skeleton::create(); + + BodyNode* bn = skel->createJointAndBodyNodePair().second; + BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; + bn = skel->createJointAndBodyNodePair(zeroDof1).second; + BodyNode* zeroDof2 = skel->createJointAndBodyNodePair(bn).second; + + BranchPtr branch = Branch::create(skel->getBodyNode(0)); + EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes()); + EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX); + EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX); +} + TEST(Skeleton, Referential) { std::vector skeletons = getSkeletons(); From fa37f1c68066733e5b35828032b4589620dc5145 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 15:08:19 -0500 Subject: [PATCH 31/79] updated the API of Group and changed some of its behavior --- dart/dynamics/Group.cpp | 104 +++++++++++++++++++++++++++++++++------- dart/dynamics/Group.h | 90 ++++++++++++++++++++++++---------- 2 files changed, 151 insertions(+), 43 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index c9b0e56bef78b..188d0a100a921 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -166,7 +166,71 @@ void Group::swapDofIndices(size_t _index1, size_t _index2) } //============================================================================== -void Group::addBodyNode(BodyNode* _bn, bool _warning) +bool Group::addComponent(BodyNode* _bn, bool _warning) +{ + bool added = false; + + added |= addBodyNode(_bn, false); + + for(size_t i=0; i < _bn->getParentJoint()->getNumDofs(); ++i) + added |= addDof(_bn->getParentJoint()->getDof(i), false); + + if(_warning && !added) + { + dtwarn << "[Group::addComponent] The BodyNode named [" << _bn->getName() + << "] (" << _bn << ") and all of its parent DegreesOfFreedom are " + << "already in the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return added; +} + +//============================================================================== +bool Group::addComponents(const std::vector& _bodyNodes, + bool _warning) +{ + bool added = false; + for(BodyNode* bn : _bodyNodes) + added |= addComponent(bn, _warning); + + return added; +} + +//============================================================================== +bool Group::removeComponent(BodyNode* _bn, bool _warning) +{ + bool removed = false; + + removed |= removeBodyNode(_bn, false); + + for(size_t i=0; i < _bn->getParentJoint()->getNumDofs(); ++i) + removed |= removeDof(_bn->getParentJoint()->getDof(i), false); + + if(_warning && !removed) + { + dtwarn << "[Group::removeComponent] The BodyNode named [" << _bn->getName() + << "] (" << _bn << ") and its parent DegreesOfFreedom were not in " + << "the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return removed; +} + +//============================================================================== +bool Group::removeComponents(const std::vector& _bodyNodes, + bool _warning) +{ + bool removed = false; + for(BodyNode* bn : _bodyNodes) + removed |= removeComponent(bn, _warning); + + return removed; +} + +//============================================================================== +bool Group::addBodyNode(BodyNode* _bn, bool _warning) { if(INVALID_INDEX != getIndexOf(_bn, false)) { @@ -176,21 +240,24 @@ void Group::addBodyNode(BodyNode* _bn, bool _warning) << "] (" << _bn << ") is already in the Group [" << getName() << "] (" << this << ")\n"; assert(false); - return; } - return; + return false; } registerBodyNode(_bn); + return true; } //============================================================================== -void Group::addBodyNodes(const std::vector& _bodyNodes, +bool Group::addBodyNodes(const std::vector& _bodyNodes, bool _warning) { + bool added = false; for(BodyNode* bn : _bodyNodes) - addBodyNode(bn, _warning); + added |= addBodyNode(bn, _warning); + + return added; } //============================================================================== @@ -210,7 +277,6 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) } unregisterBodyNode(_bn, false); - return true; } @@ -218,15 +284,15 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) bool Group::removeBodyNodes(const std::vector& _bodyNodes, bool _warning) { - bool allGood = true; + bool removed = false; for(BodyNode* bn : _bodyNodes) - allGood &= removeBodyNode(bn, _warning); + removed |= removeBodyNode(bn, _warning); - return allGood; + return removed; } //============================================================================== -void Group::addDof(DegreeOfFreedom* _dof, bool _warning) +bool Group::addDof(DegreeOfFreedom* _dof, bool _warning) { if(INVALID_INDEX != getIndexOf(_dof, false)) { @@ -236,20 +302,23 @@ void Group::addDof(DegreeOfFreedom* _dof, bool _warning) << "] (" << _dof << ") is already in the Group [" << getName() << "] (" << this << ")\n"; assert(false); - return; } - return; + return false; } registerDegreeOfFreedom(_dof); + return true; } //============================================================================== -void Group::addDofs(const std::vector& _dofs, bool _warning) +bool Group::addDofs(const std::vector& _dofs, bool _warning) { + bool added = false; for(DegreeOfFreedom* dof : _dofs) - addDof(dof, _warning); + added |= addDof(dof, _warning); + + return added; } //============================================================================== @@ -269,7 +338,6 @@ bool Group::removeDof(DegreeOfFreedom* _dof, bool _warning) } unregisterDegreeOfFreedom(_dof->getChildBodyNode(), _dof->getIndexInJoint()); - return true; } @@ -277,11 +345,11 @@ bool Group::removeDof(DegreeOfFreedom* _dof, bool _warning) bool Group::removeDofs(const std::vector& _dofs, bool _warning) { - bool allGood = true; + bool removed = false; for(DegreeOfFreedom* dof : _dofs) - allGood &= removeDof(dof, _warning); + removed |= removeDof(dof, _warning); - return allGood; + return removed; } //============================================================================== diff --git a/dart/dynamics/Group.h b/dart/dynamics/Group.h index ec26e9fabdabb..86ced7a11e2da 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -65,60 +65,100 @@ class Group : public ReferentialSkeleton /// Swap the index of DegreeOfFreedom _index1 with _index2 void swapDofIndices(size_t _index1, size_t _index2); + /// Add a BodyNode and its parent DegreesOfFreedom to this Group. If _warning + /// is true, you will be warned when the BodyNode and all its DegreesOfFreedom + /// were already in the Group, and an assertion will be thrown. + /// + /// This function will return false if the BodyNode and all its + /// DegreesOfFreedom were already in the Group. + bool addComponent(BodyNode* _bn, bool _warning=true); + + /// Add set of BodyNodes and their parent DegreesOfFreedom to this Group. If + /// _warning is true, you will be warned when an entire component was already + /// in the Group, and an assertion will be thrown. + /// + /// This function will return false if all of the components in the set were + /// already in this Group. + bool addComponents(const std::vector& _bodyNodes, + bool _warning=true); + + /// Remove a BodyNode and its parent DegreesOfFreedom from this Group. If + /// _warning is true, you will be warned if this Group does not have the + /// BodyNode or any of its DegreesOfFreedom, and an assertion will be thrown. + /// + /// This function will return false if the Group did not include the BodyNode + /// or any of its parent DegreesOfFreedom. + bool removeComponent(BodyNode* _bn, bool _warning=true); + + /// Remove a set of BodyNodes and their parent DegreesOfFreedom from this + /// Group. If _warning is true, you will be warned if any of the components + /// were completely missing from this Group, and an assertion will be thrown. + /// + /// This function will return false if none of the components in this set were + /// in the Group. + bool removeComponents(const std::vector& _bodyNodes, + bool _warning=true); + /// Add a BodyNode to this Group. If _warning is true, you will be warned when - /// you attempt to add the same BodyNode twice, and assertion will be thrown. - void addBodyNode(BodyNode* _bn, bool _warning=true); + /// you attempt to add the same BodyNode twice, and an assertion will be + /// thrown. + /// + /// This function will return false if the BodyNode was already in the Group. + bool addBodyNode(BodyNode* _bn, bool _warning=true); /// Add a set of BodyNodes to this Group. If _warning is true, you will be /// warned when you attempt to add the same BodyNode twice, and an assertion /// will be thrown. - void addBodyNodes(const std::vector& _bodyNodes, + /// + /// This function will return false if all of the BodyNodes were already in + /// the Group. + bool addBodyNodes(const std::vector& _bodyNodes, bool _warning=true); - /// Remove a BodyNode from this Group. Note: All DegreesOfFreedom belonging to - /// this BodyNode will also be removed. If _warning is true, you will be - /// warned when you attempt to remove BodyNode that is not in this Group, and - /// an assertion will be thrown. + /// Remove a BodyNode from this Group. If _warning is true, you will be warned + /// when you attempt to remove a BodyNode that is not in this Group, and an + /// assertion will be thrown. /// - /// The function will return false if the BodyNode was not already in this - /// Group. + /// The function will return false if the BodyNode was not in this Group. bool removeBodyNode(BodyNode* _bn, bool _warning=true); - /// Remove a set of BodyNodes from this Group. Note: All DegreesOfFreedom - /// belonging to each BodyNode will also be removed. If _warning is true, you - /// will be warned when you attempt to remove a BodyNode that is not in this - /// Group, and an assertion will be thrown. + /// Remove a set of BodyNodes from this Group. If _warning is true, you will + /// be warned when you attempt to remove a BodyNode that is not in this Group, + /// and an assertion will be thrown. /// - /// The function will return false if one of the BodyNodes was not already in - /// this Group. + /// The function will return false if none of the BodyNodes were in this Group. bool removeBodyNodes(const std::vector& _bodyNodes, bool _warning=true); - /// Add a DegreeOfFreedom to this Group. Note: The BodyNode of this - /// DegreeOfFreedom will also be added. If _warning is true, you will be + /// Add a DegreeOfFreedom to this Group. If _warning is true, you will be /// warned when you attempt to add the same DegreeOfFreedom twice, and an /// assertion will be thrown. - void addDof(DegreeOfFreedom* _dof, bool _warning=true); + /// + /// This function will return false if the DegreeOfFreedom was already in the + /// Group. + bool addDof(DegreeOfFreedom* _dof, bool _warning=true); - /// Add a set of DegreesOfFreedom to this Group. Note: The BodyNodes of these - /// DegreesOfFreedom will also be added. If _warning is true, you will be - /// warned when you attempt to add the same DegreeOfFreedom twice, and an + /// Add a set of DegreesOfFreedom to this Group. If _warning is true, you will + /// be warned when you attempt to add the same DegreeOfFreedom twice, and an /// assertion will be thrown. - void addDofs(const std::vector& _dofs, bool _warning=true); + /// + /// This function will return false if all of the DegreesOfFreedom was already + /// in the Group. + bool addDofs(const std::vector& _dofs, bool _warning=true); /// Remove a DegreeOfFreedom from this Group. If _warning is true, you will be /// warned when you attempt to remove a DegreeOfFreedom that is not in this /// Group, and an assertion will be thrown. /// - /// This function will return false if the DegreeOfFreedom was not already in - /// this Group. + /// This function will return false if the DegreeOfFreedom was not in this + /// Group. bool removeDof(DegreeOfFreedom* _dof, bool _warning=true); /// Remove a set of DegreesOfFreedom from this Group. If _warning is true, you /// will be warned when you attempt to remove a DegreeOfFreedom that is not /// in this Group, and an assertion will be thrown. /// - /// This function will return false if the DegreeOfFreedom was not alraedy in + /// This function will return false if none of the DegreesOfFreedom were in /// this Group. bool removeDofs(const std::vector& _dofs, bool _warning=true); From 934448c56d41fde1c1202bae8971b71e8dad1370 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 15:32:00 -0500 Subject: [PATCH 32/79] updated the constructors for Group --- dart/dynamics/Group.cpp | 51 ++++++++++++++++++++++++++++++++++++----- dart/dynamics/Group.h | 31 +++++++++++++++++++------ 2 files changed, 69 insertions(+), 13 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 188d0a100a921..24623b0e48060 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -45,18 +45,29 @@ namespace dynamics { //============================================================================== GroupPtr Group::create(const std::string& _name, - const std::vector& _bodyNodes) + const std::vector& _bodyNodes, + bool _includeDofs) { - GroupPtr group(new Group(_name, _bodyNodes)); + GroupPtr group(new Group(_name, _bodyNodes, _includeDofs)); group->mPtr = group; return group; } //============================================================================== GroupPtr Group::create(const std::string& _name, - const std::vector& _dofs) + const std::vector& _dofs, + bool _includeBodyNodes) { - GroupPtr group(new Group(_name, _dofs)); + GroupPtr group(new Group(_name, _dofs, _includeBodyNodes)); + group->mPtr = group; + return group; +} + +//============================================================================== +GroupPtr Group::create(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton) +{ + GroupPtr group(new Group(_name, _metaSkeleton)); group->mPtr = group; return group; } @@ -354,18 +365,46 @@ bool Group::removeDofs(const std::vector& _dofs, //============================================================================== Group::Group(const std::string& _name, - const std::vector& _bodyNodes) + const std::vector& _bodyNodes, bool _includeDofs) { setName(_name); addBodyNodes(_bodyNodes); + + if(_includeDofs) + { + for(size_t i=0; i < _bodyNodes.size(); ++i) + { + Joint* joint = _bodyNodes[i]->getParentJoint(); + for(size_t j=0; j < joint->getNumDofs(); ++j) + addDof(joint->getDof(j)); + } + } } //============================================================================== Group::Group(const std::string& _name, - const std::vector& _dofs) + const std::vector& _dofs, + bool _includeBodyNodes) { setName(_name); addDofs(_dofs); + + if(_includeBodyNodes) + { + for(size_t i=0; i < _dofs.size(); ++i) + { + DegreeOfFreedom* dof = _dofs[i]; + addBodyNode(dof->getChildBodyNode(), false); + } + } +} + +//============================================================================== +Group::Group(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton) +{ + setName(_name); + addComponents(_metaSkeleton->getBodyNodes()); } } // namespace dynamics diff --git a/dart/dynamics/Group.h b/dart/dynamics/Group.h index 86ced7a11e2da..56c8ed15c5c98 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -46,15 +46,26 @@ class Group : public ReferentialSkeleton { public: - /// Create a Group out of a set of BodyNodes + /// Create a Group out of a set of BodyNodes. If _includeDofs is true, then + /// the parent DegreesOfFreedom of each BodyNode will also be added to the + /// Group. static GroupPtr create( const std::string& _name = "Group", - const std::vector& _bodyNodes = std::vector()); + const std::vector& _bodyNodes = std::vector(), + bool _includeDofs = true); + + /// Create a Group out of a set of DegreesOfFreedom. If _includeBodyNodes is + /// true, then the child BodyNode of each DegreeOfFreedom will also be added + /// to the Group. + static GroupPtr create( + const std::string& _name, + const std::vector& _dofs, + bool _includeBodyNodes = true); - /// Create a Group out of a set of DegreesOfFreedom + /// Create a Group that mimics the given MetaSkeleton static GroupPtr create( const std::string& _name, - const std::vector& _dofs); + const MetaSkeletonPtr& _metaSkeleton); /// Destructor virtual ~Group() = default; @@ -165,12 +176,18 @@ class Group : public ReferentialSkeleton protected: /// Default constructor - Group(const std::string& _name = "Group", - const std::vector& _bodyNodes = std::vector()); + Group(const std::string& _name, + const std::vector& _bodyNodes, + bool _includeDofs); /// Alternative constructor Group(const std::string& _name, - const std::vector& _dofs); + const std::vector& _dofs, + bool _includeBodyNodes); + + /// MetaSkeleton-based constructor + Group(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton); }; } // dynamics From 1a5356dffb9b46b4deade850c903c7d4b0d29369 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 16:16:31 -0500 Subject: [PATCH 33/79] changed function names to match DART style --- dart/utils/C3D.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/dart/utils/C3D.cpp b/dart/utils/C3D.cpp index f6951e59ba5d4..94f8e345aa377 100644 --- a/dart/utils/C3D.cpp +++ b/dart/utils/C3D.cpp @@ -46,7 +46,7 @@ namespace dart { namespace utils { -float ConvertDecToFloat(char _bytes[4]) { +float convertDecToFloat(char _bytes[4]) { union { char theChars[4]; float theFloat; @@ -61,7 +61,7 @@ float ConvertDecToFloat(char _bytes[4]) { } -void ConvertFloatToDec(float _f, char* _bytes) { +void convertFloatToDec(float _f, char* _bytes) { char* p = (char*)&_f; _bytes[0] = p[2]; _bytes[1] = p[3]; @@ -107,8 +107,8 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* //convert if in dec format if (bDecFmt) { - hdr.freq = ConvertDecToFloat((char*)&hdr.freq); - hdr.scale = ConvertDecToFloat((char*)&hdr.scale); + hdr.freq = convertDecToFloat((char*)&hdr.freq); + hdr.scale = convertDecToFloat((char*)&hdr.scale); } int numFrames = hdr.end_frame - hdr.start_frame + 1; @@ -149,9 +149,9 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* if (c3dScale < 0) { memcpy(&frame, buf, sizeof(frame)); if(bDecFmt){ - frame.y = ConvertDecToFloat((char*)&frame.y); - frame.z = ConvertDecToFloat((char*)&frame.z); - frame.x = ConvertDecToFloat((char*)&frame.x); + frame.y = convertDecToFloat((char*)&frame.y); + frame.z = convertDecToFloat((char*)&frame.z); + frame.x = convertDecToFloat((char*)&frame.x); } v[0] = frame.y / 1000.0; v[1] = frame.z / 1000.0; @@ -159,9 +159,9 @@ bool loadC3DFile(const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int* } else { memcpy(&frameSI, buf, sizeof(frameSI)); if (bDecFmt) { - frameSI.y = (short)ConvertDecToFloat((char*)&frameSI.y); - frameSI.z = (short)ConvertDecToFloat((char*)&frameSI.z); - frameSI.x = (short)ConvertDecToFloat((char*)&frameSI.x); + frameSI.y = (short)convertDecToFloat((char*)&frameSI.y); + frameSI.z = (short)convertDecToFloat((char*)&frameSI.z); + frameSI.x = (short)convertDecToFloat((char*)&frameSI.x); } v[0] = (float)frameSI.y * pntScale / 1000.0; v[1] = (float)frameSI.z * pntScale / 1000.0; From 4dfe4b5735f171cb11ed3ba8919ecbb54df77ee0 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 16:16:47 -0500 Subject: [PATCH 34/79] removed unnecessary functions --- dart/utils/C3D.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/dart/utils/C3D.h b/dart/utils/C3D.h index a7f190de2c7e0..64bc8f8510e2e 100644 --- a/dart/utils/C3D.h +++ b/dart/utils/C3D.h @@ -90,9 +90,6 @@ bool loadC3DFile( const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, bool saveC3DFile( const char* _fileName, Eigen::EIGEN_VV_VEC3D& _pointData, int _nFrame, int _nMarker, double _freq ); -double maxElem(std::vector& _arr, int& _index); -double minElem(std::vector& _arr, int& _index); - } // namespace utils } // namespace dart From f1808c8c2024c5c28d92cd98f7990ae5709c0666 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 16 Nov 2015 16:30:41 -0500 Subject: [PATCH 35/79] removed Camera and Light classes, because they were unused --- dart/renderer/Camera.h | 108 ------------------------ dart/renderer/Light.h | 77 ----------------- dart/renderer/OpenGLRenderInterface.cpp | 13 --- dart/renderer/OpenGLRenderInterface.h | 4 - dart/renderer/RenderInterface.cpp | 26 ------ dart/renderer/RenderInterface.h | 14 --- 6 files changed, 242 deletions(-) delete mode 100644 dart/renderer/Camera.h delete mode 100644 dart/renderer/Light.h diff --git a/dart/renderer/Camera.h b/dart/renderer/Camera.h deleted file mode 100644 index f826a2e87d416..0000000000000 --- a/dart/renderer/Camera.h +++ /dev/null @@ -1,108 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jie (Jay) Tan - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * 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. - * 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. - */ - -#ifndef DART_RENDERER_CAMERA_H -#define DART_RENDERER_CAMERA_H - -#include - -namespace dart { -namespace renderer { - -#define DEFAULT_NEAR_PLANE 0.001 -#define DEFAULT_FAR_PLANE 15 - -enum AXIS { - AXIS_X, - AXIS_Y, - AXIS_Z -}; - -class Camera { -public: - Camera() {} - virtual ~Camera() {} - virtual void set(const Eigen::Vector3d& _eye, const Eigen::Vector3d& _look, const Eigen::Vector3d& _up) = 0; - virtual void slide(double _delX, double _delY, double _delZ, bool _bLocal = false) = 0; - virtual void setFrustum(float _vAng, float _asp, float _nearD, float _farD) = 0; - virtual void setOrtho(float _width, float _height, float _nearD, float _farD) = 0; - - - virtual void roll(float _angle) = 0; - virtual void pitch(float _angle) = 0; - virtual void yaw(float _angle) = 0; - virtual void localRotate(float _angle, AXIS _axis) = 0; - virtual void globalRotate(float _angle, AXIS _axis) = 0; - - virtual Eigen::Vector3d getEye(void) const { - return Eigen::Vector3d(0.0f, 0.0f, 1.0f); - } - virtual Eigen::Vector3d getLookAtDir(void) const { - return Eigen::Vector3d(0.0f, 0.0f, 0.0f); - } - virtual Eigen::Vector3d getUpDir(void) const { - return Eigen::Vector3d(0.0f, 1.0f, 0.0f); - - } - virtual bool isOrthogonal(void) const { - return mIsOrthognal; - } - - virtual float getVerticalViewAngle(void) const { - return mViewAngle; - } - virtual float getNearDistance() const { - return mNearDist; - } - virtual float getFarDistance() const { - return mFarDist; - } - virtual float getWidth() const { - return mWidth; - } - virtual float getHeight() const { - return mHeight; - } - -protected: - float mViewAngle, mAspect, mNearDist, mFarDist, mWidth, mHeight; - bool mIsOrthognal; -}; - -} // namespace renderer -} // namespace dart - -#endif // #ifndef DART_RENDERER_CAMERA_H diff --git a/dart/renderer/Light.h b/dart/renderer/Light.h deleted file mode 100644 index 60ad936356b4f..0000000000000 --- a/dart/renderer/Light.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jie (Jay) Tan - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * 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. - * 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. - */ - -#ifndef DART_RENDERER_LIGHT_H -#define DART_RENDERER_LIGHT_H - -#include - -namespace dart { -namespace renderer { - -enum LightType { - LT_PointLight, - LT_DirectionLight, -}; - -class Light { -public: - /* construction function */ - Light(); - Light(LightType type); - Light(const Eigen::Vector3d& _pos, const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, LightType _type); - - LightType GetType() const; - void SetPosition(const Eigen::Vector3d& _p); - void GetPosition(float* _pos) const; - void GetSpecular(float* _specular) const; - void GetDiffuse(float* _diffuse) const; - Eigen::Vector3d GetPosition() const; - Eigen::Vector3d GetSpecular() const; - Eigen::Vector3d GetDiffuse() const; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - private: - Eigen::Vector3d mPosition; - Eigen::Vector3d mSpecular; - Eigen::Vector3d mDiffuse; - LightType mType; -}; - -} // namespace renderer -} // namespace dart - -#endif // #ifndef DART_RENDERER_LIGHT_H diff --git a/dart/renderer/OpenGLRenderInterface.cpp b/dart/renderer/OpenGLRenderInterface.cpp index dbe670adc1a05..799c5b57c4ada 100644 --- a/dart/renderer/OpenGLRenderInterface.cpp +++ b/dart/renderer/OpenGLRenderInterface.cpp @@ -103,19 +103,6 @@ void OpenGLRenderInterface::clear(const Eigen::Vector3d& _color) { glClear(GL_COLOR_BUFFER_BIT); } -void OpenGLRenderInterface::setDefaultLight() { - -} - -void OpenGLRenderInterface::turnLightsOff() { - glDisable(GL_LIGHTING); -} - -void OpenGLRenderInterface::turnLightsOn() { - //not finished yet - glEnable(GL_LIGHTING); -} - void OpenGLRenderInterface::setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow) { } diff --git a/dart/renderer/OpenGLRenderInterface.h b/dart/renderer/OpenGLRenderInterface.h index 8b58a9fb84b9f..a3ee38ec7d7e7 100644 --- a/dart/renderer/OpenGLRenderInterface.h +++ b/dart/renderer/OpenGLRenderInterface.h @@ -65,10 +65,6 @@ class OpenGLRenderInterface : public RenderInterface { virtual void clear(const Eigen::Vector3d& _color) override; - virtual void setDefaultLight() override; - virtual void turnLightsOff() override; - virtual void turnLightsOn() override; - virtual void setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow) override; virtual void getMaterial(Eigen::Vector3d& _diffuse, Eigen::Vector3d& _specular, double& _cosinePow) const override; virtual void setDefaultMaterial() override; diff --git a/dart/renderer/RenderInterface.cpp b/dart/renderer/RenderInterface.cpp index 1258d02a6293f..cd1441d61fb2e 100644 --- a/dart/renderer/RenderInterface.cpp +++ b/dart/renderer/RenderInterface.cpp @@ -59,28 +59,6 @@ void RenderInterface::clear(const Eigen::Vector3d& _color) { } -void RenderInterface::setDefaultLight() -{ -} - -void RenderInterface::addLight(Light* _light) -{ - mLightList.push_back(_light); -} - -void RenderInterface::eraseAllLights() -{ - mLightList.clear(); -} - -void RenderInterface::turnLightsOff() -{ -} - -void RenderInterface::turnLightsOn() -{ -} - void RenderInterface::setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow) { @@ -181,9 +159,5 @@ void RenderInterface::readFrameBuffer(DecoBufferType _buffType, DecoColorChannel } -Camera* RenderInterface::getCamera(){ - return mCamera; -} - } // namespace renderer } // namespace dart diff --git a/dart/renderer/RenderInterface.h b/dart/renderer/RenderInterface.h index c00e37fc579e3..7c5e2c13ca82e 100644 --- a/dart/renderer/RenderInterface.h +++ b/dart/renderer/RenderInterface.h @@ -38,8 +38,6 @@ #define DART_RENDERER_RENDERINTERFACE_H #include -#include "Light.h" -#include "Camera.h" #include #include @@ -83,12 +81,6 @@ class RenderInterface { virtual void clear(const Eigen::Vector3d& _color); - virtual void setDefaultLight(); - virtual void addLight(Light* _light); - virtual void eraseAllLights(); - virtual void turnLightsOff(); - virtual void turnLightsOn(); - virtual void setMaterial(const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, double _cosinePow); virtual void getMaterial(Eigen::Vector3d& _diffuse, Eigen::Vector3d& _specular, double& _cosinePow) const; virtual void setDefaultMaterial(); @@ -120,12 +112,6 @@ class RenderInterface { virtual void saveToImage(const char* _filename, DecoBufferType _buffType = BT_Back); virtual void readFrameBuffer(DecoBufferType _buffType, DecoColorChannel _ch, void* _pixels); - - virtual Camera* getCamera(); - -protected: - Camera* mCamera; - std::vector mLightList; }; } // namespace renderer From e5aed46d5f17119188c52fa6a3708d3b2238519c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 18 Nov 2015 02:08:07 -0500 Subject: [PATCH 36/79] decoupled Joints from BodyNodes for the Group class --- dart/dynamics/Group.cpp | 270 ++++++++++++++++++++++++-- dart/dynamics/Group.h | 79 ++++++-- dart/dynamics/ReferentialSkeleton.cpp | 113 +++++++++-- dart/dynamics/ReferentialSkeleton.h | 23 ++- dart/dynamics/detail/JointPtr.h | 6 + unittests/testSkeleton.cpp | 24 +++ 6 files changed, 465 insertions(+), 50 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 24623b0e48060..634b9b1b0413a 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -179,6 +179,18 @@ void Group::swapDofIndices(size_t _index1, size_t _index2) //============================================================================== bool Group::addComponent(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::addComponent] Attempting to add a nullptr component " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + bool added = false; added |= addBodyNode(_bn, false); @@ -211,6 +223,19 @@ bool Group::addComponents(const std::vector& _bodyNodes, //============================================================================== bool Group::removeComponent(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::removeComponent] Attempting to remove a nullptr " + << "component from the Group [" << getName() << "] (" << this + << ")\n"; + assert(false); + } + + return false; + } + bool removed = false; removed |= removeBodyNode(_bn, false); @@ -243,6 +268,18 @@ bool Group::removeComponents(const std::vector& _bodyNodes, //============================================================================== bool Group::addBodyNode(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::addBodyNode] Attempting to add a nullptr BodyNode " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + if(INVALID_INDEX != getIndexOf(_bn, false)) { if(_warning) @@ -274,6 +311,19 @@ bool Group::addBodyNodes(const std::vector& _bodyNodes, //============================================================================== bool Group::removeBodyNode(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::removeBodyNode] Attempting to remove a nullptr " + << "BodyNode from the Group [" << getName() << "] (" << this + << ")\n"; + assert(false); + } + + return false; + } + if(INVALID_INDEX == getIndexOf(_bn, false)) { if(_warning) @@ -303,62 +353,248 @@ bool Group::removeBodyNodes(const std::vector& _bodyNodes, } //============================================================================== -bool Group::addDof(DegreeOfFreedom* _dof, bool _warning) +bool Group::addJoint(Joint* _joint, bool _addDofs, bool _warning) { - if(INVALID_INDEX != getIndexOf(_dof, false)) + if(nullptr == _joint) { if(_warning) { - dtwarn << "[Group::addDof] The DegreeOfFreedom named [" << _dof->getName() - << "] (" << _dof << ") is already in the Group [" << getName() + dtwarn << "[Group::addJoint] Attempting to add a nullptr Joint to the " + << "Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + bool added = false; + if(INVALID_INDEX == getIndexOf(_joint)) + { + registerJoint(_joint); + added = true; + } + + if(_addDofs) + { + for(size_t i=0; i < _joint->getNumDofs(); ++i) + added |= addDof(_joint->getDof(i), false, false); + } + + if(!added && _warning) + { + if(_addDofs) + { + dtwarn << "[Group::addJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") and all its DOFs are already in the " + << "Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::addJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") is already in the Group [" << getName() << "] (" << this << ")\n"; assert(false); } + } + + return added; +} + +//============================================================================== +bool Group::addJoints(const std::vector& _joints, + bool _addDofs, bool _warning) +{ + bool added = false; + for(Joint* joint : _joints) + added |= addJoint(joint, _addDofs, _warning); + + return added; +} + +//============================================================================== +bool Group::removeJoint(Joint* _joint, bool _removeDofs, bool _warning) +{ + if(nullptr == _joint) + { + if(_warning) + { + dtwarn << "[Group::removeJoint] Attempting to remove a nullptr Joint " + << "from the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } return false; } - registerDegreeOfFreedom(_dof); - return true; + // Make sure the Joint continues to exist for the duration of this scope + JointPtr hold(_joint); + + bool removed = false; + if(INVALID_INDEX != getIndexOf(_joint, false)) + { + unregisterJoint(_joint->getChildBodyNode()); + removed = true; + } + + if(_removeDofs) + { + for(size_t i=0; i < _joint->getNumDofs(); ++i) + removed |= removeDof(_joint->getDof(i), false, false); + } + + if(!removed && _warning) + { + if(_removeDofs) + { + dtwarn << "[Group::removeJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") and its DOFs were NOT in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::removeJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") was NOT in the Group [" << getName() + << "] (" << this << ")\n"; + assert(false); + } + } + + return removed; +} + +//============================================================================== +bool Group::addDof(DegreeOfFreedom* _dof, bool _addJoint, bool _warning) +{ + if(nullptr == _dof) + { + if(_warning) + { + dtwarn << "[Group::addDof] Attempting to add a nullptr DegreeOfFreedom " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + bool added = false; + if(INVALID_INDEX == getIndexOf(_dof, false)) + { + registerDegreeOfFreedom(_dof); + added = true; + } + + if(_addJoint) + added |= addJoint(_dof->getJoint(), false, false); + + if(!added && _warning) + { + if(_addJoint) + { + dtwarn << "[Group::addDof] The DegreeOfFreedom named [" << _dof->getName() + << "] (" << _dof << ") and its Joint are already in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::addDof] The DegreeOfFreedom named [" << _dof->getName() + << "] (" << _dof << ") is already in the Group [" << getName() + << "] (" << this << ")\n"; + assert(false); + } + } + + return added; } //============================================================================== -bool Group::addDofs(const std::vector& _dofs, bool _warning) +bool Group::addDofs(const std::vector& _dofs, + bool _addJoint, bool _warning) { bool added = false; for(DegreeOfFreedom* dof : _dofs) - added |= addDof(dof, _warning); + added |= addDof(dof, _addJoint, _warning); return added; } //============================================================================== -bool Group::removeDof(DegreeOfFreedom* _dof, bool _warning) +bool Group::removeDof(DegreeOfFreedom* _dof, bool _cleanupJoint, bool _warning) { - if(INVALID_INDEX == getIndexOf(_dof, false)) + if(nullptr == _dof) { if(_warning) { - dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" - << _dof->getName() << "] (" << _dof << ") is NOT in the Group [" - << getName() << "] (" << this << ")\n"; + dtwarn << "[Group::removeDof] Attempting to remove a nullptr " + << "DegreeOfFreedom from the Group [" << getName() << "] (" + << this << ")\n"; assert(false); } return false; } - unregisterDegreeOfFreedom(_dof->getChildBodyNode(), _dof->getIndexInJoint()); - return true; + // Make sure the DegreeOfFreedom continues to exist for the duration of this + // scope + DegreeOfFreedomPtr hold(_dof); + + bool removed = false; + if(INVALID_INDEX != getIndexOf(_dof, false)) + { + unregisterDegreeOfFreedom(_dof->getChildBodyNode(), _dof->getIndexInJoint()); + removed = true; + } + + if(_cleanupJoint) + { + // Check whether any DOFs belonging to the Joint are remaining in the Group + bool performCleanup = true; + Joint* joint = _dof->getJoint(); + for(size_t i=0; i < joint->getNumDofs(); ++i) + { + if(getIndexOf(joint->getDof(i)) == INVALID_INDEX) + { + performCleanup = false; + break; + } + } + + // Remove the Joint if none of its DOFs remain + if(performCleanup) + removed |= removeJoint(joint, false, false); + } + + if(!removed && _warning) + { + if(_cleanupJoint) + { + dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" + << _dof->getName() << "] (" << _dof << ") and its Joint were NOT " + << "in the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" + << _dof->getName() << "] (" << _dof << ") was NOT in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + } + + return removed; } //============================================================================== bool Group::removeDofs(const std::vector& _dofs, - bool _warning) + bool _cleanupJoint, bool _warning) { bool removed = false; for(DegreeOfFreedom* dof : _dofs) - removed |= removeDof(dof, _warning); + removed |= removeDof(dof, _cleanupJoint, _warning); return removed; } diff --git a/dart/dynamics/Group.h b/dart/dynamics/Group.h index 56c8ed15c5c98..b47fbcd3eb852 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -118,8 +118,8 @@ class Group : public ReferentialSkeleton bool addBodyNode(BodyNode* _bn, bool _warning=true); /// Add a set of BodyNodes to this Group. If _warning is true, you will be - /// warned when you attempt to add the same BodyNode twice, and an assertion - /// will be thrown. + /// warned when you attempt to add a BodyNode that is already in the Group, + /// and an assertion will be thrown. /// /// This function will return false if all of the BodyNodes were already in /// the Group. @@ -141,38 +141,87 @@ class Group : public ReferentialSkeleton bool removeBodyNodes(const std::vector& _bodyNodes, bool _warning=true); - /// Add a DegreeOfFreedom to this Group. If _warning is true, you will be + /// Add a Joint to this Group. If _addDofs is true, it will also add all the + /// DegreesOfFreedom of the Joint. If _warning is true, you will be warned + /// if the Joint (and all its DOFs if _addDofs is set to true) was already in + /// the Group, and an assertion will be thrown. + /// + /// This function will return false if the Joint (and all its DOFs, if + /// _addDofs is true) was already in the Group. + bool addJoint(Joint* _joint, bool _addDofs=true, bool _warning=true); + + /// Add a set of Joints to this Group. If _addDofs is true, it will also add + /// all the DOFs of each Joint. If _warning is true, you will be warned when + /// you attempt to add a Joint that is already in the Group (and all its DOFs + /// are in the Group, if _addDofs is set to true), and an assertion will be + /// thrown. + /// + /// This function will return false if all the Joints (and their DOFs if + /// _addDofs is set to true) were already in the Group. + bool addJoints(const std::vector& _joints, bool _addDofs=true, + bool _warning=true); + + /// Remove a Joint from this Group. If _removeDofs is true, it will also + /// remove any DOFs belonging to this Joint. If _warning is true, you will + /// be warned if you attempt to remove a Joint which is not in this Group (and + /// none of its DOFs are in the Group if _removeDofs is set to true), and an + /// assertion will be thrown. + /// + /// This function will return false if the Joint was not in the Group (and + /// neither were any of its DOFs, if _removeDofs is set to true). + bool removeJoint(Joint* _joint, bool _removeDofs=true, bool _warning=true); + + /// Remove a set of Joints from this Group. If _removeDofs is true, it will + /// also remove any DOFs belonging to any of the Joints. If _warning is true, + /// you will be warned if you attempt to remove a Joint which is not in this + /// Group (and none of its DOFs are in the Group if _removeDofs is set to + /// true), and an assertion will be thrown. + /// + /// This function will return false if none of the Joints (nor any of their + /// DOFs if _removeDofs is set to true) were in the Group. + bool removeJoints(const std::vector& _joints, bool _removeDofs=true, + bool _warning=true); + + /// Add a DegreeOfFreedom to this Group. If _addJoint is true, the Joint of + /// this DOF will also be added to the Group. If _warning is true, you will be /// warned when you attempt to add the same DegreeOfFreedom twice, and an /// assertion will be thrown. /// /// This function will return false if the DegreeOfFreedom was already in the /// Group. - bool addDof(DegreeOfFreedom* _dof, bool _warning=true); + bool addDof(DegreeOfFreedom* _dof, bool _addJoint=true, bool _warning=true); - /// Add a set of DegreesOfFreedom to this Group. If _warning is true, you will - /// be warned when you attempt to add the same DegreeOfFreedom twice, and an - /// assertion will be thrown. + /// Add a set of DegreesOfFreedom to this Group. If _addJoint is true, the + /// Joint of each DOF will also be added to the Group. If _warning is true, + /// you will be warned when you attempt to add the same DegreeOfFreedom twice, + /// and an assertion will be thrown. /// /// This function will return false if all of the DegreesOfFreedom was already /// in the Group. - bool addDofs(const std::vector& _dofs, bool _warning=true); + bool addDofs(const std::vector& _dofs, + bool _addJoint = true, bool _warning=true); - /// Remove a DegreeOfFreedom from this Group. If _warning is true, you will be + /// Remove a DegreeOfFreedom from this Group. If _cleanupJoint is true, the + /// Joint of this DOF will be removed, but only if no other DOFs belonging to + /// the Joint are remaining in the Group. If _warning is true, you will be /// warned when you attempt to remove a DegreeOfFreedom that is not in this /// Group, and an assertion will be thrown. /// /// This function will return false if the DegreeOfFreedom was not in this /// Group. - bool removeDof(DegreeOfFreedom* _dof, bool _warning=true); - - /// Remove a set of DegreesOfFreedom from this Group. If _warning is true, you - /// will be warned when you attempt to remove a DegreeOfFreedom that is not - /// in this Group, and an assertion will be thrown. + bool removeDof(DegreeOfFreedom* _dof, bool _cleanupJoint=true, + bool _warning=true); + + /// Remove a set of DegreesOfFreedom from this Group. If _cleanupJoint is + /// true, the Joint of this DOF will be removed, but only if no other DOFs + /// belonging to the Joint are remaining in the Group. If _warning is true, + /// you will be warned when you attempt to remove a DegreeOfFreedom that is + /// not in this Group, and an assertion will be thrown. /// /// This function will return false if none of the DegreesOfFreedom were in /// this Group. bool removeDofs(const std::vector& _dofs, - bool _warning=true); + bool _cleanupJoint=true, bool _warning=true); protected: /// Default constructor diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 718b546972701..476ba48edc99e 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -151,27 +151,19 @@ size_t ReferentialSkeleton::getIndexOf(const BodyNode* _bn, bool _warning) const //============================================================================== size_t ReferentialSkeleton::getNumJoints() const { - return mBodyNodes.size(); + return mJoints.size(); } //============================================================================== Joint* ReferentialSkeleton::getJoint(size_t _idx) { - BodyNode* bn = getVectorObjectIfAvailable(_idx, mBodyNodes); - if(nullptr == bn) - return nullptr; - - return bn->getParentJoint(); + return getVectorObjectIfAvailable(_idx, mJoints); } //============================================================================== const Joint* ReferentialSkeleton::getJoint(size_t _idx) const { - const BodyNode* bn = getVectorObjectIfAvailable(_idx, mBodyNodes); - if(nullptr == bn) - return nullptr; - - return bn->getParentJoint(); + return getVectorObjectIfAvailable(_idx, mJoints); } //============================================================================== @@ -202,7 +194,7 @@ size_t ReferentialSkeleton::getIndexOf(const Joint* _joint, bool _warning) const return INVALID_INDEX; } - return it->second.mBodyNodeIndex; + return it->second.mJointIndex; } //============================================================================== @@ -958,6 +950,7 @@ math::LinearJacobian ReferentialSkeleton::getCOMLinearJacobianDeriv( void ReferentialSkeleton::registerComponent(BodyNode* _bn) { registerBodyNode(_bn); + registerJoint(_bn->getParentJoint()); size_t nDofs = _bn->getParentJoint()->getNumDofs(); for(size_t i=0; i < nDofs; ++i) @@ -995,6 +988,42 @@ void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) updateCaches(); } +//============================================================================== +void ReferentialSkeleton::registerJoint(Joint* _joint) +{ + BodyNode* bn = _joint->getChildBodyNode(); + + std::unordered_map::iterator it = + mIndexMap.find(bn); + + if( it == mIndexMap.end() ) + { + // Create an index map entry for this Joint, and only add the Joint's index + // to it + IndexMap indexing; + + mJoints.push_back(_joint); + indexing.mJointIndex = mJoints.size()-1; + + mIndexMap[bn] = indexing; + } + else + { + IndexMap& indexing = it->second; + + if(INVALID_INDEX == indexing.mJointIndex) + { + mJoints.push_back(_joint); + indexing.mJointIndex = mJoints.size()-1; + } + } + + // Updating the caches isn't necessary after registering a joint right now, + // but it might matter in the future, so it might be better to be safe than + // sorry. + updateCaches(); +} + //============================================================================== void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) { @@ -1037,6 +1066,7 @@ void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) void ReferentialSkeleton::unregisterComponent(BodyNode* _bn) { unregisterBodyNode(_bn, true); + unregisterJoint(_bn); } //============================================================================== @@ -1093,6 +1123,57 @@ void ReferentialSkeleton::unregisterBodyNode( updateCaches(); } +//============================================================================== +void ReferentialSkeleton::unregisterJoint(BodyNode* _child) +{ + if(nullptr == _child) + { + dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister " + << "a Joint from a nullptr BodyNode. This is most likely a bug. " + << "Please report this!\n"; + assert(false); + return; + } + + Joint* joint = _child->getParentJoint(); + + std::unordered_map::iterator it = + mIndexMap.find(_child); + + if( it == mIndexMap.end() || INVALID_INDEX == it->second.mJointIndex) + { + dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister " + << "a Joint named [" << joint->getName() << "] (" << joint << "), " + << "which is the parent Joint of BodyNode [" << _child->getName() + << "] (" << _child << "), but the Joint is not currently in this " + << "ReferentialSkeleton! This is most likely a bug. Please report " + << "this!\n"; + assert(false); + return; + } + + size_t jointIndex = it->second.mJointIndex; + mJoints.erase(mJoints.begin() + jointIndex); + it->second.mJointIndex = INVALID_INDEX; + + for(size_t i = jointIndex; i < mJoints.size(); ++i) + { + // Re-index all of the Joints in this ReferentialSkeleton which came after + // the Joint that was removed. + JointPtr alteredJoint = mJoints[i]; + IndexMap& indexing = mIndexMap[alteredJoint.getBodyNodePtr()]; + indexing.mJointIndex = i; + } + + if(it->second.isExpired()) + mIndexMap.erase(it); + + // Updating the caches isn't necessary after unregistering a joint right now, + // but it might matter in the future, so it might be better to be safe than + // sorry. + updateCaches(); +} + //============================================================================== void ReferentialSkeleton::unregisterDegreeOfFreedom( BodyNode* _bn, size_t _localIndex) @@ -1185,7 +1266,8 @@ void ReferentialSkeleton::updateCaches() //============================================================================== ReferentialSkeleton::IndexMap::IndexMap() - : mBodyNodeIndex(INVALID_INDEX) + : mBodyNodeIndex(INVALID_INDEX), + mJointIndex(INVALID_INDEX) { // Do nothing } @@ -1193,7 +1275,10 @@ ReferentialSkeleton::IndexMap::IndexMap() //============================================================================== bool ReferentialSkeleton::IndexMap::isExpired() const { - if(mBodyNodeIndex != INVALID_INDEX) + if(INVALID_INDEX != mBodyNodeIndex) + return false; + + if(INVALID_INDEX != mJointIndex) return false; for(size_t i=0; i < mDofIndices.size(); ++i) diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index 3f141cf02d0d7..f7baeb0f8231f 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -331,10 +331,14 @@ class ReferentialSkeleton : public MetaSkeleton /// to this ReferentialSkeleton. This can only be used by derived classes. void registerComponent(BodyNode* _bn); - /// Add a BodyNode to this ReferentialSkeleton, ignoring its DegreesOfFreedom. - /// This can only be used by derived classes. + /// Add a BodyNode to this ReferentialSkeleton, ignoring its Joint and + /// DegreesOfFreedom. This can only be used by derived classes. void registerBodyNode(BodyNode* _bn); + /// Add a Joint to this Referential Skeleton, ignoring its DegreesOfFreedom. + /// This can only be used by derived classes. + void registerJoint(Joint* _joint); + /// Add a DegreeOfFreedom to this ReferentialSkeleton. This can only be used /// by derived classes. void registerDegreeOfFreedom(DegreeOfFreedom* _dof); @@ -347,6 +351,10 @@ class ReferentialSkeleton : public MetaSkeleton /// DegreesOfFreedom. This can only be used by derived classes. void unregisterBodyNode(BodyNode* _bn, bool _unregisterDofs); + /// Remove a Joint from this ReferentialSkeleton. This can only be used by + /// derived classes. + void unregisterJoint(BodyNode* _child); + /// Remove a DegreeOfFreedom from this ReferentialSkeleton. This can only be /// used by derived classes. void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex); @@ -365,10 +373,14 @@ class ReferentialSkeleton : public MetaSkeleton /// Index of the BodyNode size_t mBodyNodeIndex; - /// Indices of the DegreesOfFreedom + /// Index of the parent Joint + size_t mJointIndex; + + /// Indices of the parent DegreesOfFreedom std::vector mDofIndices; - /// Default constructor. Initializes mBodyNodeIndex to INVALID_INDEX + /// Default constructor. Initializes mBodyNodeIndex and mJointIndex to + /// INVALID_INDEX IndexMap(); /// Returns true if nothing in this entry is mapping to a valid index any @@ -389,6 +401,9 @@ class ReferentialSkeleton : public MetaSkeleton /// Raw const BodyNode pointers. This vector is used for the MetaSkeleton API mutable std::vector mRawConstBodyNodes; + /// Joints that this ReferentialSkeleton references + std::vector mJoints; + /// DegreesOfFreedom that this ReferentialSkeleton references std::vector mDofs; diff --git a/dart/dynamics/detail/JointPtr.h b/dart/dynamics/detail/JointPtr.h index dc63bc1e5286f..d9a71956f1098 100644 --- a/dart/dynamics/detail/JointPtr.h +++ b/dart/dynamics/detail/JointPtr.h @@ -106,6 +106,12 @@ class TemplateJointPtr return mBodyNodePtr->getParentJoint(); } + /// Get the BodyNode that this JointPtr is tied to + TemplateBodyNodePtr getBodyNodePtr() const + { + return mBodyNodePtr; + } + /// Set the Joint for this JointPtr void set(JointT* _ptr) { diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index b42afe63f03b0..df1a27a996acf 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -854,6 +854,19 @@ size_t checkForBodyNodes( return count; } +void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) +{ + EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints()); + + // Linkages should have the property: + // getJoint(i) == getBodyNode(i)->getParentJoint() + for(size_t i=0; i < refSkel->getNumJoints(); ++i) + { + EXPECT_EQ(refSkel->getJoint(i), refSkel->getBodyNode(i)->getParentJoint()); + EXPECT_EQ(refSkel->getIndexOf(refSkel->getJoint(i)), i); + } +} + TEST(Skeleton, Linkage) { // Test a variety of uses of Linkage::Criteria @@ -866,6 +879,7 @@ TEST(Skeleton, Linkage) ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), skel->getBodyNode("c3b4"), "midchain"); checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(midchain); Linkage::Criteria criteria; criteria.mStart = skel->getBodyNode("c5b2"); @@ -874,6 +888,7 @@ TEST(Skeleton, Linkage) LinkagePtr path = Linkage::create(criteria, "path"); checkForBodyNodes(path, skel, true, "c5b2", "c5b1", "c1b3", "c3b1", "c3b2", "c3b3", "c4b1", "c4b2", "c4b3"); + checkLinkageJointConsistency(path); skel->getBodyNode(0)->copyTo(nullptr); criteria.mTargets.clear(); @@ -889,6 +904,7 @@ TEST(Skeleton, Linkage) "c3b1(1)", "c1b3(1)", "c2b1(1)", "c2b2(1)", "c2b3(1)", "c5b1", "c5b2", "c1b2", "c1b1", "c5b1(1)", "c5b2(1)", "c1b2(1)", "c1b1(1)"); + checkLinkageJointConsistency(combinedTreeBases); SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2"); criteria.mTargets.clear(); @@ -908,23 +924,28 @@ TEST(Skeleton, Linkage) ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"), skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint"); checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1"); + checkLinkageJointConsistency(downstreamFreeJoint); ChainPtr emptyChain = Chain::create(skel->getBodyNode("c1b1"), skel->getBodyNode("c1b3"), "emptyChain"); checkForBodyNodes(emptyChain, skel, true); + checkLinkageJointConsistency(emptyChain); ChainPtr chainFromNull = Chain::create(nullptr, skel->getBodyNode("c1b2"), "chainFromNull"); checkForBodyNodes(chainFromNull, skel, true, "c1b1"); + checkLinkageJointConsistency(chainFromNull); ChainPtr upstreamFreeJoint = Chain::create(skel->getBodyNode("c1b3"), skel->getBodyNode("c1b1"), "upstreamFreeJoint"); checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upstreamFreeJoint); // Using nullptr as the target should bring us towards the root of the tree ChainPtr upTowardsRoot = Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot"); checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upTowardsRoot); criteria.mTargets.clear(); criteria.mTargets.push_back(skel->getBodyNode("c4b3")); @@ -933,6 +954,7 @@ TEST(Skeleton, Linkage) LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage"); checkForBodyNodes(terminatedLinkage, skel, true, "c1b3", "c3b1", "c3b2"); + checkLinkageJointConsistency(terminatedLinkage); criteria.mStart = skel->getBodyNode("c1b1"); criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM; @@ -945,12 +967,14 @@ TEST(Skeleton, Linkage) checkForBodyNodes(terminatedSubtree, skel, true, "c1b1", "c1b2", "c1b3", "c5b1", "c5b2", "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(terminatedSubtree); criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; criteria.mStart.mNode = skel->getBodyNode("c3b1"); LinkagePtr terminatedUpstream = Linkage::create(criteria, "terminatedUpstream"); checkForBodyNodes(terminatedUpstream, skel, true, "c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1"); + checkLinkageJointConsistency(terminatedUpstream); } int main(int argc, char* argv[]) From cba5ffb8cc23b132e5b4e5e4ff1c2c6d96719a32 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 18 Nov 2015 18:50:49 -0500 Subject: [PATCH 37/79] updating API for Group construction --- dart/dynamics/Group.cpp | 45 +++++++++++++++++++++++++++++------------ dart/dynamics/Group.h | 18 +++++++++++------ 2 files changed, 44 insertions(+), 19 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 634b9b1b0413a..9572220a93961 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -46,9 +46,9 @@ namespace dynamics { //============================================================================== GroupPtr Group::create(const std::string& _name, const std::vector& _bodyNodes, - bool _includeDofs) + bool _includeJoints, bool _includeDofs) { - GroupPtr group(new Group(_name, _bodyNodes, _includeDofs)); + GroupPtr group(new Group(_name, _bodyNodes, _includeJoints, _includeDofs)); group->mPtr = group; return group; } @@ -56,9 +56,9 @@ GroupPtr Group::create(const std::string& _name, //============================================================================== GroupPtr Group::create(const std::string& _name, const std::vector& _dofs, - bool _includeBodyNodes) + bool _includeBodyNodes, bool _includeJoints) { - GroupPtr group(new Group(_name, _dofs, _includeBodyNodes)); + GroupPtr group(new Group(_name, _dofs, _includeBodyNodes, _includeJoints)); group->mPtr = group; return group; } @@ -368,7 +368,7 @@ bool Group::addJoint(Joint* _joint, bool _addDofs, bool _warning) } bool added = false; - if(INVALID_INDEX == getIndexOf(_joint)) + if(INVALID_INDEX == getIndexOf(_joint, false)) { registerJoint(_joint); added = true; @@ -555,7 +555,7 @@ bool Group::removeDof(DegreeOfFreedom* _dof, bool _cleanupJoint, bool _warning) Joint* joint = _dof->getJoint(); for(size_t i=0; i < joint->getNumDofs(); ++i) { - if(getIndexOf(joint->getDof(i)) == INVALID_INDEX) + if(getIndexOf(joint->getDof(i), false) == INVALID_INDEX) { performCleanup = false; break; @@ -601,18 +601,26 @@ bool Group::removeDofs(const std::vector& _dofs, //============================================================================== Group::Group(const std::string& _name, - const std::vector& _bodyNodes, bool _includeDofs) + const std::vector& _bodyNodes, + bool _includeJoints, bool _includeDofs) { setName(_name); addBodyNodes(_bodyNodes); - if(_includeDofs) + if(_includeDofs || _includeJoints) { for(size_t i=0; i < _bodyNodes.size(); ++i) { Joint* joint = _bodyNodes[i]->getParentJoint(); - for(size_t j=0; j < joint->getNumDofs(); ++j) - addDof(joint->getDof(j)); + + if(_includeJoints) + addJoint(joint, false); + + if(_includeDofs) + { + for(size_t j=0; j < joint->getNumDofs(); ++j) + addDof(joint->getDof(j)); + } } } } @@ -620,10 +628,10 @@ Group::Group(const std::string& _name, //============================================================================== Group::Group(const std::string& _name, const std::vector& _dofs, - bool _includeBodyNodes) + bool _includeBodyNodes, bool _includeJoints) { setName(_name); - addDofs(_dofs); + addDofs(_dofs, _includeJoints); if(_includeBodyNodes) { @@ -640,7 +648,18 @@ Group::Group(const std::string& _name, const MetaSkeletonPtr& _metaSkeleton) { setName(_name); - addComponents(_metaSkeleton->getBodyNodes()); + + if(_metaSkeleton) + { + for(size_t i=0; i < _metaSkeleton->getNumBodyNodes(); ++i) + addBodyNode(_metaSkeleton->getBodyNode(i)); + + for(size_t i=0; i < _metaSkeleton->getNumJoints(); ++i) + addJoint(_metaSkeleton->getJoint(i), false); + + for(size_t i=0; i < _metaSkeleton->getNumDofs(); ++i) + addDof(_metaSkeleton->getDof(i), false); + } } } // namespace dynamics diff --git a/dart/dynamics/Group.h b/dart/dynamics/Group.h index b47fbcd3eb852..a2f90da5283c6 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -46,21 +46,25 @@ class Group : public ReferentialSkeleton { public: - /// Create a Group out of a set of BodyNodes. If _includeDofs is true, then - /// the parent DegreesOfFreedom of each BodyNode will also be added to the - /// Group. + /// Create a Group out of a set of BodyNodes. If _includeJoints is true, the + /// parent Joint of each BodyNode will also be added to the Group. If + /// _includeDofs is true, then the parent DegreesOfFreedom of each BodyNode + /// will also be added to the Group. static GroupPtr create( const std::string& _name = "Group", const std::vector& _bodyNodes = std::vector(), + bool _includeJoints = true, bool _includeDofs = true); /// Create a Group out of a set of DegreesOfFreedom. If _includeBodyNodes is /// true, then the child BodyNode of each DegreeOfFreedom will also be added - /// to the Group. + /// to the Group. If _includeJoints is true, then the Joint of each + /// DegreeOfFreedom will also be added to the Group. static GroupPtr create( const std::string& _name, const std::vector& _dofs, - bool _includeBodyNodes = true); + bool _includeBodyNodes = true, + bool _includeJoints = true); /// Create a Group that mimics the given MetaSkeleton static GroupPtr create( @@ -227,12 +231,14 @@ class Group : public ReferentialSkeleton /// Default constructor Group(const std::string& _name, const std::vector& _bodyNodes, + bool _includeJoints, bool _includeDofs); /// Alternative constructor Group(const std::string& _name, const std::vector& _dofs, - bool _includeBodyNodes); + bool _includeBodyNodes, + bool _includeJoints); /// MetaSkeleton-based constructor Group(const std::string& _name, From bc57bc26ef04129f8f878e1e7eb55b524dacfad2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 18 Nov 2015 18:51:05 -0500 Subject: [PATCH 38/79] created unit tests for new Group features --- unittests/testSkeleton.cpp | 67 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index df1a27a996acf..c3c0c9cc13f25 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -977,6 +977,73 @@ TEST(Skeleton, Linkage) checkLinkageJointConsistency(terminatedUpstream); } +TEST(Skeleton, Group) +{ + SkeletonPtr skel = constructLinkageTestSkeleton(); + + // Make twice as many BodyNodes in the Skeleton + SkeletonPtr skel2 = constructLinkageTestSkeleton(); + skel2->getRootBodyNode()->moveTo(skel, nullptr); + + // Test nullptr construction + GroupPtr nullGroup = Group::create("null_group", nullptr); + EXPECT_EQ(nullGroup->getNumBodyNodes(), 0u); + EXPECT_EQ(nullGroup->getNumJoints(), 0u); + EXPECT_EQ(nullGroup->getNumDofs(), 0u); + + // Test conversion from Skeleton + GroupPtr skel1Group = Group::create("skel1_group", skel); + EXPECT_EQ(skel1Group->getNumBodyNodes(), skel->getNumBodyNodes()); + EXPECT_EQ(skel1Group->getNumJoints(), skel->getNumJoints()); + EXPECT_EQ(skel1Group->getNumDofs(), skel->getNumDofs()); + + for(size_t i=0; i < skel1Group->getNumBodyNodes(); ++i) + EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i)); + + for(size_t i=0; i < skel1Group->getNumJoints(); ++i) + EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i)); + + for(size_t i=0; i < skel1Group->getNumDofs(); ++i) + EXPECT_EQ(skel1Group->getDof(i), skel->getDof(i)); + + // Test arbitrary Groups by plucking random BodyNodes, Joints, and + // DegreesOfFreedom from a Skeleton. + GroupPtr group = Group::create(); + std::vector bodyNodes; + std::vector joints; + std::vector dofs; + for(size_t i=0; i < 2*skel->getNumBodyNodes(); ++i) + { + size_t randomIndex = floor(random(0, skel->getNumBodyNodes())); + BodyNode* bn = skel->getBodyNode(randomIndex); + if(group->addBodyNode(bn, false)) + bodyNodes.push_back(bn); + + randomIndex = floor(random(0, skel->getNumJoints())); + Joint* joint = skel->getJoint(randomIndex); + if(group->addJoint(joint, false, false)) + joints.push_back(joint); + + randomIndex = floor(random(0, skel->getNumDofs())); + DegreeOfFreedom* dof = skel->getDof(randomIndex); + if(group->addDof(dof, false, false)) + dofs.push_back(dof); + } + + EXPECT_EQ(group->getNumBodyNodes(), bodyNodes.size()); + EXPECT_EQ(group->getNumJoints(), joints.size()); + EXPECT_EQ(group->getNumDofs(), dofs.size()); + + for(size_t i=0; i < group->getNumBodyNodes(); ++i) + EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]); + + for(size_t i=0; i < group->getNumJoints(); ++i) + EXPECT_EQ(group->getJoint(i), joints[i]); + + for(size_t i=0; i < group->getNumDofs(); ++i) + EXPECT_EQ(group->getDof(i), dofs[i]); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 4ceafe1f94a0b4c0f4ecff697c367481e6ee76e3 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 19 Nov 2015 13:08:42 -0500 Subject: [PATCH 39/79] removed OpenGLCamera --- dart/renderer/OpenGLCamera.h | 68 ------------------------------------ 1 file changed, 68 deletions(-) delete mode 100644 dart/renderer/OpenGLCamera.h diff --git a/dart/renderer/OpenGLCamera.h b/dart/renderer/OpenGLCamera.h deleted file mode 100644 index 458b4b4f8a605..0000000000000 --- a/dart/renderer/OpenGLCamera.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Jie (Jay) Tan - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * 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. - * 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. - */ - -#ifndef DART_RENDERER_OPENGLCAMERA_H -#define DART_RENDERER_OPENGLCAMERA_H - -#include -#include "dart/renderer/Camera.h" - -namespace dart { -namespace renderer { - -class OpenGLCamera : public Camera -{ -public: - OpenGLCamera() {} - virtual ~OpenGLCamera() {} - virtual void set(const Eigen::Vector3d& _Eye, const Eigen::Vector3d& _look, const Eigen::Vector3d& _up) {} - virtual void slide(double _delX, double _delY, double _delZ, bool _bLocal = false) {} - virtual void setFrustum(float _vAng, float _asp, float _nearD, float _farD) {} - virtual void setOrtho(float _width, float _height, float _nearD, float _farD) {} - - - virtual void roll(float _angle) {} - virtual void pitch(float _angle) {} - virtual void yaw(float _angle) {} - virtual void localRotate(float _angle, AXIS _axis) {} - virtual void globalRotate(float _angle, AXIS _axis) {} - -}; - -} // namespace renderer -} // namespace dart - -#endif // #ifndef DART_RENDERER_OPENGLCAMERA_H From 2d165673e2b2e5d43966c7b6e046933545686a71 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 20 Nov 2015 18:09:21 -0500 Subject: [PATCH 40/79] Fix packaging of dart-core/dart libraries --- dart/CMakeLists.txt | 4 ++-- debian/libdart-core5-dev.install | 1 + debian/libdart5-dev.install | 1 - 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/CMakeLists.txt b/dart/CMakeLists.txt index 5a0b5542426e0..a1f92e79a3dcc 100644 --- a/dart/CMakeLists.txt +++ b/dart/CMakeLists.txt @@ -36,6 +36,7 @@ set(dart_core_hdrs ${dart_optimizer_hdrs} ${dart_collision_hdrs} ${dart_constraint_hdrs} + ${dart_renderer_hdrs} ${dart_simulation_hdrs} ) set(dart_core_srcs @@ -47,6 +48,7 @@ set(dart_core_srcs ${dart_optimizer_srcs} ${dart_collision_srcs} ${dart_constraint_srcs} + ${dart_renderer_srcs} ${dart_simulation_srcs} ) if(NOT BUILD_CORE_ONLY) @@ -54,13 +56,11 @@ if(NOT BUILD_CORE_ONLY) dart.h ${dart_gui_hdrs} ${dart_planning_hdrs} - ${dart_renderer_hdrs} ${dart_utils_hdrs} ) set(dart_srcs ${dart_gui_srcs} ${dart_planning_srcs} - ${dart_renderer_srcs} ${dart_utils_srcs} ) endif() diff --git a/debian/libdart-core5-dev.install b/debian/libdart-core5-dev.install index b02d27661cffd..5dda48f9d8fbf 100644 --- a/debian/libdart-core5-dev.install +++ b/debian/libdart-core5-dev.install @@ -7,6 +7,7 @@ usr/include/dart/dynamics/* usr/include/dart/integration/* usr/include/dart/lcpsolver/* usr/include/dart/math/* +usr/include/dart/optimizer/* usr/include/dart/renderer/* usr/include/dart/simulation/* usr/share/dartcore/DARTCoreConfig.cmake diff --git a/debian/libdart5-dev.install b/debian/libdart5-dev.install index 59e05fe1b0ca9..ef992b5fe6edb 100644 --- a/debian/libdart5-dev.install +++ b/debian/libdart5-dev.install @@ -1,5 +1,4 @@ usr/include/dart/dart.h -usr/include/dart/optimizer/* usr/include/dart/planning/* usr/include/dart/utils/* usr/include/dart/gui/* From 0aa402175398215af9e6fb7331a4e86f85484bc8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 26 Nov 2015 04:45:57 -0500 Subject: [PATCH 41/79] Rename Parser.[h/cpp] to XmlHelpers.[h/cpp] --- dart/utils/SkelParser.h | 2 +- dart/utils/{Parser.cpp => XmlHelpers.cpp} | 2 +- dart/utils/{Parser.h => XmlHelpers.h} | 6 +++--- dart/utils/sdf/SdfParser.h | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) rename dart/utils/{Parser.cpp => XmlHelpers.cpp} (99%) rename dart/utils/{Parser.h => XmlHelpers.h} (98%) diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index c5b6250ba742b..a28a563c93e49 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -46,7 +46,7 @@ #include #include "dart/common/Deprecated.h" -#include "dart/utils/Parser.h" +#include "dart/utils/XmlHelpers.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Joint.h" diff --git a/dart/utils/Parser.cpp b/dart/utils/XmlHelpers.cpp similarity index 99% rename from dart/utils/Parser.cpp rename to dart/utils/XmlHelpers.cpp index 84076111f9c15..68eb1ca788949 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/utils/Parser.h" +#include "dart/utils/XmlHelpers.h" #include #include diff --git a/dart/utils/Parser.h b/dart/utils/XmlHelpers.h similarity index 98% rename from dart/utils/Parser.h rename to dart/utils/XmlHelpers.h index 34a1d55b96b07..6e1b6edb46f8c 100644 --- a/dart/utils/Parser.h +++ b/dart/utils/XmlHelpers.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_UTILS_PARSER_H -#define DART_UTILS_PARSER_H +#ifndef DART_UTILS_XMLHELPERS_H_ +#define DART_UTILS_XMLHELPERS_H_ #include #include @@ -148,4 +148,4 @@ class ElementEnumerator } // namespace utils } // namespace dart -#endif // #ifndef DART_UTILS_PARSER_H +#endif // #ifndef DART_UTILS_XMLHELPER_H_ diff --git a/dart/utils/sdf/SdfParser.h b/dart/utils/sdf/SdfParser.h index 0389bf6f2320a..666d1cb468c41 100644 --- a/dart/utils/sdf/SdfParser.h +++ b/dart/utils/sdf/SdfParser.h @@ -11,7 +11,7 @@ #include #include "dart/common/Deprecated.h" -#include "dart/utils/Parser.h" +#include "dart/utils/XmlHelpers.h" #include "dart/common/ResourceRetriever.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/WeldJoint.h" From 04098274b7404845b96620b6dcb6ebd321690113 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 26 Nov 2015 08:22:29 -0500 Subject: [PATCH 42/79] Add attribute getters and revise ElementEnumerator - The attribute getters are defined for different types - ElementEnumerator is templated to cover const and non-const elements - The element value getters are changed to takes const element pointer --- dart/utils/SkelParser.cpp | 14 +-- dart/utils/XmlHelpers.cpp | 217 +++++++++++++++++++++++++---------- dart/utils/XmlHelpers.h | 173 ++++++++++++++++++++-------- dart/utils/sdf/SdfParser.cpp | 14 +-- 4 files changed, 296 insertions(+), 122 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 3ca4512a6974a..97977ec225a17 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -466,7 +466,7 @@ dynamics::SkeletonPtr SkelParser::readSkeleton( //-------------------------------------------------------------------------- // Name attribute - std::string name = getAttribute(_skeletonElement, "name"); + std::string name = getAttributeString(_skeletonElement, "name"); newSkeleton->setName(name); //-------------------------------------------------------------------------- @@ -584,7 +584,7 @@ SkelParser::SkelBodyNode SkelParser::readBodyNode( Eigen::Isometry3d initTransform = Eigen::Isometry3d::Identity(); // Name attribute - newBodyNode->mName = getAttribute(_bodyNodeElement, "name"); + newBodyNode->mName = getAttributeString(_bodyNodeElement, "name"); //-------------------------------------------------------------------------- // gravity @@ -905,7 +905,7 @@ dynamics::Marker::Properties SkelParser::readMarker( tinyxml2::XMLElement* _markerElement) { // Name attribute - std::string name = getAttribute(_markerElement, "name"); + std::string name = getAttributeString(_markerElement, "name"); // offset Eigen::Vector3d offset = Eigen::Vector3d::Zero(); @@ -927,13 +927,13 @@ void SkelParser::readJoint(tinyxml2::XMLElement* _jointElement, //-------------------------------------------------------------------------- // Name attribute - std::string name = getAttribute(_jointElement, "name"); + std::string name = getAttributeString(_jointElement, "name"); SkelJoint joint; //-------------------------------------------------------------------------- // Type attribute - joint.type = getAttribute(_jointElement, "type"); + joint.type = getAttributeString(_jointElement, "type"); assert(!joint.type.empty()); if (joint.type == std::string("weld")) joint.properties = readWeldJoint(_jointElement, joint, name); @@ -968,7 +968,7 @@ void SkelParser::readJoint(tinyxml2::XMLElement* _jointElement, // Actuator attribute if (hasAttribute(_jointElement, "actuator")) { - const std::string actuator = getAttribute(_jointElement, "actuator"); + const std::string actuator = getAttributeString(_jointElement, "actuator"); if (actuator == "force") joint.properties->mActuatorType = dynamics::Joint::FORCE; @@ -1885,7 +1885,7 @@ SkelParser::JointPropPtr SkelParser::readPlanarJoint( tinyxml2::XMLElement* planeElement = getElement(_jointElement, "plane"); // Type attribute - std::string type = getAttribute(planeElement, "type"); + std::string type = getAttributeString(planeElement, "type"); if (type == "xy") { diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index 68eb1ca788949..9a2c8a0879619 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -405,7 +405,7 @@ Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str) return T; } -std::string getValueString(tinyxml2::XMLElement* _parentElement, const std::string& _name) +std::string getValueString(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -415,7 +415,7 @@ std::string getValueString(tinyxml2::XMLElement* _parentElement, const std::stri return str; } -bool getValueBool(tinyxml2::XMLElement* _parentElement, const std::string& _name) +bool getValueBool(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -438,7 +438,7 @@ bool getValueBool(tinyxml2::XMLElement* _parentElement, const std::string& _name } } -int getValueInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) +int getValueInt(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -448,7 +448,7 @@ int getValueInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) return toInt(str); } -unsigned int getValueUInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) +unsigned int getValueUInt(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -458,7 +458,7 @@ unsigned int getValueUInt(tinyxml2::XMLElement* _parentElement, const std::strin return toUInt(str); } -float getValueFloat(tinyxml2::XMLElement* _parentElement, const std::string& _name) +float getValueFloat(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -468,7 +468,7 @@ float getValueFloat(tinyxml2::XMLElement* _parentElement, const std::string& _na return toFloat(str); } -double getValueDouble(tinyxml2::XMLElement* _parentElement, const std::string& _name) +double getValueDouble(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -478,7 +478,7 @@ double getValueDouble(tinyxml2::XMLElement* _parentElement, const std::string& _ return toDouble(str); } -char getValueChar(tinyxml2::XMLElement* _parentElement, const std::string& _name) +char getValueChar(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -488,7 +488,7 @@ char getValueChar(tinyxml2::XMLElement* _parentElement, const std::string& _name return toChar(str); } -Eigen::Vector2d getValueVector2d(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Vector2d getValueVector2d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -498,7 +498,7 @@ Eigen::Vector2d getValueVector2d(tinyxml2::XMLElement* _parentElement, const std return toVector2d(str); } -Eigen::Vector3d getValueVector3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Vector3d getValueVector3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -508,7 +508,7 @@ Eigen::Vector3d getValueVector3d(tinyxml2::XMLElement* _parentElement, const std return toVector3d(str); } -Eigen::Vector3i getValueVector3i(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Vector3i getValueVector3i(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -518,7 +518,7 @@ Eigen::Vector3i getValueVector3i(tinyxml2::XMLElement* _parentElement, const std return toVector3i(str); } -Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Vector6d getValueVector6d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -529,7 +529,7 @@ Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std } //============================================================================== -Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, +Eigen::VectorXd getValueVectorXd(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); @@ -540,7 +540,7 @@ Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, return toVectorXd(str); } -Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Vector3d getValueVec3(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -550,7 +550,7 @@ Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::st return toVector3d(str); } -Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -560,7 +560,7 @@ Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const return toIsometry3d(str); } -Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name) +Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != nullptr); assert(!_name.empty()); @@ -631,79 +631,174 @@ bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) return result != 0; } +//============================================================================== std::string getAttribute(tinyxml2::XMLElement * element, const char* const name) { - const char* const result = element->Attribute(name); - if( result == 0 ) - { - std::ostringstream oss; - oss << "Missing attribute " << name << " on " << element->Name(); - throw std::runtime_error(oss.str()); - } - return std::string(result); + return getAttributeString(element, name); } -void getAttribute(tinyxml2::XMLElement* element, - const char* const name, - double* d) +//============================================================================== +void getAttribute(tinyxml2::XMLElement* element, const char* const name, + double* d) { - int result = element->QueryDoubleAttribute(name, d); - if( result != tinyxml2::XML_NO_ERROR ) - { - std::ostringstream oss; - oss << "Error parsing double attribute " << name << " on " << element->Name(); - throw std::runtime_error(oss.str()); - } + *d = getAttributeDouble(element, name); } -ElementEnumerator::ElementEnumerator(tinyxml2::XMLElement* _parent, - const std::string& _name) - : m_name(_name), - m_parent(_parent), - m_current(nullptr) +//============================================================================== +std::string getAttributeString(const tinyxml2::XMLElement* element, + const std::string& attributeName) { + const char* const result = element->Attribute(attributeName.c_str()); + + if (nullptr == result) + { + dtwarn << "[getAttribute] Error in parsing string type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning empty string.\n"; + return std::string(); + } + + return std::string(result); } -ElementEnumerator::~ElementEnumerator() +//============================================================================== +bool getAttributeBool(const tinyxml2::XMLElement* element, + const std::string& attributeName) { + bool val = false; + const int result = element->QueryBoolAttribute(attributeName.c_str(), + &val); + + if (result != tinyxml2::XML_NO_ERROR) + { + dtwarn << "[getAttribute] Error in parsing bool type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning false instead.\n"; + return false; + } + + return val; } -bool ElementEnumerator::valid() const +//============================================================================== +int getAttributeInt(const tinyxml2::XMLElement* element, + const std::string& attributeName) { - return m_current != nullptr; + int val = 0; + const int result = element->QueryIntAttribute(attributeName.c_str(), &val); + + if (result != tinyxml2::XML_NO_ERROR) + { + dtwarn << "[getAttribute] Error in parsing int type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning zero instead.\n"; + return 0; + } + + return val; } -bool ElementEnumerator::next() +//============================================================================== +unsigned int getAttributeUInt(const tinyxml2::XMLElement* element, + const std::string& attributeName) { - if(!m_parent) - return false; + unsigned int val = 0u; + const int result = element->QueryUnsignedAttribute(attributeName.c_str(), + &val); - if(m_current) - m_current = m_current->NextSiblingElement(m_name.c_str()); - else - m_current = m_parent->FirstChildElement(m_name.c_str()); + if (result != tinyxml2::XML_NO_ERROR) + { + dtwarn << "[getAttribute] Error in parsing unsiged int type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning zero instead.\n"; + return 0u; + } + + return val; +} + +//============================================================================== +float getAttributeFloat(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + float val = 0.0f; + const int result = element->QueryFloatAttribute(attributeName.c_str(), + &val); + + if (result != tinyxml2::XML_NO_ERROR) + { + dtwarn << "[getAttribute] Error in parsing float type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning zero instead.\n"; + return 0.0f; + } + + return val; +} + +//============================================================================== +double getAttributeDouble(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + double val = 0.0; + const int result = element->QueryDoubleAttribute(attributeName.c_str(), + &val); + + if (result != tinyxml2::XML_NO_ERROR) + { + dtwarn << "[getAttribute] Error in parsing double type attribute [" + << attributeName << "] of an element [" << element->Name() + << "]. Returning zero instead.\n"; + return 0.0; + } + + return val; +} + +//============================================================================== +char getAttributeChar(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + const std::string val = getAttributeString(element, attributeName); + + return toChar(val); +} - if(!valid()) - m_parent = nullptr; +//============================================================================== +Eigen::Vector2d getAttributeVector2d(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + const std::string val = getAttributeString(element, attributeName); - return valid(); + return toVector2d(val); } -bool ElementEnumerator::operator==(const ElementEnumerator& _rhs) const +//============================================================================== +Eigen::Vector3d getAttributeVector3d(const tinyxml2::XMLElement* element, + const std::string& attributeName) { - // If they point at the same node, then the names must match - return (this->m_parent == _rhs.m_parent) && - (this->m_current == _rhs.m_current) && - (this->m_current != 0 || (this->m_name == _rhs.m_name)); + const std::string val = getAttributeString(element, attributeName); + + return toVector3d(val); } -ElementEnumerator& ElementEnumerator::operator=(const ElementEnumerator& _rhs) +//============================================================================== +Eigen::Vector6d getAttributeVector6d(const tinyxml2::XMLElement* element, + const std::string& attributeName) { - this->m_name = _rhs.m_name; - this->m_parent = _rhs.m_parent; - this->m_current = _rhs.m_current; - return *this; + const std::string val = getAttributeString(element, attributeName); + + return toVector6d(val); +} + +//============================================================================== +Eigen::VectorXd getAttributeVectorXd(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + const std::string val = getAttributeString(element, attributeName); + + return toVectorXd(val); } } // namespace utils diff --git a/dart/utils/XmlHelpers.h b/dart/utils/XmlHelpers.h index 6e1b6edb46f8c..8bdb1c5ed341d 100644 --- a/dart/utils/XmlHelpers.h +++ b/dart/utils/XmlHelpers.h @@ -77,75 +77,154 @@ Eigen::VectorXd toVectorXd (const std::string& _str); Eigen::Isometry3d toIsometry3d(const std::string& _str); Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str); -std::string getValueString (tinyxml2::XMLElement* _parentElement, const std::string& _name); -bool getValueBool (tinyxml2::XMLElement* _parentElement, const std::string& _name); -int getValueInt (tinyxml2::XMLElement* _parentElement, const std::string& _name); -unsigned int getValueUInt (tinyxml2::XMLElement* _parentElement, const std::string& _name); -float getValueFloat (tinyxml2::XMLElement* _parentElement, const std::string& _name); -double getValueDouble (tinyxml2::XMLElement* _parentElement, const std::string& _name); -char getValueChar (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector2d getValueVector2d (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector3d getValueVector3d (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector3i getValueVector3i (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector6d getValueVector6d (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::VectorXd getValueVectorXd (tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name); +std::string getValueString (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +bool getValueBool (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +int getValueInt (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +unsigned int getValueUInt (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +float getValueFloat (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +double getValueDouble (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +char getValueChar (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Vector2d getValueVector2d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Vector3d getValueVector3d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Vector3i getValueVector3i (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Vector6d getValueVector6d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::VectorXd getValueVectorXd (const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* _parentElement, const std::string& _name); void openXMLFile(tinyxml2::XMLDocument& doc, const common::Uri& uri, const common::ResourceRetrieverPtr& retriever = nullptr); bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); bool hasAttribute(tinyxml2::XMLElement* element, const char* const name); + +// Please use getAttributeString() instead. +DEPRECATED(6.0) std::string getAttribute(tinyxml2::XMLElement* element, const char* const name); + +// Please use getAttributeDouble() instead. +DEPRECATED(6.0) void getAttribute(tinyxml2::XMLElement* element, const char* const name, double* d); -//------------------------------------------------------------------------------ -// -//------------------------------------------------------------------------------ -/// \brief -class ElementEnumerator +std::string getAttributeString (const tinyxml2::XMLElement* element, const std::string& attributeName); +bool getAttributeBool (const tinyxml2::XMLElement* element, const std::string& attributeName); +int getAttributeInt (const tinyxml2::XMLElement* element, const std::string& attributeName); +unsigned int getAttributeUInt (const tinyxml2::XMLElement* element, const std::string& attributeName); +float getAttributeFloat (const tinyxml2::XMLElement* element, const std::string& attributeName); +double getAttributeDouble (const tinyxml2::XMLElement* element, const std::string& attributeName); +char getAttributeChar (const tinyxml2::XMLElement* element, const std::string& attributeName); +Eigen::Vector2d getAttributeVector2d(const tinyxml2::XMLElement* element, const std::string& attributeName); +Eigen::Vector3d getAttributeVector3d(const tinyxml2::XMLElement* element, const std::string& attributeName); +Eigen::Vector6d getAttributeVector6d(const tinyxml2::XMLElement* element, const std::string& attributeName); +Eigen::VectorXd getAttributeVectorXd(const tinyxml2::XMLElement* element, const std::string& attributeName); + +/// TemplatedElementEnumerator is a convenience class to help visiting all the +/// child elements of given parent element. This class is templated to cover +/// const and non-const tinyxml2::XMLElement types. +template +class TemplatedElementEnumerator { -public: - /// \brief - ElementEnumerator(tinyxml2::XMLElement* _parent, const std::string& _name); +protected: - /// \brief - virtual ~ElementEnumerator(); + using ElementPtr = typename std::add_pointer::type; + using ElementRef = typename std::add_lvalue_reference::type; - /// \brief - bool valid() const; - - /// \brief - bool next(); +public: - /// \brief - tinyxml2::XMLElement* get() const { return m_current; } + /// Constructor that takes parent element and + TemplatedElementEnumerator(ElementPtr parentElement, + const std::string& childElementName) + : mParentElement(parentElement), + mChildElementName(childElementName), + mCurrentElement(nullptr) + { + } + + /// Destructor + ~TemplatedElementEnumerator() {} + + /// Set the current element to the next sibling element or to the first child + /// element of given parent element if it exists; returns success + bool next() + { + if (!mParentElement) + return false; + + if (mCurrentElement) + { + mCurrentElement + = mCurrentElement->NextSiblingElement(mChildElementName.c_str()); + } + else + { + mCurrentElement + = mParentElement->FirstChildElement(mChildElementName.c_str()); + } + + if (!valid()) + mParentElement = nullptr; + + return valid(); + } + + /// Get the current element + ElementPtr get() const { return mCurrentElement; } + + /// Dereference operator + ElementPtr operator->() const { return mCurrentElement; } + + /// Dereference operator + ElementRef operator*() const { return *mCurrentElement; } + + /// Equality operator + bool operator==(const TemplatedElementEnumerator& rhs) const + { + // If they point at the same node, then the names must match + return (this->mParentElement == rhs.mParentElement) + && (this->mCurrentElement == rhs.mCurrentElement) + && (this->mCurrentElement != nullptr + || (this->mChildElementName == rhs.mChildElementName)); + } + + /// Assignment operator + TemplatedElementEnumerator& operator=( + const TemplatedElementEnumerator& rhs) + { + this->mParentElement = rhs.mParentElement; + this->mChildElementName = rhs.mChildElementName; + this->mCurrentElement = rhs.mCurrentElement; + + return *this; + } - /// \brief - tinyxml2::XMLElement* operator->() const { return m_current; } +private: - /// \brief - tinyxml2::XMLElement& operator*() const { return *m_current; } + /// Returns true if the current element is valid (not a nullptr) + bool valid() const + { + return mCurrentElement != nullptr; + } - /// \brief - bool operator==(const ElementEnumerator& _rhs) const; +private: - /// \brief - ElementEnumerator & operator=(const ElementEnumerator& _rhs); + /// Parent element + ElementPtr mParentElement; -private: - /// \brief - std::string m_name; + /// Child element name + std::string mChildElementName; - /// \brief - tinyxml2::XMLElement* m_parent; + /// Currently visiting child element + ElementPtr mCurrentElement; - /// \brief - tinyxml2::XMLElement* m_current; }; +// ElementEnumerator is for iterating elements for +using ElementEnumerator + = TemplatedElementEnumerator; +using ConstElementEnumerator + = TemplatedElementEnumerator; + } // namespace utils } // namespace dart -#endif // #ifndef DART_UTILS_XMLHELPER_H_ +#endif // #ifndef DART_UTILS_XMLHELPERS_H_ diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 331e7209c9b05..a4d06ef95b901 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -68,7 +68,7 @@ simulation::WorldPtr SdfParser::readSdfFile( //-------------------------------------------------------------------------- // version attribute - std::string version = getAttribute(sdfElement, "version"); + std::string version = getAttributeString(sdfElement, "version"); // TODO: We need version aware SDF parser (see #264) // We support 1.4 only for now. if (version != "1.4" && version != "1.5") @@ -134,7 +134,7 @@ dynamics::SkeletonPtr SdfParser::readSkeleton( //-------------------------------------------------------------------------- // version attribute - std::string version = getAttribute(sdfElement, "version"); + std::string version = getAttributeString(sdfElement, "version"); // TODO: We need version aware SDF parser (see #264) // We support 1.4 only for now. if (version != "1.4" && version != "1.5") @@ -186,7 +186,7 @@ simulation::WorldPtr SdfParser::readWorld( //-------------------------------------------------------------------------- // Name attribute - std::string name = getAttribute(_worldElement, "name"); + std::string name = getAttributeString(_worldElement, "name"); newWorld->setName(name); //-------------------------------------------------------------------------- @@ -392,7 +392,7 @@ dynamics::SkeletonPtr SdfParser::makeSkeleton( //-------------------------------------------------------------------------- // Name attribute - std::string name = getAttribute(_skeletonElement, "name"); + std::string name = getAttributeString(_skeletonElement, "name"); newSkeleton->setName(name); //-------------------------------------------------------------------------- @@ -469,7 +469,7 @@ SdfParser::SDFBodyNode SdfParser::readBodyNode( Eigen::Isometry3d initTransform = Eigen::Isometry3d::Identity(); // Name attribute - std::string name = getAttribute(_bodyNodeElement, "name"); + std::string name = getAttributeString(_bodyNodeElement, "name"); properties.mName = name; //-------------------------------------------------------------------------- @@ -716,12 +716,12 @@ SdfParser::SDFJoint SdfParser::readJoint(tinyxml2::XMLElement* _jointElement, //-------------------------------------------------------------------------- // Type attribute - std::string type = getAttribute(_jointElement, "type"); + std::string type = getAttributeString(_jointElement, "type"); assert(!type.empty()); //-------------------------------------------------------------------------- // Name attribute - std::string name = getAttribute(_jointElement, "name"); + std::string name = getAttributeString(_jointElement, "name"); //-------------------------------------------------------------------------- // parent From 984e1c2e9acc949dbc6ddaba855493852f79b4a4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 26 Nov 2015 08:33:25 -0500 Subject: [PATCH 43/79] Style fixes --- dart/utils/XmlHelpers.cpp | 618 ++++++++++++++++++++------------------ dart/utils/XmlHelpers.h | 80 ++--- 2 files changed, 374 insertions(+), 324 deletions(-) diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index 9a2c8a0879619..1f1fb62ffc9ce 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -48,199 +48,211 @@ namespace dart { namespace utils { -std::string toString(bool _v) +//============================================================================== +std::string toString(bool v) { - return boost::lexical_cast(_v); + return boost::lexical_cast(v); } -std::string toString(int _v) +//============================================================================== +std::string toString(int v) { - return boost::lexical_cast(_v); + return boost::lexical_cast(v); } -std::string toString(unsigned int _v) +//============================================================================== +std::string toString(unsigned int v) { - return boost::lexical_cast(_v); + return boost::lexical_cast(v); } -std::string toString(float _v) +//============================================================================== +std::string toString(float v) { - //if (std::isfinite(_v)) - return boost::lexical_cast(_v); - //else - // return std::string("0"); + return boost::lexical_cast(v); } -std::string toString(double _v) +//============================================================================== +std::string toString(double v) { - //if (std::isfinite(_v)) - return boost::lexical_cast(_v); - //else - // return std::string("0"); + return boost::lexical_cast(v); } -std::string toString(char _v) +//============================================================================== +std::string toString(char v) { - return boost::lexical_cast(_v); + return boost::lexical_cast(v); } //============================================================================== -std::string toString(const Eigen::Vector2d& _v) +std::string toString(const Eigen::Vector2d& v) { - return boost::lexical_cast(_v.transpose()); + return boost::lexical_cast(v.transpose()); } //============================================================================== -std::string toString(const Eigen::Vector3d& _v) +std::string toString(const Eigen::Vector3d& v) { - return boost::lexical_cast(_v.transpose()); + return boost::lexical_cast(v.transpose()); } //============================================================================== -std::string toString(const Eigen::Vector3i& _v) +std::string toString(const Eigen::Vector3i& v) { - return boost::lexical_cast(_v.transpose()); + return boost::lexical_cast(v.transpose()); } //============================================================================== -std::string toString(const Eigen::Vector6d& _v) +std::string toString(const Eigen::Vector6d& v) { - return boost::lexical_cast(_v.transpose()); + return boost::lexical_cast(v.transpose()); } //============================================================================== -std::string toString(const Eigen::VectorXd& _v) +std::string toString(const Eigen::VectorXd& v) { - return boost::lexical_cast(_v.transpose()); + return boost::lexical_cast(v.transpose()); } -std::string toString(const Eigen::Isometry3d& _v) +//============================================================================== +std::string toString(const Eigen::Isometry3d& v) { - std::ostringstream ostr; - ostr.precision(6); + std::ostringstream ostr; + ostr.precision(6); - Eigen::Vector3d xyz = math::matrixToEulerXYZ(_v.linear()); + Eigen::Vector3d xyz = math::matrixToEulerXYZ(v.linear()); - ostr << _v.translation()(0) << " " - << _v.translation()(1) << " " - << _v.translation()(2) << " "; - ostr << xyz[0] << " " << xyz[1] << " " << xyz[2]; + ostr << v.translation()(0) << " " + << v.translation()(1) << " " + << v.translation()(2) << " "; + ostr << xyz[0] << " " << xyz[1] << " " << xyz[2]; - return ostr.str(); + return ostr.str(); } -bool toBool(const std::string& _str) +//============================================================================== +bool toBool(const std::string& str) { - if (boost::to_upper_copy(_str) == "TRUE" || _str == "1") - return true; - else if (boost::to_upper_copy(_str) == "FALSE" || _str == "0") - return false; - else - { - dterr << "value [" - << _str - << "] is not a valid boolean type. " - << "Retuning false." - << std::endl; - return false; - } + if (boost::to_upper_copy(str) == "TRUE" || str == "1") + return true; + else if (boost::to_upper_copy(str) == "FALSE" || str == "0") + return false; + else + { + dterr << "value [" + << str + << "] is not a valid boolean type. " + << "Retuning false." + << std::endl; + return false; + } } -int toInt(const std::string& _str) +//============================================================================== +int toInt(const std::string& str) { - return boost::lexical_cast(_str); + return boost::lexical_cast(str); } -unsigned int toUInt(const std::string& _str) +//============================================================================== +unsigned int toUInt(const std::string& str) { - return boost::lexical_cast(_str); + return boost::lexical_cast(str); } -float toFloat(const std::string& _str) +//============================================================================== +float toFloat(const std::string& str) { - return boost::lexical_cast(_str); + return boost::lexical_cast(str); } -double toDouble(const std::string& _str) +//============================================================================== +double toDouble(const std::string& str) { - return boost::lexical_cast(_str); + return boost::lexical_cast(str); } - -char toChar(const std::string& _str) +//============================================================================== +char toChar(const std::string& str) { - return boost::lexical_cast(_str); + return boost::lexical_cast(str); } -Eigen::Vector2d toVector2d(const std::string& _str) +//============================================================================== +Eigen::Vector2d toVector2d(const std::string& str) { - Eigen::Vector2d ret; + Eigen::Vector2d ret; - std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); - assert(pieces.size() == 2); + std::vector pieces; + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); + assert(pieces.size() == 2); - for (size_t i = 0; i < pieces.size(); ++i) + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") { - if (pieces[i] != "") - { - try - { - ret(i) = boost::lexical_cast(pieces[i].c_str()); - } - catch(boost::bad_lexical_cast& e) - { - std::cerr << "value [" - << pieces[i] - << "] is not a valid double for Eigen::Vector2d[" - << i - << std::endl; - } - } + try + { + ret(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch (boost::bad_lexical_cast& e) + { + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for Eigen::Vector2d[" + << i + << std::endl; + } } + } - return ret; + return ret; } -Eigen::Vector3d toVector3d(const std::string& _str) +//============================================================================== +Eigen::Vector3d toVector3d(const std::string& str) { - Eigen::Vector3d ret; + Eigen::Vector3d ret; - std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); - assert(pieces.size() == 3); + std::vector pieces; + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); + assert(pieces.size() == 3); - for (size_t i = 0; i < pieces.size(); ++i) + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") { - if (pieces[i] != "") - { - try - { - ret(i) = boost::lexical_cast(pieces[i].c_str()); - } - catch(boost::bad_lexical_cast& e) - { - std::cerr << "value [" - << pieces[i] - << "] is not a valid double for Eigen::Vector3d[" - << i - << "]" - << std::endl; - } - } + try + { + ret(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch(boost::bad_lexical_cast& e) + { + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for Eigen::Vector3d[" + << i + << "]" + << std::endl; + } } + } - return ret; + return ret; } -Eigen::Vector3i toVector3i(const std::string& _str) +//============================================================================== +Eigen::Vector3i toVector3i(const std::string& str) { Eigen::Vector3i ret; std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); assert(pieces.size() == 3); for (size_t i = 0; i < pieces.size(); ++i) @@ -266,43 +278,45 @@ Eigen::Vector3i toVector3i(const std::string& _str) return ret; } -Eigen::Vector6d toVector6d(const std::string& _str) +//============================================================================== +Eigen::Vector6d toVector6d(const std::string& str) { - Eigen::Vector6d ret; + Eigen::Vector6d ret; - std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); - assert(pieces.size() == 6); + std::vector pieces; + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); + assert(pieces.size() == 6); - for (size_t i = 0; i < pieces.size(); ++i) + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") { - if (pieces[i] != "") - { - try - { - ret(i) = boost::lexical_cast(pieces[i].c_str()); - } - catch(boost::bad_lexical_cast& e) - { - std::cerr << "value [" - << pieces[i] - << "] is not a valid double for Eigen::Vector6d[" - << i - << "]" - << std::endl; - } - } + try + { + ret(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch(boost::bad_lexical_cast& e) + { + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for Eigen::Vector6d[" + << i + << "]" + << std::endl; + } } + } - return ret; + return ret; } //============================================================================== -Eigen::VectorXd toVectorXd(const std::string& _str) +Eigen::VectorXd toVectorXd(const std::string& str) { std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); + std::string trimedStr = boost::trim_copy(str); boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); assert(pieces.size() > 0); @@ -332,71 +346,75 @@ Eigen::VectorXd toVectorXd(const std::string& _str) return ret; } -Eigen::Isometry3d toIsometry3d(const std::string& _str) +//============================================================================== +Eigen::Isometry3d toIsometry3d(const std::string& str) { - Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); - Eigen::Vector6d elements = Eigen::Vector6d::Zero(); - std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); - assert(pieces.size() == 6); + Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); + Eigen::Vector6d elements = Eigen::Vector6d::Zero(); + std::vector pieces; + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); + assert(pieces.size() == 6); - for (size_t i = 0; i < pieces.size(); ++i) + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") { - if (pieces[i] != "") - { - try - { - elements(i) = boost::lexical_cast(pieces[i].c_str()); - } - catch(boost::bad_lexical_cast& e) - { - std::cerr << "value [" - << pieces[i] - << "] is not a valid double for SE3[" - << i - << "]" - << std::endl; - } - } + try + { + elements(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch(boost::bad_lexical_cast& e) + { + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for SE3[" + << i + << "]" + << std::endl; + } } + } - T.linear() = math::eulerXYZToMatrix(elements.tail<3>()); - T.translation() = elements.head<3>(); - return T; + T.linear() = math::eulerXYZToMatrix(elements.tail<3>()); + T.translation() = elements.head<3>(); + return T; } -Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str) +//============================================================================== +Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& str) { Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); Eigen::Vector6d elements = Eigen::Vector6d::Zero(); std::vector pieces; - std::string trimedStr = boost::trim_copy(_str); - boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on); + std::string trimedStr = boost::trim_copy(str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); assert(pieces.size() == 6); for (size_t i = 0; i < pieces.size(); ++i) { - if (pieces[i] != "") + if (pieces[i] != "") + { + try + { + elements(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch(boost::bad_lexical_cast& e) { - try - { - elements(i) = boost::lexical_cast(pieces[i].c_str()); - } - catch(boost::bad_lexical_cast& e) - { - std::cerr << "value [" - << pieces[i] - << "] is not a valid double for SE3[" - << i - << "]" - << std::endl; - } + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for SE3[" + << i + << "]" + << std::endl; } + } } Eigen::Vector3d reverseEulerAngles( - elements.tail<3>()[2], + elements.tail<3>()[2], elements.tail<3>()[1], elements.tail<3>()[0]); @@ -405,195 +423,226 @@ Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str) return T; } -std::string getValueString(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +std::string getValueString(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return str; + return str; } -bool getValueBool(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +bool getValueBool(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - if (boost::to_upper_copy(str) == "TRUE" || str == "1") - return true; - else if (boost::to_upper_copy(str) == "FALSE" || str == "0") - return false; - else - { - std::cerr << "value [" - << str - << "] is not a valid boolean type. " - << "Returning false." - << std::endl; - assert(0); - return false; - } + if (boost::to_upper_copy(str) == "TRUE" || str == "1") + return true; + else if (boost::to_upper_copy(str) == "FALSE" || str == "0") + return false; + else + { + std::cerr << "value [" + << str + << "] is not a valid boolean type. " + << "Returning false." + << std::endl; + assert(0); + return false; + } } -int getValueInt(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +int getValueInt(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toInt(str); + return toInt(str); } -unsigned int getValueUInt(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +unsigned int getValueUInt(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toUInt(str); + return toUInt(str); } -float getValueFloat(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +float getValueFloat(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toFloat(str); + return toFloat(str); } -double getValueDouble(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +double getValueDouble(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toDouble(str); + return toDouble(str); } -char getValueChar(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +char getValueChar(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toChar(str); + return toChar(str); } -Eigen::Vector2d getValueVector2d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Vector2d getValueVector2d(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toVector2d(str); + return toVector2d(str); } -Eigen::Vector3d getValueVector3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Vector3d getValueVector3d(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toVector3d(str); + return toVector3d(str); } -Eigen::Vector3i getValueVector3i(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Vector3i getValueVector3i(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); return toVector3i(str); } -Eigen::Vector6d getValueVector6d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Vector6d getValueVector6d(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toVector6d(str); + return toVector6d(str); } //============================================================================== -Eigen::VectorXd getValueVectorXd(const tinyxml2::XMLElement* _parentElement, - const std::string& _name) +Eigen::VectorXd getValueVectorXd(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); return toVectorXd(str); } -Eigen::Vector3d getValueVec3(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Vector3d getValueVec3(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toVector3d(str); + return toVector3d(str); } -Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toIsometry3d(str); + return toIsometry3d(str); } -Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation( + const tinyxml2::XMLElement* parentElement, const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); - return toIsometry3dWithExtrinsicRotation(str); + return toIsometry3dWithExtrinsicRotation(str); } -bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name) +//============================================================================== +bool hasElement(tinyxml2::XMLElement* parentElement, const std::string& name) { - assert(_parentElement != nullptr); - assert(!_name.empty()); + assert(parentElement != nullptr); + assert(!name.empty()); - return _parentElement->FirstChildElement(_name.c_str()) == nullptr ? false : true; + return parentElement->FirstChildElement(name.c_str()) + == nullptr ? false : true; } -tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, - const std::string& _name) +//============================================================================== +tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* parentElement, + const std::string& name) { - assert(!_name.empty()); + assert(!name.empty()); - return _parentElement->FirstChildElement(_name.c_str()); + return parentElement->FirstChildElement(name.c_str()); } //============================================================================== -void openXMLFile( - tinyxml2::XMLDocument& doc, const common::Uri& uri, - const common::ResourceRetrieverPtr& _retriever) +void openXMLFile(tinyxml2::XMLDocument& doc, + const common::Uri& uri, + const common::ResourceRetrieverPtr& retrieverOrNullPtr) { common::ResourceRetrieverPtr retriever; - if(_retriever) - retriever = _retriever; + if(retrieverOrNullPtr) + retriever = retrieverOrNullPtr; else retriever = std::make_shared(); @@ -625,6 +674,7 @@ void openXMLFile( } } +//============================================================================== bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) { const char* const result = element->Attribute(name); @@ -633,7 +683,7 @@ bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) //============================================================================== std::string getAttribute(tinyxml2::XMLElement * element, - const char* const name) + const char* const name) { return getAttributeString(element, name); } diff --git a/dart/utils/XmlHelpers.h b/dart/utils/XmlHelpers.h index 8bdb1c5ed341d..a1aededdf0a18 100644 --- a/dart/utils/XmlHelpers.h +++ b/dart/utils/XmlHelpers.h @@ -49,47 +49,47 @@ namespace dart { namespace utils { -std::string toString(bool _v); -std::string toString(int _v); -std::string toString(unsigned int _v); -std::string toString(float _v); -std::string toString(double _v); -std::string toString(char _v); -std::string toString(const Eigen::Vector2d& _v); -std::string toString(const Eigen::Vector3d& _v); -std::string toString(const Eigen::Vector3i& _v); -std::string toString(const Eigen::Vector6d& _v); -std::string toString(const Eigen::VectorXd& _v); -std::string toString(const Eigen::Isometry3d& _v); - -bool toBool (const std::string& _str); -int toInt (const std::string& _str); -unsigned int toUInt (const std::string& _str); -float toFloat (const std::string& _str); -double toDouble (const std::string& _str); -char toChar (const std::string& _str); -Eigen::Vector2d toVector2d (const std::string& _str); -Eigen::Vector3d toVector3d (const std::string& _str); -Eigen::Vector3i toVector3i (const std::string& _str); -Eigen::Vector6d toVector6d (const std::string& _str); -Eigen::VectorXd toVectorXd (const std::string& _str); +std::string toString(bool v); +std::string toString(int v); +std::string toString(unsigned int v); +std::string toString(float v); +std::string toString(double v); +std::string toString(char v); +std::string toString(const Eigen::Vector2d& v); +std::string toString(const Eigen::Vector3d& v); +std::string toString(const Eigen::Vector3i& v); +std::string toString(const Eigen::Vector6d& v); +std::string toString(const Eigen::VectorXd& v); +std::string toString(const Eigen::Isometry3d& v); + +bool toBool (const std::string& str); +int toInt (const std::string& str); +unsigned int toUInt (const std::string& str); +float toFloat (const std::string& str); +double toDouble (const std::string& str); +char toChar (const std::string& str); +Eigen::Vector2d toVector2d (const std::string& str); +Eigen::Vector3d toVector3d (const std::string& str); +Eigen::Vector3i toVector3i (const std::string& str); +Eigen::Vector6d toVector6d (const std::string& str); +Eigen::VectorXd toVectorXd (const std::string& str); // TODO: The definition of _str is not clear for transform (see: #250) -Eigen::Isometry3d toIsometry3d(const std::string& _str); -Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str); - -std::string getValueString (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -bool getValueBool (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -int getValueInt (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -unsigned int getValueUInt (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -float getValueFloat (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -double getValueDouble (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -char getValueChar (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector2d getValueVector2d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector3d getValueVector3d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector3i getValueVector3i (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Vector6d getValueVector6d (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::VectorXd getValueVectorXd (const tinyxml2::XMLElement* _parentElement, const std::string& _name); -Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Isometry3d toIsometry3d(const std::string& str); +Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& str); + +std::string getValueString (const tinyxml2::XMLElement* parentElement, const std::string& name); +bool getValueBool (const tinyxml2::XMLElement* parentElement, const std::string& name); +int getValueInt (const tinyxml2::XMLElement* parentElement, const std::string& name); +unsigned int getValueUInt (const tinyxml2::XMLElement* parentElement, const std::string& name); +float getValueFloat (const tinyxml2::XMLElement* parentElement, const std::string& name); +double getValueDouble (const tinyxml2::XMLElement* parentElement, const std::string& name); +char getValueChar (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::Vector2d getValueVector2d (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::Vector3d getValueVector3d (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::Vector3i getValueVector3i (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::Vector6d getValueVector6d (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::VectorXd getValueVectorXd (const tinyxml2::XMLElement* parentElement, const std::string& name); +Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* parentElement, const std::string& name); Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* _parentElement, const std::string& _name); void openXMLFile(tinyxml2::XMLDocument& doc, const common::Uri& uri, From 482d17c2527e10a5aed6ae549e11bc45186442fc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 26 Nov 2015 08:42:16 -0500 Subject: [PATCH 44/79] More style fixes and const correctness fixes --- dart/utils/XmlHelpers.cpp | 15 +++++++++++++-- dart/utils/XmlHelpers.h | 20 +++++++++++++++----- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index 1f1fb62ffc9ce..1182e28e34b1a 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -617,7 +617,8 @@ Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation( } //============================================================================== -bool hasElement(tinyxml2::XMLElement* parentElement, const std::string& name) +bool hasElement(const tinyxml2::XMLElement* parentElement, + const std::string& name) { assert(parentElement != nullptr); assert(!name.empty()); @@ -626,6 +627,16 @@ bool hasElement(tinyxml2::XMLElement* parentElement, const std::string& name) == nullptr ? false : true; } +//============================================================================== +const tinyxml2::XMLElement* getElement( + const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(!name.empty()); + + return parentElement->FirstChildElement(name.c_str()); +} + //============================================================================== tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* parentElement, const std::string& name) @@ -675,7 +686,7 @@ void openXMLFile(tinyxml2::XMLDocument& doc, } //============================================================================== -bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) +bool hasAttribute(const tinyxml2::XMLElement* element, const char* const name) { const char* const result = element->Attribute(name); return result != 0; diff --git a/dart/utils/XmlHelpers.h b/dart/utils/XmlHelpers.h index a1aededdf0a18..58f0477f7e09a 100644 --- a/dart/utils/XmlHelpers.h +++ b/dart/utils/XmlHelpers.h @@ -90,13 +90,23 @@ Eigen::Vector3i getValueVector3i (const tinyxml2::XMLElement* parentElement, Eigen::Vector6d getValueVector6d (const tinyxml2::XMLElement* parentElement, const std::string& name); Eigen::VectorXd getValueVectorXd (const tinyxml2::XMLElement* parentElement, const std::string& name); Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* parentElement, const std::string& name); -Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(const tinyxml2::XMLElement* parentElement, const std::string& name); -void openXMLFile(tinyxml2::XMLDocument& doc, const common::Uri& uri, +void openXMLFile(tinyxml2::XMLDocument& doc, + const common::Uri& uri, const common::ResourceRetrieverPtr& retriever = nullptr); -bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); -tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); -bool hasAttribute(tinyxml2::XMLElement* element, const char* const name); + +bool hasElement(const tinyxml2::XMLElement* parentElement, + const std::string& name); + +const tinyxml2::XMLElement* getElement( + const tinyxml2::XMLElement* parentElement, + const std::string& name); + +tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* parentElement, + const std::string& name); + +bool hasAttribute(const tinyxml2::XMLElement* element, const char* const name); // Please use getAttributeString() instead. DEPRECATED(6.0) From 9d24b2f2337169abc12599ef3aa5ae8bf0655fba Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 26 Nov 2015 09:49:41 -0500 Subject: [PATCH 45/79] Reinstate VSK parser adapting it to the current API -- not finished yet --- dart/math/Geometry.cpp | 56 +- dart/math/Geometry.h | 22 + dart/simulation/World.cpp | 2 - dart/utils/ParserVsk.cpp_ | 977 ----------------------- dart/utils/VskParser.cpp | 843 +++++++++++++++++++ dart/utils/{ParserVsk.h_ => VskParser.h} | 34 +- data/{skel_old => vsk}/Nick01.vsk | 0 data/{skel_old => vsk}/SehoonVSK3.vsk | 0 data/{skel_old => vsk}/Yuting.vsk | 0 data/vsk/test/empty.vsk | 3 + unittests/testVskParser.cpp | 111 +++ 11 files changed, 1041 insertions(+), 1007 deletions(-) delete mode 100644 dart/utils/ParserVsk.cpp_ create mode 100644 dart/utils/VskParser.cpp rename dart/utils/{ParserVsk.h_ => VskParser.h} (73%) rename data/{skel_old => vsk}/Nick01.vsk (100%) rename data/{skel_old => vsk}/SehoonVSK3.vsk (100%) rename data/{skel_old => vsk}/Yuting.vsk (100%) create mode 100644 data/vsk/test/empty.vsk create mode 100644 unittests/testVskParser.cpp diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index e1693fe9bba2f..3063efb0fd17b 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1445,27 +1445,44 @@ Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v) { } //============================================================================== -Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin, - const Eigen::Vector3d& _axisZ) +Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis, + const AxisType axisType) { - Eigen::Isometry3d result = Eigen::Isometry3d::Identity(); + assert(axis != Eigen::Vector3d::Zero()); + + // First axis + const Eigen::Vector3d axis0 = axis.normalized(); + + // Second axis + Eigen::Vector3d axis1 = axis0.cross(Eigen::Vector3d::UnitX()); + if (axis1.norm() < DART_EPSILON) + axis1 = axis0.cross(Eigen::Vector3d::UnitY()); + axis1.normalize(); - // Compute orientation - const Eigen::Vector3d& axisZ = _axisZ.normalized(); - Eigen::Vector3d axisY = axisZ.cross(Eigen::Vector3d::UnitX()); - if (axisY.norm() < 1e-6) - axisY = axisZ.cross(Eigen::Vector3d::UnitY()); - axisY.normalize(); + // Third axis + const Eigen::Vector3d axis2 = axis0.cross(axis1).normalized(); - const Eigen::Vector3d& axisX = (axisY.cross(axisZ)).normalized(); + // Assign the three axes + Eigen::Matrix3d result; + int index = axisType; + result.col(index) = axis0; + result.col(++index % 3) = axis1; + result.col(++index % 3) = axis2; + + assert(verifyRotation(result)); + + return result; +} - // Assign orientation - result.linear().col(0) = axisX; - result.linear().col(1) = axisY; - result.linear().col(2) = axisZ; +//============================================================================== +Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis, + const Eigen::Vector3d& translation, + AxisType axisType) +{ + Eigen::Isometry3d result = Eigen::Isometry3d::Identity(); - // Assign translation - result.translation() = _origin; + result.linear() = computeRotation(axis, axisType); + result.translation() = translation; // Verification assert(verifyTransform(result)); @@ -1473,6 +1490,13 @@ Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin, return result; } +//============================================================================== +Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin, + const Eigen::Vector3d& _axisZ) +{ + return computeTransform(_axisZ, _origin, AxisType::AXIS_Z); +} + //============================================================================== SupportPolygon computeSupportPolgyon(const SupportGeometry& _geometry, const Eigen::Vector3d& _axis1, diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h index 0025514c6b352..28499e0216f19 100644 --- a/dart/math/Geometry.h +++ b/dart/math/Geometry.h @@ -40,6 +40,7 @@ #include +#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" namespace dart { @@ -416,7 +417,28 @@ Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original, const Eigen::Vector3d& _comShift, double _mass); +enum AxisType +{ + AXIS_X = 0, + AXIS_Y = 1, + AXIS_Z = 2 +}; + +/// Compute a rotation matrix from a vector. One axis of the rotated coordinates +/// by the rotation matrix matches the input axis where the axis is specified +/// by axisType. +Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis, + AxisType axisType = AxisType::AXIS_X); + +/// Compute a transform from a vector and a position. The rotation of the result +/// transform is computed by computeRotationMatrix(), and the translation is +/// just the input translation. +Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis, + const Eigen::Vector3d& translation, + AxisType axisType = AxisType::AXIS_X); + /// Generate frame given origin and z-axis +DEPRECATED(6.0) Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin, const Eigen::Vector3d& _axisZ); diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 6857d1e5576a1..b194bc904840a 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -277,8 +277,6 @@ size_t World::getNumSkeletons() const //============================================================================== std::string World::addSkeleton(dynamics::SkeletonPtr _skeleton) { - assert(_skeleton != nullptr && "Attempted to add nullptr skeleton to world."); - if(nullptr == _skeleton) { dtwarn << "[World::addSkeleton] Attempting to add a nullptr Skeleton to " diff --git a/dart/utils/ParserVsk.cpp_ b/dart/utils/ParserVsk.cpp_ deleted file mode 100644 index 608f2c9947e0e..0000000000000 --- a/dart/utils/ParserVsk.cpp_ +++ /dev/null @@ -1,977 +0,0 @@ -/* - * Copyright (c) 2012, Georgia Tech Research Corporation - * All rights reserved. - * - * Author(s): Sehoon Ha , Matthew Dutton - * Date: 06/29/2012 - * - * Georgia Tech Graphics Lab and Humanoid Robotics Lab - * - * Directed by Prof. C. Karen Liu and Prof. Mike Stilman - * - * - * This file is provided under the following "BSD-style" License: - * 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. - * 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. - */ - -#include "ParserVsk.h" - -// Standard Library -#include -#include -#include -using namespace std; - -// TinyXML-2 Library -// http://www.grinninglizard.com/tinyxml2/index.html -#include - -#include -using namespace Eigen; - -// Local Files -#include "Skeleton.h" -#include "BodyNode.h" -#include "Joint.h" -#include "Marker.h" -#include "Dof.h" -#include "Transformation.h" -#include "TrfmTranslate.h" -#include "TrfmRotateExpmap.h" -#include "TrfmRotateEuler.h" -#include "Marker.h" -#include "Shape.h" -#include "ShapeEllipsoid.h" -#include "ShapeBox.h" -#include "math/UtilsRotation.h" -#include "utils/UtilsCode.h" - -#define SCALE_VSK 1.0e-3 -Vector3d expShoulder(0,0,0); -double lenShoulder = 0; - -using namespace kinematics; - - -// Forward Declarations of helper functions -Vector3d adjustPos(const Vector3d& _pos); -VectorXd getDofVectorXd(Transformation* trfm); - -// Parsing Helper Functions -bool readJointFree(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel); -bool readJointBall(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel, Vector3d orient); -bool readJointHardySpicer(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel); -bool readJointHinge(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel); -bool readSegment(tinyxml2::XMLElement*_segment, BodyNode* _parent, map& _paramsList, map& _segmentindex, Skeleton* _skel); -bool readMarker(tinyxml2::XMLElement*_marker, map& _paramsList, map& _segmentindex, Skeleton* _skel); -bool readShape(tinyxml2::XMLElement* _prim, map& _paramsList, map& _massList, map& _segmentindex, Skeleton* _skel); -void autoGenerateShape(Skeleton* skel); -void autoGenerateShapeParent(Skeleton* skel); - -namespace { - struct ElementEnumerator - { - private: - typedef tinyxml2::XMLElement XMLElement; - - XMLElement * m_parent; - std::string m_name; - XMLElement * m_current; - - public: - ElementEnumerator(XMLElement * parent, const char * const name) - : m_parent(parent) - , m_name(name) - , m_current(0) - { } - - bool valid() const { return m_current != 0; } - - bool next() - { - if( !m_parent ) - return false; - - if( m_current ) - m_current = m_current->NextSiblingElement(m_name.c_str()); - else - m_current = m_parent->FirstChildElement(m_name.c_str()); - - if( !valid() ) - m_parent = 0; - - return valid(); - } - - XMLElement * get() const { return m_current; } - XMLElement * operator->() const { return m_current; } - XMLElement & operator*() const { return *m_current; } - - bool operator==(ElementEnumerator const & rhs) const - { - // If they point at the same node, then the names must match - return (this->m_parent == rhs.m_parent) && - (this->m_current == rhs.m_current) && - (this->m_current != 0 || (this->m_name == rhs.m_name)); - } - - ElementEnumerator & operator=(ElementEnumerator const & rhs) - { - this->m_parent = rhs.m_parent; - this->m_name = rhs.m_name; - this->m_current = rhs.m_current; - } - }; -} // end anon namespace - - -static void openXMLFile(tinyxml2::XMLDocument & doc, const char* const filename) -{ - int const result = doc.LoadFile(filename); - switch(result) - { - case tinyxml2::XML_SUCCESS: - break; - case tinyxml2::XML_ERROR_FILE_NOT_FOUND: - throw std::runtime_error("File not found"); - case tinyxml2::XML_ERROR_FILE_COULD_NOT_BE_OPENED: - throw std::runtime_error("File not found"); - default: - { - std::ostringstream oss; - oss << "Parse error = " << result; - throw std::runtime_error(oss.str()); - } - }; -} - -static std::string getAttribute( tinyxml2::XMLElement * element, const char* const name ) -{ - const char* const result = element->Attribute(name); - if( result == 0 ) - { - std::ostringstream oss; - oss << "Missing attribute " << name << " on " << element->Name(); - throw std::runtime_error(oss.str()); - } - return std::string(result); -} - -static void getAttribute( tinyxml2::XMLElement * element, const char* const name, double * d ) -{ - int result = element->QueryDoubleAttribute(name, d); - if( result != tinyxml2::XML_NO_ERROR ) - { - std::ostringstream oss; - oss << "Error parsing double attribute " << name << " on " << element->Name(); - throw std::runtime_error(oss.str()); - } -} - -int readVSKFile(const char* const filename, Skeleton* _skel){ - // cout << "Entering Read VSK File" << endl; - - // Load xml and create Document - tinyxml2::XMLDocument _stateFile; - try - { - openXMLFile( _stateFile, filename ); - } - catch(std::exception const & e) - { - // cout << "LoadFile Fails: " << e.what() << endl; - return VSK_ERROR; - } - // cout << "Load " << filename << " (xml) Successfully" << endl; - - // Load Kinematic Model which defines Parameters, Skeletons and Markers - tinyxml2::XMLElement* kinmodel = NULL; - kinmodel = _stateFile.FirstChildElement( "KinematicModel" ); - if(!kinmodel) return VSK_ERROR; - - // Read parameters and fill paramsList - map paramsList; - paramsList.clear(); - { - tinyxml2::XMLElement* params = 0; - params = kinmodel->FirstChildElement( "Parameters" ); - if(!params) return VSK_ERROR; - // read all params - ElementEnumerator childparam(params, "Parameter"); - while( childparam.next() ) { - string pname = getAttribute(childparam.get(), "NAME"); - double val = 0; - getAttribute(childparam.get(), "VALUE", &val); - paramsList[pname] = val; - // // cout << pname << " = " << val << endl; - } - } - - // Read skeleton and fill the Skeleton* and segmentindex - map segmentindex; - segmentindex.clear(); - { - tinyxml2::XMLElement* skel= 0; - skel = kinmodel->FirstChildElement( "Skeleton" ); - if(!skel) return VSK_ERROR; - // read all segments - ElementEnumerator childseg( skel, "Segment"); - while( childseg.next() ) { - if(!readSegment(childseg.get(), NULL, paramsList, segmentindex, _skel)) - { - return VSK_ERROR; - } - } - } - - // Read markers and add them to _skel - { - tinyxml2::XMLElement* markerset = 0; - markerset = kinmodel->FirstChildElement( "MarkerSet" ); - if(!markerset) return VSK_ERROR; - // read all markers - tinyxml2::XMLElement* markers = 0; - markers = markerset->FirstChildElement( "Markers" ); - if(!markers) return VSK_ERROR; - ElementEnumerator childmarker(markers, "Marker"); - while ( childmarker.next() ) { - if(!readMarker(childmarker.get(), paramsList, segmentindex, _skel)) - { - return VSK_ERROR; - } - } - } - - // Read masses - tinyxml2::XMLElement* masses = 0; - map masslist; - masslist.clear(); - { - try { - masses = kinmodel->FirstChildElement( "Masses" ); - if(masses) { - ElementEnumerator childmass(masses, "Mass"); - while ( childmass.next() ) { - string const mname = getAttribute( childmass.get(), "NAME" ); - - double mi=0; - getAttribute(childmass.get(), "VALUE", &mi); - - masslist[mname] = mi; - // cout<<"mass: "<FirstChildElement( "Shapes" ); - if(shapes) { - ElementEnumerator childshape(shapes, "Shape"); - while ( childshape.next() ) { - if(!readShape(childshape->ToElement(), paramsList, masslist, segmentindex, _skel)) - return false; - } - } - else { - // cout << "No shapes found." << endl; - } - } - catch( std::exception const & ) - { } - - // fill in the default if prims absent - } - - autoGenerateShapeParent(_skel); - - _skel->initSkel(); - // - // cout << "VSK Parser exiting successfully" << endl; - return VSK_OK; -} - - - -bool readSegment(tinyxml2::XMLElement*_segment, - BodyNode* _parent, - map& _paramsList, - map& _segmentindex, - Skeleton* _skel) -{ - string sname = getAttribute(_segment, "NAME"); - - // cout<<"\nsegment: "<getName()<createBodyNode( sname.c_str() ); - - // make a joint - Joint* jt = new Joint(_parent, blink); - Vector3d orientation(0,0,0); - - // HARDCODED: constant rotation for humerus and changed translation - if(sname.compare(1, 8, "humerus")!=0) - { - // cout << "THE COMMON CODE!!" << endl; - string txyz = getAttribute(_segment, "POSITION"); - vector tokens; utils::tokenize(txyz, tokens); - assert(tokens.size()==3); - Vector3d pos(0,0,0); - for(unsigned int i=0; i::iterator it = _paramsList.find(strval); - if(it !=_paramsList.end()) pos[i] = neg*it->second; - else { - istringstream instr(tokens[i]); - instr >> pos[i]; - } - } - Vector3d pos2 = adjustPos(pos); - if(pos2 != Vector3d(0,0,0)){ - // create new transformation - Dof** dofs = new Dof*[3]; - for(int i=0; i<3; i++) { - stringstream dofNameBuf; - dofNameBuf << sname << "_" << i; - string dofName = dofNameBuf.str(); - const char* pDofName = dofName.c_str(); - dofs[i] = new Dof(pos2[i], pDofName); - } - TrfmTranslate* tele = new TrfmTranslate(dofs[0],dofs[1],dofs[2]); - // add transformation to joint - jt->addTransform(tele, false); - // don't add to model because it's not variable - // cout<<"telescope: "< tokens; utils::tokenize(txtOrientation, tokens); - assert(tokens.size()==3); - for (int i = 0; i < 3; ++i) { - orientation[i] = utils::strTodouble(tokens[i]); - // // cout << "ORI = " << orientation[i] << " <== " << tokens[i] << endl; - } - orientation = adjustPos(orientation) / SCALE_VSK; - } - - } - // HARDCODED: constant rotation for humerus and changed translation - else { - // cout << "HUMERUS SPECIAL CODE!!!!!" << endl; - char lr = sname[0]; - //adjusted: +-Shoulder ShoulderHeight 0 - string paramShoulder = "ShoulderLen"; - paramShoulder.push_back(lr); - - // create new transformations - // telescope - Vector3d pos(0.0, 1.0, 0.0); // adjusted for skel - pos *= _paramsList[paramShoulder]; - // cout<<"shoulder len: "<<_paramsList[paramShoulder]<addTransform(tele, false); - // don't add to model because it's not variable - // cout<<"telescope: "<addTransform(consrot, false); - // don't add to model because it's not variable - // cout<<"const rotation: "<<-expShoulder<FirstChildElement( "Segment"); - string cname = getAttribute(humerus, "NAME"); - if(cname.compare(hname)!=0){ - // cout<<"Error: childname of "< tokens; utils::tokenize(hxyz, tokens); - assert(tokens.size()==3); - Vector3d pos(0,0,0); - for(unsigned int i=0; i::iterator it = _paramsList.find(strval); - if(it !=_paramsList.end()) pos[i] = neg*it->second; - else { - istringstream instr(tokens[i]); - instr >> pos[i]; - } - } - pos = adjustPos(pos); - lenShoulder = pos.norm(); - string paramShoulder = "ShoulderLen"; - paramShoulder.push_back(lr); - _paramsList[paramShoulder] = lenShoulder; - // cout<<"shoulder len: "<<_paramsList[paramShoulder]<addTransform(consrot, false); - // don't add to model because it's not variable - // cout<<"const rotation: "<FirstChildElement( "JointFree" ); - if( tf ) { - foundJoint = true; - readJointFree(tf, jt, _skel); - } - } - if(!foundJoint){ - tf = _segment->FirstChildElement( "JointBall" ); - if( tf ) { - foundJoint = true; - readJointBall(tf, jt, _skel, orientation); - } - } - if(!foundJoint){ - tf = _segment->FirstChildElement( "JointHardySpicer" ); - if( tf ) { - foundJoint = true; - readJointHardySpicer(tf, jt, _skel); - } - } - if(!foundJoint){ - tf = _segment->FirstChildElement( "JointHinge" ); - if( tf ) { - foundJoint = true; - readJointHinge(tf, jt, _skel); - } - } - // if(!foundJoint) cout<<"fixed joint!\n"; - - for(int i=0; igetNumTransforms(); i++){ - if(!jt->getTransform(i)->isVariable()) continue; - for(int j=0; jgetTransform(i)->getNumDofs(); j++){ - // cout<getTransform(i)->getDof(j)->getName()<<" "; - } - // cout<addNode(blink); - // _segmentindex[sname]=blink->getModelID(); - _segmentindex[sname]=blink->getSkelIndex(); - - // marker the subtree - ElementEnumerator childseg( _segment, "Segment" ); - while ( childseg.next() ) { - if(!readSegment(childseg->ToElement(), blink, _paramsList, _segmentindex, _skel)) - return false; - } - return true; -} - -bool readJointFree(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel) { - // cout<<"read free\n"; - - // create new transformation - string tname1 = string(_jt->getChildNode()->getName()) + "_t"; - string tname1_0 = tname1 + "Free0"; - string tname1_1 = tname1 + "Free1"; - string tname1_2 = tname1 + "Free2"; - vector dofs1; - dofs1.resize(3); - dofs1[0] = new Dof(0.0, tname1_0.c_str(), -100.0, 100.0); - dofs1[1] = new Dof(0.0, tname1_1.c_str(), -100.0, 100.0); - dofs1[2] = new Dof(0.0, tname1_2.c_str(), -100.0, 100.0); - // dofs1[1] = new Dof(0.0, -100.0, 100.0); - // dofs1[2] = new Dof(0.0, -100.0, 100.0); - // add transformation to joint - TrfmTranslate* trans = new TrfmTranslate(dofs1[0], dofs1[1], dofs1[2], tname1.c_str()); - _jt->addTransform(trans); - // add transformation to model because it's a variable dof - _skel->addTransform(trans); - - string tname2 = string(_jt->getChildNode()->getName()) + "_a"; - string tname2_0 = tname2 + "Free3"; - string tname2_1 = tname2 + "Free4"; - string tname2_2 = tname2 + "Free5"; - vector dofs2; - dofs2.resize(3); - dofs2[0] = new Dof(0.0, tname2_0.c_str(), -3.1415, 3.1415); - dofs2[1] = new Dof(0.0, tname2_1.c_str(), -3.1415, 3.1415); - dofs2[2] = new Dof(0.0, tname2_2.c_str(), -3.1415, 3.1415); - // dofs2[0] = new Dof(0.0, -3.1415, 3.1415); - // dofs2[1] = new Dof(0.0, -3.1415, 3.1415); - // dofs2[2] = new Dof(0.0, -3.1415, 3.1415); - // add transformation to joint - TrfmRotateExpMap* expmap= new TrfmRotateExpMap(dofs2[0], dofs2[1], dofs2[2], tname2.c_str()); - _jt->addTransform(expmap); - // add transformation to model because it's a variable dof - _skel->addTransform(expmap); - - return true; -} - -bool readJointBall(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel, Vector3d orient) { - // cout << "read ball\n"; - // cout << "orientation = " << orient << endl; - string tname2 = string(_jt->getChildNode()->getName()) + "_a"; - string tname2_0 = tname2 + "Ball0"; - string tname2_1 = tname2 + "Ball1"; - string tname2_2 = tname2 + "Ball2"; - vector dofs2; - dofs2.resize(3); - dofs2[0] = new Dof(orient[0], tname2_0.c_str(), -3.1415, 3.1415); - dofs2[1] = new Dof(orient[1], tname2_1.c_str(), -3.1415, 3.1415); - dofs2[2] = new Dof(orient[2], tname2_2.c_str(), -3.1415, 3.1415); - - - // dofs2[1] = new Dof(0.0, -3.1415, 3.1415); - // dofs2[2] = new Dof(0.0, -3.1415, 3.1415); - // add transformation to joint - TrfmRotateExpMap* expmap= new TrfmRotateExpMap(dofs2[0], dofs2[1], dofs2[2], tname2.c_str()); - _jt->addTransform(expmap); - // add transformation to model because it's a variable dof - _skel->addTransform(expmap); - - return true; -} - - -bool readJointHardySpicer(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel) { - // cout<<"read hardy spicer\n"; - - // Read axisxyz and parse it into tokens - string axisxyz = getAttribute(_je, "AXIS-PAIR"); - vector tokens; - tokens.clear(); - - string tname2 = string(_jt->getChildNode()->getName()) + "_a"; - string tname2_1 = tname2 + "Hardy0"; - const char* pTname1 = tname2_1.c_str(); - string tname2_2 = tname2 + "Hardy1"; - const char* pTname2 = tname2_2.c_str(); - - - // Use utils::tokenize - tokens.clear(); - utils::tokenize(axisxyz, tokens); - assert(tokens.size()==6); - - // Which axis do we have? - Transformation *r1=NULL; - if(tokens[1].compare("1")==0){ - r1 = new TrfmRotateEulerX(new Dof(0.0, pTname1, -3.1415, 3.1415)); - } - else if(tokens[2].compare("1")==0){ - r1 = new TrfmRotateEulerY(new Dof(0.0, pTname1, -3.1415, 3.1415)); - } - else if(tokens[0].compare("1")==0){ - r1 = new TrfmRotateEulerZ(new Dof(0.0, pTname1, -3.1415, 3.1415)); - } - assert(r1!=NULL); - _jt->addTransform(r1); - _skel->addTransform(r1); - - Transformation *r2=NULL; - if(tokens[4].compare("1")==0){ - r2 = new TrfmRotateEulerX(new Dof(0.0, pTname2, -3.1415, 3.1415)); - } - else if(tokens[5].compare("1")==0){ - r2 = new TrfmRotateEulerY(new Dof(0.0, pTname2, -3.1415, 3.1415)); - } - else if(tokens[3].compare("1")==0){ - r2 = new TrfmRotateEulerZ(new Dof(0.0, pTname2, -3.1415, 3.1415)); - } - assert(r2!=NULL); - _jt->addTransform(r2); - _skel->addTransform(r2); - - return true; -} - - -bool readJointHinge(tinyxml2::XMLElement* _je, Joint* _jt, Skeleton* _skel) { - // cout<<"read hinge\n"; - - string tname = string(_jt->getChildNode()->getName()) + "_a"; - tname += "Hinge0"; - const char* pTname = tname.c_str(); - - string axisxyz = getAttribute(_je, "AXIS"); - vector tokens; - tokens.clear(); - - // Use utils::tokenize - utils::tokenize(axisxyz, tokens); - assert(tokens.size()==3); - - // Read axes data - Transformation *r1=NULL; - Vector3d axis(utils::strTodouble(tokens[0]), - utils::strTodouble(tokens[1]), - utils::strTodouble(tokens[2])); - // if(tokens[1].compare("1")==0){ - if ((axis - adjustPos(Vector3d(1.0, 0.0, 0.0)) / SCALE_VSK ).norm() < 0.01) { - r1 = new TrfmRotateEulerX(new Dof(0.0, pTname, -3.1415, 3.1415)); - // cout << "RotateEulerX" << endl; - } - else if ((axis - adjustPos(Vector3d(0.0, 1.0, 0.0)) / SCALE_VSK ).norm() < 0.01) { - // else if(tokens[2].compare("1")==0){ - r1 = new TrfmRotateEulerY(new Dof(0.0, pTname, -3.1415, 3.1415)); - // cout << "RotateEulerY" << endl; - } - else if ((axis - adjustPos(Vector3d(0.0, 0.0, 1.0)) / SCALE_VSK ).norm() < 0.01) { - // else if(tokens[0].compare("1")==0){ - r1 = new TrfmRotateEulerZ(new Dof(0.0, pTname, -3.1415, 3.1415)); - // cout << "RotateEulerZ" << endl; - } - assert(r1!=NULL); - _jt->addTransform(r1); - _skel->addTransform(r1); - - return true; -} - -bool readMarker(tinyxml2::XMLElement*_marker, map& _paramsList, map& _segmentindex, Skeleton* _skel) { - string mname = getAttribute(_marker, "NAME"); - string sname = getAttribute(_marker, "SEGMENT"); - - // get the local position - string pxyz = getAttribute(_marker, "POSITION"); - vector tokens; - utils::tokenize(pxyz, tokens); - assert(tokens.size()==3); - - - Vector3d lpos(0,0,0); - for(unsigned int i=0; i::iterator it = _paramsList.find(strval); - if(it !=_paramsList.end()) lpos[i] = neg*it->second; - else { - istringstream instr(tokens[i]); - instr >> lpos[i]; - } - } - // rearrange the coordinates - Vector3d lpos2 = adjustPos(lpos); - - // HARDCODED for clavicle - if(sname.compare(1, 8, "clavicle")==0){ - // char lr = sname[0]; - // compute angle for the clavicle - // left first; so negate the previous computed for right in reading segments and then same for the right - expShoulder = -expShoulder; - - // create new position - Vector3d negExpShoulder = -expShoulder; - Quaterniond qr = dart_math::expToQuat(negExpShoulder); // negative for the markers - dart_math::rotatePoint(qr, lpos2); - } - - Marker* m = new Marker(mname.c_str(), lpos2, _skel->getNode(_segmentindex[sname])); - _skel->addMarker(m); - // cout<<"marker: "<& _paramsList, map& _massList, map& _segmentindex, Skeleton* _skel) { - string bname = getAttribute(_prim, "SEGMENT"); - int segIdx = _segmentindex[bname]; - BodyNode* blink = _skel->getNode(segIdx); - - string mname = getAttribute(_prim, "MASS"); - double mass = _massList[mname]; - - string sname = getAttribute(_prim, "SCALE"); - double scale = 0; - map::iterator it = _paramsList.find(sname); - if(it !=_paramsList.end()) scale = it->second; - else { - istringstream instr(sname); - instr >> scale; - } - - string dimxyz = getAttribute(_prim, "DIMENSION"); - vector tokens; - utils::tokenize(dimxyz, tokens); - - assert(tokens.size()==3); - - Vector3d dim(0,0,0); - for(unsigned int i=0; i> dim[i]; - } - dim = adjustPos(dim*scale); - - string offxyz = getAttribute(_prim, "OFFSET"); - utils::tokenize(offxyz, tokens); - assert(tokens.size()==3); - - Vector3d off(0,0,0); - for(unsigned int i=0; i> off[i]; - } - off = adjustPos(off*scale); - - - Shape *prim = NULL; - string ptype = getAttribute(_prim, "TYPE"); - if(ptype.compare("ELLIPSOID")==0){ - prim = new ShapeEllipsoid(dim); - } - // else if(ptype.compare("SPHERE")==0){ - // prim = new ShapeSphere(off, dim[0]); - // } - // else if(ptype.compare("CYLINDER")==0){ - // prim = new ShapeCylinder(off, dim); - // } - // else if(ptype.compare("CYLINDERX")==0){ - // prim = new ShapeCylinderX(off, dim); - // } - // else if(ptype.compare("CYLINDERZ")==0){ - // prim = new ShapeCylinderZ(off, dim); - // } - // else if(ptype.compare("HEAD")==0){ - // prim = new ShapeHead(off, dim); - // } - else if(ptype.compare("CUBE")==0){ - prim = new ShapeBox(dim); - } - else { - // cout << "Shape type " << ptype << " not recognized!\n"; - return false; - } - - //set color - const char * const szname = _prim->Attribute("RGB"); - if( szname != 0 ) - { - string cname(szname); - tokens.clear(); - utils::tokenize(cname, tokens); - if (tokens.size() == 3) - { - Vector3d clr(0,0,0); - for(unsigned int i=0; i> clr[i]; - } - prim->setColor(clr/255.0); - } - else - { - prim->setColor(Vector3d(0.5, 0.5, 1.0)); - } - } - //set local transform - if(prim) { - prim->setOffset(off); - } - - blink->addVisualizationShape(prim); - blink->addCollisionShape(prim); - blink->setMass(mass); - blink->setLocalCOM( off ); - return true; -} - - -Vector3d adjustPos(const Vector3d& _pos) { - // rearrange the coordinates - Vector3d pos2 =_pos; - // pos2[0] = _pos[1]; - // pos2[1] = _pos[2]; - // pos2[2] = _pos[0]; - pos2 *= SCALE_VSK; - return pos2; -} - -VectorXd getDofVectorXd(Transformation* tr) { - const int nDofs = tr->getNumDofs(); - VectorXd data(nDofs); - for (int i = 0; i < nDofs; ++i) { - Dof* dof = tr->getDof(i); - data[i] = dof->getValue(); - } - return data; -} - -void autoGenerateShape(Skeleton* skel) { - for(int i=0; igetNumNodes(); i++){ - - if(skel->getNode(i)->getNumShapes() > 0) - continue; - - ShapeEllipsoid *pm = new ShapeEllipsoid(0.05 * Vector3d(1.0,1.0,1.0)); - pm->setColor(Vector3d(0.5, 0.5, 1.0)); - BodyNode* node = skel->getNode(i); - node->addShape(pm); - node->setMass(1.0); - Vector3d vecZero(0,0,0); - node->setLocalCOM(vecZero); - pm->setOffset(vecZero); - } -} - -void autoGenerateShapeParent(Skeleton* skel) -{ - // autoGenerateShape(skel); return; - - // cout << "Auto-generating primitives" << endl; - - double massSum = 0.0; - for(int i=0; igetNumNodes(); i++){ - BodyNode* node = skel->getNode(i); - Joint* joint = node->getParentJoint(); - - if(node->getNumShapes() > 0) - continue; - - // Search translate matrix - Vector3d size = 0.1 * Vector3d(1,1,1); - Vector3d offset(0,0,0); - // cout << endl; - // cout << "Node = " << node->getName() << endl; - if (node->getParentNode() == NULL) - { - // cout << "computing size for the root" << endl; - size = 0.1 * Vector3d(1,1,1); - continue; - } - BodyNode* parent = node->getParentNode(); - - // cout << "Parent Node = " << node->getParentNode()->getName() << endl; - for (int j = 0; j < joint->getNumTransforms(); ++j) - { - Transformation* trfm = joint->getTransform(j); - if (trfm->getType() == Transformation::T_TRANSLATE) - { - const VectorXd dofdata = getDofVectorXd(trfm); - if (dofdata.size() == 3) - { - for (int k = 0; k < 3; ++k) - { - size[k] = fabs(dofdata[k]); - offset[k] = dofdata[k] * 0.5; - } - break; - } - } - } - - double maxSize = max(size[0], size[1]); - maxSize = max(maxSize, size[2]); - maxSize *= 0.35; - maxSize = min(0.1, maxSize); - - for (int j = 0; j < 3; ++j) - { - size[j] = max(size[j], maxSize); - } - - double density = 2000.0; - double mass = density * size[0] * size[1] * size[2]; - massSum += mass; - // cout << "Size = " << size << endl; - // cout << "Offset = " << offset << endl; - // cout << "Mass = " << mass << endl; - - // size = 0.1 * Vector3d(vl_1); - // offset = Vector3d(vl_0); - // Ellipsoid *pm = new Ellipsoid(vl_0, 0.1*Vector3d(vl_1), 1.0); - ShapeEllipsoid *pm = new ShapeEllipsoid(size); - pm->setColor(Vector3d(0.5, 0.5, 1.0)); - pm->setOffset(offset); - - BodyNode* target = parent; - target->setLocalCOM(offset); - target->addVisualizationShape(pm); - target->addCollisionShape(pm); - target->setMass(mass); - } - - autoGenerateShape(skel); - // cout << "Sum of mass = " << massSum << endl; -} - diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp new file mode 100644 index 0000000000000..29aad02f61090 --- /dev/null +++ b/dart/utils/VskParser.cpp @@ -0,0 +1,843 @@ +/* + * Copyright (c) 2012-2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Sehoon Ha , + * Matthew Dutton , + * Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include "dart/utils/VskParser.h" + +// Standard Library +#include +#include +#include + +// TinyXML-2 Library +// http://www.grinninglizard.com/tinyxml2/index.html +#include + +#include + +// Local Files +#include "dart/common/LocalResourceRetriever.h" +#include "dart/common/Uri.h" +#include "dart/dynamics/dynamics.h" +#include "dart/utils/XmlHelpers.h" + +#define SCALE_VSK 1.0e-3 + +namespace dart { +namespace utils { + +namespace { + +using BodyPropPtr = std::shared_ptr; +using JointPropPtr = std::shared_ptr; + +using ParameterMap = std::map; +using SegmentIndexMap = std::map; +using BodyNodeColorMap = std::map; + +struct VskData +{ + ParameterMap parameterMap; + SegmentIndexMap segmentIndexMap; + BodyNodeColorMap bodyNodeColorMap; +}; + +const double vsk_scale = 1.0e-3; + +const std::vector vskJointTypes + = {"JointFree", "JointBall", "JointHardySpicer", "JointHinge", "JointDummy"}; + +bool readParameters(tinyxml2::XMLElement* parametersEle, + ParameterMap& paramMap); + +bool readSkeletonElement(const tinyxml2::XMLElement* skeletonEle, + dynamics::SkeletonPtr& skel, + VskData& vskData); + +// Parsing Helper Functions +bool readSegment(const tinyxml2::XMLElement* segment, + dynamics::BodyNode* parent, + const dynamics::SkeletonPtr& skel, + VskData& vskData); + +bool readJoint(const std::string& jointType, + const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +bool readJointFree(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +bool readJointBall(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +bool readJointHinge(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +bool readJointDummy(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap); + +template +std::pair createJointAndNodePair( + const dynamics::SkeletonPtr& skeleton, + dynamics::BodyNode* parentBodyNode, + const dynamics::Joint::Properties* jointProperties, + const dynamics::BodyNode::Properties& bodyNodeProperties) +{ + return skeleton->createJointAndBodyNodePair( + parentBodyNode, + *static_cast(jointProperties), + bodyNodeProperties); +} + +bool readMarkerSet(const tinyxml2::XMLElement* markerSetEle, + const dynamics::SkeletonPtr& skel, + VskData& vskData); + +bool readMarker(const tinyxml2::XMLElement* marker, + const dynamics::SkeletonPtr& skel, + VskData& vskData); + +bool readStick(const tinyxml2::XMLElement* stickEle, + const dynamics::SkeletonPtr& skel, + VskData& vskData); + +void generateShapes(const dynamics::SkeletonPtr& skel, + VskData& vskData); + +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever); + +void tokenize(const std::string& str, + std::vector& tokens, + const std::string& delimiters = " "); + +} // anonymous namespace + +//============================================================================== +dynamics::SkeletonPtr VskParser::readSkeleton( + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr) +{ + const common::ResourceRetrieverPtr retriever + = getRetriever(retrieverOrNullptr); + + // Load VSK file and create document + tinyxml2::XMLDocument vskDocument; + try + { + openXMLFile(vskDocument, fileUri, retriever); + } + catch (std::exception const& e) + { + dtwarn << "[VskParser::readSkeleton] Failed to load file '" + << fileUri.toString() << "': " + << e.what() << std::endl; + return nullptr; + } + + // Check if is included in the document + tinyxml2::XMLElement* kinematicModelEle + = vskDocument.FirstChildElement("KinematicModel"); + if (!kinematicModelEle) + { + dtwarn << "[VskParser::readSkeleton] This file '" << fileUri.toString() + << "' doesn't include 'KinematicModel' tag. " + << "Returning null pointer instead.\n"; + return nullptr; + } + + VskData vskData; + + // Read element + tinyxml2::XMLElement* parametersEle + = kinematicModelEle->FirstChildElement("Parameters"); + if (parametersEle) + readParameters(parametersEle, vskData.parameterMap); + + // Read element + dynamics::SkeletonPtr newSkeleton = nullptr; + tinyxml2::XMLElement* skelEle + = kinematicModelEle->FirstChildElement("Skeleton"); + if (skelEle) + { + const bool result = readSkeletonElement(skelEle, newSkeleton, vskData); + + if (!result) + { + dtwarn << "[VskParser::readSkeleton] Failed to parse a element " + << "from file '" << fileUri.getPath() << "'. " + << "Returning null pointer.\n"; + } + } + else + { + dtwarn << "[VskParser::readSkeleton] Failed to find element " + << "under element " + << "from file '" << fileUri.getPath() << "'. " + << "Returning null pointer.\n"; + return nullptr; + } + + // Read elements and add all the markers to newSkeleton + ElementEnumerator markerSet(kinematicModelEle, "MarkerSet"); + while (markerSet.next()) + { + const bool result = readMarkerSet(markerSet.get(), newSkeleton, vskData); + + if (!result) + { + dtwarn << "[VskParser::readSkeleton] Failed to parse a marker from " + << "file '" << fileUri.toString() << "'. Ignoring the marker.\n"; + } + } + // TODO: Each Marker is belongs to a MarkerSet but DART doesn't store the + // marker set information. + + // TODO: Read sticks + + // fill in the default if prims absent + generateShapes(newSkeleton, vskData); + + return newSkeleton; +} + +namespace { + +//============================================================================== +bool readParameters(tinyxml2::XMLElement* parametersEle, ParameterMap& paramMap) +{ + if (nullptr == parametersEle) + return false; + + ElementEnumerator param(parametersEle, "Parameter"); + while (param.next()) + { + const std::string pname + = getAttributeString(param.get(), "NAME"); + const double val = getAttributeDouble(param.get(), "VALUE"); + + paramMap[pname] = val; + } + + return true; +} + +//============================================================================== +bool readSkeletonElement(const tinyxml2::XMLElement* skeletonEle, + dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + // Read skeleton and fill the Skeleton* and segmentindex + + skel = dynamics::Skeleton::create(); + + // Read all segments + ConstElementEnumerator segment(skeletonEle, "Segment"); + while (segment.next()) + { + if (!readSegment(segment.get(), nullptr, skel, vskData)) + return false; + } + + return true; +} + +//============================================================================== +double getParameter(const ParameterMap& ParameterMap, + const std::string& paramNameOrValue) +{ + assert(!paramNameOrValue.empty()); + + int sign = 1; + std::string paramNameOrValueWithoutSign = paramNameOrValue; + + if (paramNameOrValueWithoutSign[0] == '-') + { + sign = -1; + paramNameOrValueWithoutSign.erase(paramNameOrValueWithoutSign.begin()); + } + + ParameterMap::const_iterator result + = ParameterMap.find(paramNameOrValueWithoutSign); + const bool found = result != ParameterMap.end(); + + if (found) + return sign * result->second; + else + return toDouble(paramNameOrValue); +} + +//============================================================================== +template +Eigen::Matrix getParameters( + const ParameterMap& ParameterMap, + const std::string& paramNamesOrValues) +{ + std::vector tokens; + tokenize(paramNamesOrValues, tokens); + + assert(tokens.size() == NumParams); + + Eigen::Matrix result; + + for (size_t i = 0; i < NumParams; ++i) + result[i] = getParameter(ParameterMap, tokens[i]); + + return result; +} + +//============================================================================== +template +Eigen::Matrix readAttributeVector( + const tinyxml2::XMLElement* element, + const std::string& name, + const ParameterMap& parameterMap) +{ + const std::string positionStr = getAttributeString(element, name); + + return getParameters(parameterMap, positionStr); +} + +//============================================================================== +bool readSegment(const tinyxml2::XMLElement* segment, + dynamics::BodyNode* parentBodyNode, + const dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + // Attribute: NAME + const std::string name = getAttributeString(segment, "NAME"); + + // Attribute: POSITION + // The position of the segment's joint attaching it to its parent in the + // reference coordinate system of the parent segment. + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + if (hasAttribute(segment, "POSITION")) + { + position = readAttributeVector<3>(segment, "POSITION", + vskData.parameterMap); + position *= vsk_scale; + } + + // Attribute: ORIENTATION + Eigen::Vector3d orientation = Eigen::Vector3d::Zero(); + if (hasAttribute(segment, "ORIENTATION")) + { + orientation = readAttributeVector<3>(segment, "ORIENTATION", + vskData.parameterMap); + } + + // Attribute: BOUNDS + Eigen::Vector6d bounds = Eigen::Vector6d::Zero(); + if (hasAttribute(segment, "BOUNDS")) + bounds = readAttributeVector<6>(segment, "BOUNDS", vskData.parameterMap); + + // Attribute: RGB + Eigen::Vector3d rgb = Eigen::Vector3d(0.5, 0.5, 0.5); + if (hasAttribute(segment, "RGB")) + { + rgb = readAttributeVector<3>(segment, "RGB", vskData.parameterMap); + rgb /= 255.0; + } + + dynamics::BodyNode::Properties bodyNodeProperties; + bodyNodeProperties.mName = name; + + Eigen::Isometry3d tfFromParent; + tfFromParent.translation() = position; + tfFromParent.linear() = math::expMapRot(orientation); + + // Joint + const tinyxml2::XMLElement* jointEle = nullptr; + bool found = false; + JointPropPtr jointProperties; + std::string __jointType; + + for (auto jointType : vskJointTypes) + { + jointEle = segment->FirstChildElement(jointType.c_str()); + if (jointEle) + { + found = true; + __jointType = jointType; + readJoint(jointType, jointEle, jointProperties, tfFromParent, + vskData.parameterMap); + break; + } + } + + if (!found) + { + dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; + return false; + } + + jointProperties->mName = "Joint-" + bodyNodeProperties.mName; + +// // add to the model +// skel->addNode(blink); +// // _segmentindex[sname]=blink->getModelID(); +// segmentindex[name]=blink->getSkelIndex(); + + + // Create joint and body node + dynamics::BodyNode* bodyNode = nullptr; + + if (__jointType == "JointFree") + { + auto pair = createJointAndNodePair( + skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); + bodyNode = pair.second; + } + else if (__jointType == "JointBall") + { + auto pair = createJointAndNodePair( + skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); + bodyNode = pair.second; + } + else if (__jointType == "JointHardySpicer") + { + auto pair = createJointAndNodePair( + skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); + bodyNode = pair.second; + } + else if (__jointType == "JointHinge") + { + auto pair = createJointAndNodePair( + skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); + bodyNode = pair.second; + } + else if (__jointType == "JointDummy") + { + auto pair = createJointAndNodePair( + skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); + bodyNode = pair.second; + } + else + { + dtwarn << "[ParserVsk::readSegment] Attempting to parse unsupported joint " + << "type.\n"; + return false; + } + + vskData.bodyNodeColorMap[bodyNode] = rgb; + + // marker the subtree + ConstElementEnumerator childSegment(segment, "Segment"); + while (childSegment.next()) + { + if (!readSegment(childSegment->ToElement(), bodyNode, skel, vskData)) + return false; + } + return true; +} + +//============================================================================== +bool readJoint(const std::string& jointType, + const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + if (jointType == "JointFree") + { + return readJointFree(jointEle, jointProperties, tfFromParent, + parameterMap); + } + else if (jointType == "JointBall") + { + return readJointBall(jointEle, jointProperties, tfFromParent, + parameterMap); + } + else if (jointType == "JointHardySpicer") + { + return readJointHardySpicer(jointEle, jointProperties, tfFromParent, + parameterMap); + } + else if (jointType == "JointHinge") + { + return readJointHinge(jointEle, jointProperties, tfFromParent, + parameterMap); + } + else if (jointType == "JointDummy") + { + return readJointDummy(jointEle, jointProperties, tfFromParent, + parameterMap); + } + else + { + dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; + return false; + } +} + +//============================================================================== +bool readJointFree(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + dynamics::FreeJoint::Properties properties; + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + + jointProperties + = Eigen::make_aligned_shared( + properties); + + return true; +} + +//============================================================================== +bool readJointBall(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + dynamics::BallJoint::Properties properties; + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + + jointProperties + = Eigen::make_aligned_shared( + properties); + + return true; +} + +//============================================================================== +bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + dynamics::UniversalJoint::Properties properties; + + // Attribute: AXIS-PAIR + Eigen::Vector3d axis1 = Eigen::Vector3d::UnitX(); + Eigen::Vector3d axis2 = Eigen::Vector3d::UnitY(); + if (hasAttribute(jointEle, "AXIS-PAIR")) + { + Eigen::Vector6d axisPair + = readAttributeVector<6>(jointEle, "AXIS-PAIR", parameterMap); + axis1 = axisPair.head<3>(); + axis2 = axisPair.tail<3>(); + } + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + properties.mAxis[0] = axis1; + properties.mAxis[1] = axis2; + + jointProperties + = Eigen::make_aligned_shared( + properties); + + return true; +} + +//============================================================================== +bool readJointHinge(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + dynamics::RevoluteJoint::Properties properties; + + // Attribute: AXIS + Eigen::Vector3d axis = Eigen::Vector3d::UnitX(); + if (hasAttribute(jointEle, "AXIS")) + axis = readAttributeVector<3>(jointEle, "AXIS", parameterMap); + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + properties.mAxis = axis; + + jointProperties + = Eigen::make_aligned_shared( + properties); + + return true; +} + +//============================================================================== +bool readJointDummy(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const ParameterMap& parameterMap) +{ + dynamics::WeldJoint::Properties properties; + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + + jointProperties + = Eigen::make_aligned_shared( + properties); + + return true; +} + +//============================================================================== +bool readMarkerSet(const tinyxml2::XMLElement* markerSetEle, + const dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + // std::string name = getAttributeString(markerSetEle, "NAME"); + + // Read all elements in element + const tinyxml2::XMLElement* markersEle + = markerSetEle->FirstChildElement("Markers"); + ConstElementEnumerator marker(markersEle, "Marker"); + while (marker.next()) + { + if (!readMarker(marker.get(), skel, vskData)) + return false; + } + + // Read all elements in element + const tinyxml2::XMLElement* sticksEle + = markerSetEle->FirstChildElement("Sticks"); + ConstElementEnumerator stick(sticksEle, "Stick"); + while (stick.next()) + { + if (!readStick(stick.get(), skel, vskData)) + return false; + } + + return true; +} + +//============================================================================== +bool readMarker(const tinyxml2::XMLElement* markerEle, + const dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + // Attribute: NAME + const std::string name = getAttributeString(markerEle, "NAME"); + + // Attribute: POSITION + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + if (hasAttribute(markerEle, "POSITION")) + { + position = readAttributeVector<3>(markerEle, "POSITION", + vskData.parameterMap); + position *= vsk_scale; + } + + // Attribute: SEGMENT + const std::string segment = getAttributeString(markerEle, "SEGMENT"); + + // Attribute: COVARIANCE + // Eigen::VectorXd covariance + // = getAttribute(markerEle, "COVARIANCE"); + + // Attribute: RGB + // Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); + // if (hasAttribute(markerEle, "RGB")) + // { + // rgb = getAttributeVector3d(markerEle, "RGB"); + // rgb /= 255.0; + // } + + // Attribute: RADIUS + // double radius = 0.01; + // if (hasAttribute(markerEle, "RADIUS")) + // radius = getAttributeDouble(markerEle, "RADIUS"); + + dynamics::BodyNode* bodyNode = skel->getBodyNode(segment); + if (!bodyNode) + { + dtwarn << "[VskParser::readMarker] Failed to create a Marker [" + << name << ": couldn't find a BodyNode [" << segment << "] in a" + << "Skeleton [" << skel->getName() << "].\n"; + return false; + } + + dynamics::Marker* marker = new dynamics::Marker(name, position, bodyNode); + bodyNode->addMarker(marker); + + return true; +} + +//============================================================================== +bool readStick(const tinyxml2::XMLElement* stickEle, + const dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + std::string marker1 = getAttributeString(stickEle, "MARKER1"); + std::string marker2 = getAttributeString(stickEle, "MARKER2"); + + // Attribute: RGB + Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); + if (hasAttribute(stickEle, "RGB")) + { + rgb = getAttributeVector3d(stickEle, "RGB"); + rgb /= 255.0; + } + + return true; +} + +//============================================================================== +void generateShapes(const dynamics::SkeletonPtr& skel, + VskData& vskData) +{ + // Generate shapes for bodies that have their parents + for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) + { + dynamics::BodyNode* bodyNode = skel->getBodyNode(i); + dynamics::Joint* joint = skel->getJoint(i); + Eigen::Isometry3d tf = joint->getTransformFromParentBodyNode(); + dynamics::BodyNode* parent = bodyNode->getParentBodyNode(); + + // Don't add shape for a body doesn't have parent or a body is too close to + // the parent. + if (!parent || tf.translation().norm() < DART_EPSILON) + continue; + + // Determine the diameters of the ellipsoid shape. The diameter along X-axis + // is the distance between the parent body and the current body. Other + // diameters are 35% of the distance. + Eigen::Vector3d size; + size[0] = tf.translation().norm(); + size[1] = size[2] = 0.35 * size[0]; + + // Determine the local transform of the shape + Eigen::Isometry3d localTransform = Eigen::Isometry3d::Identity(); + localTransform.linear() = math::computeRotation(tf.translation(), + math::AxisType::AXIS_X); + localTransform.translation() = 0.5 * tf.translation(); + + dynamics::ShapePtr shape(new dynamics::EllipsoidShape(size)); + shape->setLocalTransform(localTransform); + shape->setColor(vskData.bodyNodeColorMap[parent]); + + parent->addVisualizationShape(shape); + parent->addCollisionShape(shape); + parent->setLocalCOM(localTransform.translation()); + // TODO(JS): Inertia should support local transform rather than just offset. + + // Update mass + const double density = 2000.0; + // TODO: Add static function that computes mass from density and radii of + // an ellipsoid. + double mass = density * size[0] * size[1] * size[2]; + parent->setMass(mass); + } + + // Generate shpae for bodies with no shape + for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) + { + dynamics::BodyNode* bodyNode = skel->getBodyNode(i); + + if (bodyNode->getNumVisualizationShapes() > 0) + continue; + + // Use hard-coded size ellipsoid + Eigen::Vector3d size = Eigen::Vector3d::Constant(0.05); + + dynamics::ShapePtr shape(new dynamics::EllipsoidShape(size)); + shape->setColor(vskData.bodyNodeColorMap[bodyNode]); + + bodyNode->addVisualizationShape(shape); + bodyNode->addCollisionShape(shape); + + // Update mass + const double density = 2000.0; + // TODO: Add static function that computes mass from density and radii of + // an ellipsoid. + double mass = density * size[0] * size[1] * size[2]; + bodyNode->setMass(mass); + } +} + +//============================================================================== +common::ResourceRetrieverPtr getRetriever( + const common::ResourceRetrieverPtr& retriever) +{ + if (retriever) + return retriever; + else + return std::make_shared(); +} + +//============================================================================== +void tokenize(const std::string& str, + std::vector& tokens, + const std::string& delimiters) +{ + // Skip delimiters at beginning. + std::string::size_type lastPos = str.find_first_not_of(delimiters, 0); + + // Find first "non-delimiter". + std::string::size_type pos = str.find_first_of(delimiters, lastPos); + + while (std::string::npos != pos || std::string::npos != lastPos) + { + // Found a token, add it to the vector. + tokens.push_back(str.substr(lastPos, pos - lastPos)); + + // Skip delimiters. Note the "not_of" + lastPos = str.find_first_not_of(delimiters, pos); + + // Find next "non-delimiter" + pos = str.find_first_of(delimiters, lastPos); + } +} + +} // anonymous namespace + +} // namespace utils +} // namespace dart + diff --git a/dart/utils/ParserVsk.h_ b/dart/utils/VskParser.h similarity index 73% rename from dart/utils/ParserVsk.h_ rename to dart/utils/VskParser.h index e4fe1e6f0f758..eab756a542ebb 100644 --- a/dart/utils/ParserVsk.h_ +++ b/dart/utils/VskParser.h @@ -1,9 +1,9 @@ /* - * Copyright (c) 2011, Georgia Tech Research Corporation + * Copyright (c) 2011-2015, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Sehoon Ha - * Date: 06/12/2011 + * Jeongseok Lee * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -35,17 +35,27 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_KINEMATICS_PARSER_VSK_H -#define DART_KINEMATICS_PARSER_VSK_H +#ifndef DART_UTILS_VSKPARSER_H_ +#define DART_UTILS_VSKPARSER_H_ -namespace kinematics { - class Skeleton; -} // namespace kinematics +#include "dart/common/LocalResourceRetriever.h" +#include "dart/common/Uri.h" +#include "dart/dynamics/Skeleton.h" -#define VERBOSE false -#define VSK_OK 0 -#define VSK_ERROR 1 -int readVSKFile(const char* const filename, kinematics::Skeleton* skel); +namespace dart { +namespace utils { -#endif // #ifndef DART_KINEMATICS_PARSER_VSK_H +class VskParser +{ +public: + /// Read Skeleton from skel file + static dynamics::SkeletonPtr readSkeleton( + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr); +}; + +} // namespace utils +} // namespace dart + +#endif // #ifndef DART_UTILS_VSKPARSER_H_ diff --git a/data/skel_old/Nick01.vsk b/data/vsk/Nick01.vsk similarity index 100% rename from data/skel_old/Nick01.vsk rename to data/vsk/Nick01.vsk diff --git a/data/skel_old/SehoonVSK3.vsk b/data/vsk/SehoonVSK3.vsk similarity index 100% rename from data/skel_old/SehoonVSK3.vsk rename to data/vsk/SehoonVSK3.vsk diff --git a/data/skel_old/Yuting.vsk b/data/vsk/Yuting.vsk similarity index 100% rename from data/skel_old/Yuting.vsk rename to data/vsk/Yuting.vsk diff --git a/data/vsk/test/empty.vsk b/data/vsk/test/empty.vsk new file mode 100644 index 0000000000000..44315a0d0f1e3 --- /dev/null +++ b/data/vsk/test/empty.vsk @@ -0,0 +1,3 @@ + + + diff --git a/unittests/testVskParser.cpp b/unittests/testVskParser.cpp new file mode 100644 index 0000000000000..b03d5b02abdc2 --- /dev/null +++ b/unittests/testVskParser.cpp @@ -0,0 +1,111 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include +#include +#include "TestHelpers.h" + +#include "tinyxml2.h" + +#include "dart/dynamics/SoftBodyNode.h" +#include "dart/dynamics/RevoluteJoint.h" +#include "dart/dynamics/PlanarJoint.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/simulation/World.h" +#include "dart/simulation/World.h" +#include "dart/utils/VskParser.h" + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace utils; + +//============================================================================== +TEST(VskParser, EmptySkeleton) +{ + WorldPtr world(new World()); + SkeletonPtr skeleton + = VskParser::readSkeleton(DART_DATA_PATH"vsk/test/empty.vsk"); + world->addSkeleton(skeleton); + + EXPECT_TRUE(world != nullptr); + EXPECT_TRUE(skeleton == nullptr); + EXPECT_EQ(world->getNumSkeletons(), 0u); + + world->step(); +} + +//============================================================================== +TEST(VskParser, SingleStepSimulations) +{ + WorldPtr world(new World()); + EXPECT_NE(world , nullptr); + + SkeletonPtr nick + = VskParser::readSkeleton(DART_DATA_PATH"vsk/Nick01.vsk"); + EXPECT_NE(nick , nullptr); + + SkeletonPtr sehoon + = VskParser::readSkeleton(DART_DATA_PATH"vsk/SehoonVSK3.vsk"); + EXPECT_NE(sehoon, nullptr); + + SkeletonPtr yuting + = VskParser::readSkeleton(DART_DATA_PATH"vsk/Yuting.vsk"); + EXPECT_NE(yuting, nullptr); + + world->removeAllSkeletons(); + world->addSkeleton(nick); + EXPECT_EQ(world->getNumSkeletons(), 1u); + world->step(); + + world->removeAllSkeletons(); + world->addSkeleton(sehoon); + EXPECT_EQ(world->getNumSkeletons(), 1u); + world->step(); + + world->removeAllSkeletons(); + world->addSkeleton(yuting); + EXPECT_EQ(world->getNumSkeletons(), 1u); + world->step(); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 5dc38cfb09fb187ba93c3dcba3a48015197174d7 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 27 Nov 2015 08:14:18 -0500 Subject: [PATCH 46/79] Update APIs of shape classes - Remove initMeshes() since it's not used anywhere - Rename updateVolume() to computeVolume() to match the naming convention - Add static methods computing volume and moments of inertia for shape whoes volume is not zero --- dart/dynamics/BoxShape.cpp | 44 +++++++++++++++++++--------- dart/dynamics/BoxShape.h | 13 +++++++-- dart/dynamics/CylinderShape.cpp | 38 +++++++++++++++++------- dart/dynamics/CylinderShape.h | 11 +++++-- dart/dynamics/EllipsoidShape.cpp | 35 +++++++++++++++++------ dart/dynamics/EllipsoidShape.h | 11 +++++-- dart/dynamics/LineSegmentShape.cpp | 8 +++--- dart/dynamics/LineSegmentShape.h | 4 +-- dart/dynamics/MeshShape.cpp | 6 ++-- dart/dynamics/MeshShape.h | 4 +-- dart/dynamics/PlaneShape.cpp | 7 ++--- dart/dynamics/PlaneShape.h | 4 +-- dart/dynamics/Shape.h | 23 ++++++++++++--- dart/dynamics/SoftMeshShape.cpp | 6 ++-- dart/dynamics/SoftMeshShape.h | 4 +-- dart/utils/VskParser.cpp | 46 +++++++++++++++++++++--------- dart/utils/VskParser.h | 2 +- dart/utils/XmlHelpers.h | 4 +-- 18 files changed, 189 insertions(+), 81 deletions(-) diff --git a/dart/dynamics/BoxShape.cpp b/dart/dynamics/BoxShape.cpp index 11ef3fa0d0d03..80dbd6c2ef5a2 100644 --- a/dart/dynamics/BoxShape.cpp +++ b/dart/dynamics/BoxShape.cpp @@ -35,8 +35,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include "dart/dynamics/BoxShape.h" - #include "dart/renderer/RenderInterface.h" namespace dart { @@ -49,20 +49,38 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size) assert(_size[1] > 0.0); assert(_size[2] > 0.0); mBoundingBoxDim = _size; - initMeshes(); - computeVolume(); + updateVolume(); } BoxShape::~BoxShape() { } +//============================================================================== +double BoxShape::computeVolume(const Eigen::Vector3d& size) +{ + return size[0] * size[1] * size[2]; +} + +//============================================================================== +Eigen::Matrix3d BoxShape::computeInertia(const Eigen::Vector3d& size, + double mass) +{ + Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); + + inertia(0, 0) = mass / 12.0 * (std::pow(size[1], 2) + std::pow(size[2], 2)); + inertia(1, 1) = mass / 12.0 * (std::pow(size[0], 2) + std::pow(size[2], 2)); + inertia(2, 2) = mass / 12.0 * (std::pow(size[0], 2) + std::pow(size[1], 2)); + + return inertia; +} + void BoxShape::setSize(const Eigen::Vector3d& _size) { assert(_size[0] > 0.0); assert(_size[1] > 0.0); assert(_size[2] > 0.0); mSize = _size; mBoundingBoxDim = _size; - computeVolume(); + updateVolume(); } const Eigen::Vector3d& BoxShape::getSize() const { @@ -84,18 +102,16 @@ void BoxShape::draw(renderer::RenderInterface* _ri, _ri->popMatrix(); } -Eigen::Matrix3d BoxShape::computeInertia(double _mass) const { - Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); - inertia(0, 0) = _mass / 12.0 * (mSize(1) * mSize(1) + mSize(2) * mSize(2)); - inertia(1, 1) = _mass / 12.0 * (mSize(0) * mSize(0) + mSize(2) * mSize(2)); - inertia(2, 2) = _mass / 12.0 * (mSize(0) * mSize(0) + mSize(1) * mSize(1)); - - return inertia; +//============================================================================== +Eigen::Matrix3d BoxShape::computeInertia(double mass) const +{ + return computeInertia(mSize, mass); } -void BoxShape::computeVolume() { - // a * b * c - mVolume = mSize[0] * mSize[1] * mSize[2]; +//============================================================================== +void BoxShape::updateVolume() +{ + mVolume = computeVolume(mSize); } } // namespace dynamics diff --git a/dart/dynamics/BoxShape.h b/dart/dynamics/BoxShape.h index 22d0184360506..f8ac14ea91d1e 100644 --- a/dart/dynamics/BoxShape.h +++ b/dart/dynamics/BoxShape.h @@ -62,15 +62,22 @@ class BoxShape : public Shape { const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _default = true) const; + /// \brief Compute volume from given properties + static double computeVolume(const Eigen::Vector3d& size); + + /// \brief Compute moments of inertia of a box + static Eigen::Matrix3d computeInertia(const Eigen::Vector3d& size, + double mass); + // Documentation inherited. - virtual Eigen::Matrix3d computeInertia(double _mass) const; + Eigen::Matrix3d computeInertia(double mass) const override; protected: // Documentation inherited. - void computeVolume(); + void updateVolume() override; private: - /// \brief Size of this box + /// \brief Side lengths of the box Eigen::Vector3d mSize; public: diff --git a/dart/dynamics/CylinderShape.cpp b/dart/dynamics/CylinderShape.cpp index dc23d9419bb1c..0a5716a9f9108 100644 --- a/dart/dynamics/CylinderShape.cpp +++ b/dart/dynamics/CylinderShape.cpp @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include "dart/dynamics/CylinderShape.h" - #include "dart/renderer/RenderInterface.h" namespace dart { @@ -48,8 +48,7 @@ CylinderShape::CylinderShape(double _radius, double _height) assert(0.0 < _radius); assert(0.0 < _height); _updateBoundingBoxDim(); - initMeshes(); - computeVolume(); + updateVolume(); } double CylinderShape::getRadius() const { @@ -60,7 +59,7 @@ void CylinderShape::setRadius(double _radius) { assert(0.0 < _radius); mRadius = _radius; _updateBoundingBoxDim(); - computeVolume(); + updateVolume(); } double CylinderShape::getHeight() const { @@ -71,7 +70,7 @@ void CylinderShape::setHeight(double _height) { assert(0.0 < _height); mHeight = _height; _updateBoundingBoxDim(); - computeVolume(); + updateVolume(); } void CylinderShape::draw(renderer::RenderInterface* _ri, @@ -89,19 +88,38 @@ void CylinderShape::draw(renderer::RenderInterface* _ri, _ri->popMatrix(); } -void CylinderShape::computeVolume() { - mVolume = DART_PI * mRadius * mRadius * mHeight; +//============================================================================== +double CylinderShape::computeVolume(double radius, double height) +{ + return DART_PI * std::pow(radius, 2) * height; } -Eigen::Matrix3d CylinderShape::computeInertia(double _mass) const { +//============================================================================== +Eigen::Matrix3d CylinderShape::computeInertia( + double radius, double height, double mass) +{ Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero(); - inertia(0, 0) = _mass * (3.0 * mRadius * mRadius + mHeight * mHeight) / 12.0; + + inertia(0, 0) = mass * (3.0 * std::pow(radius, 2) + std::pow(height, 2)) + / 12.0; inertia(1, 1) = inertia(0, 0); - inertia(2, 2) = 0.5 * _mass * mRadius * mRadius; + inertia(2, 2) = 0.5 * mass * radius * radius; return inertia; } +//============================================================================== +void CylinderShape::updateVolume() +{ + mVolume = computeVolume(mRadius, mHeight); +} + +//============================================================================== +Eigen::Matrix3d CylinderShape::computeInertia(double mass) const +{ + return computeInertia(mRadius, mHeight, mass); +} + void CylinderShape::_updateBoundingBoxDim() { mBoundingBoxDim[0] = mRadius * 2.0; mBoundingBoxDim[1] = mRadius * 2.0; diff --git a/dart/dynamics/CylinderShape.h b/dart/dynamics/CylinderShape.h index 6e7a857ab6cdf..06b74ca89c632 100644 --- a/dart/dynamics/CylinderShape.h +++ b/dart/dynamics/CylinderShape.h @@ -67,12 +67,19 @@ class CylinderShape : public Shape { const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; + /// \brief Compute volume from given properties + static double computeVolume(double radius, double height); + + /// \brief Compute moments of inertia of a cylinder + static Eigen::Matrix3d computeInertia( + double radius, double height, double mass); + // Documentation inherited. - virtual Eigen::Matrix3d computeInertia(double _mass) const; + Eigen::Matrix3d computeInertia(double mass) const override; protected: // Documentation inherited. - virtual void computeVolume(); + void updateVolume() override; private: /// \brief diff --git a/dart/dynamics/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp index 4e7efb9231fdb..5616670248f8f 100644 --- a/dart/dynamics/EllipsoidShape.cpp +++ b/dart/dynamics/EllipsoidShape.cpp @@ -45,7 +45,6 @@ namespace dynamics { EllipsoidShape::EllipsoidShape(const Eigen::Vector3d& _size) : Shape(ELLIPSOID) { setSize(_size); - initMeshes(); } EllipsoidShape::~EllipsoidShape() { @@ -57,7 +56,7 @@ void EllipsoidShape::setSize(const Eigen::Vector3d& _size) { assert(_size[2] > 0.0); mSize = _size; mBoundingBoxDim = _size; - computeVolume(); + updateVolume(); } const Eigen::Vector3d&EllipsoidShape::getSize() const { @@ -81,15 +80,32 @@ void EllipsoidShape::draw(renderer::RenderInterface* _ri, _ri->popMatrix(); } -Eigen::Matrix3d EllipsoidShape::computeInertia(double _mass) const { +//============================================================================== +double EllipsoidShape::computeVolume(const Eigen::Vector3d& size) +{ + // 4/3* Pi* a/2* b/2* c/2 + return DART_PI * size[0] * size[1] * size[2] / 6.0; +} + +//============================================================================== +Eigen::Matrix3d EllipsoidShape::computeInertia( + const Eigen::Vector3d& size, double mass) +{ Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); - inertia(0, 0) = _mass / 20.0 * (mSize(1) * mSize(1) + mSize(2) * mSize(2)); - inertia(1, 1) = _mass / 20.0 * (mSize(0) * mSize(0) + mSize(2) * mSize(2)); - inertia(2, 2) = _mass / 20.0 * (mSize(0) * mSize(0) + mSize(1) * mSize(1)); + + inertia(0, 0) = mass / 20.0 * (std::pow(size[1], 2) + std::pow(size[2], 2)); + inertia(1, 1) = mass / 20.0 * (std::pow(size[0], 2) + std::pow(size[2], 2)); + inertia(2, 2) = mass / 20.0 * (std::pow(size[0], 2) + std::pow(size[1], 2)); return inertia; } +//============================================================================== +Eigen::Matrix3d EllipsoidShape::computeInertia(double mass) const +{ + return computeInertia(mSize, mass); +} + bool EllipsoidShape::isSphere() const { if (mSize[0] == mSize[1] && mSize[1] == mSize[2]) return true; @@ -97,9 +113,10 @@ bool EllipsoidShape::isSphere() const { return false; } -void EllipsoidShape::computeVolume() { - // 4/3* Pi* a/2* b/2* c/2 - mVolume = DART_PI * mSize(0) * mSize(1) *mSize(2) / 6; +//============================================================================== +void EllipsoidShape::updateVolume() +{ + mVolume = computeVolume(mSize); } } // namespace dynamics diff --git a/dart/dynamics/EllipsoidShape.h b/dart/dynamics/EllipsoidShape.h index 3368e5cb7d707..e2c274ba57aa3 100644 --- a/dart/dynamics/EllipsoidShape.h +++ b/dart/dynamics/EllipsoidShape.h @@ -62,15 +62,22 @@ class EllipsoidShape : public Shape { const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(), bool _useDefaultColor = true) const; + /// \brief Compute volume from given properties + static double computeVolume(const Eigen::Vector3d& size); + + /// \brief Compute moments of inertia of a ellipsoid + static Eigen::Matrix3d computeInertia( + const Eigen::Vector3d& size, double mass); + // Documentation inherited. - virtual Eigen::Matrix3d computeInertia(double _mass) const; + Eigen::Matrix3d computeInertia(double mass) const override; /// \brief True if (mDim[0] == mDim[1] == mDim[2]). bool isSphere(void) const; protected: // Documentation inherited. - void computeVolume(); + void updateVolume() override; private: /// \brief Size of this ellipsoid diff --git a/dart/dynamics/LineSegmentShape.cpp b/dart/dynamics/LineSegmentShape.cpp index d7af0938712f7..3113f51f5c10e 100644 --- a/dart/dynamics/LineSegmentShape.cpp +++ b/dart/dynamics/LineSegmentShape.cpp @@ -56,7 +56,7 @@ LineSegmentShape::LineSegmentShape(float _thickness) mThickness = 1.0f; } - computeVolume(); + updateVolume(); mVariance = DYNAMIC_VERTICES; } @@ -77,7 +77,7 @@ LineSegmentShape::LineSegmentShape(const Eigen::Vector3d& _v1, addVertex(_v1); addVertex(_v2); - computeVolume(); + updateVolume(); mVariance = DYNAMIC_VERTICES; } @@ -362,9 +362,9 @@ Eigen::Matrix3d LineSegmentShape::computeInertia(double _mass) const } //============================================================================== -void LineSegmentShape::computeVolume() +void LineSegmentShape::updateVolume() { - mVolume = 0; + mVolume = 0.0; } } // namespace dynamics diff --git a/dart/dynamics/LineSegmentShape.h b/dart/dynamics/LineSegmentShape.h index 847cfa46919f1..bc220ea039def 100644 --- a/dart/dynamics/LineSegmentShape.h +++ b/dart/dynamics/LineSegmentShape.h @@ -107,14 +107,14 @@ class LineSegmentShape : public Shape /// The returned inertia matrix will be like a very thin cylinder. The _mass /// will be evenly distributed across all lines. - virtual Eigen::Matrix3d computeInertia(double _mass) const override; + virtual Eigen::Matrix3d computeInertia(double mass) const override; // TODO(MXG): Consider supporting colors-per-vertex protected: // Documentation inherited - void computeVolume() override; + void updateVolume() override; /// Line thickness for rendering float mThickness; diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 0621a1edbb586..e66595cedaa8b 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -214,7 +214,7 @@ void MeshShape::setMesh( mResourceRetriever = _resourceRetriever; _updateBoundingBoxDim(); - computeVolume(); + updateVolume(); } void MeshShape::setScale(const Eigen::Vector3d& _scale) { @@ -222,7 +222,7 @@ void MeshShape::setScale(const Eigen::Vector3d& _scale) { assert(_scale[1] > 0.0); assert(_scale[2] > 0.0); mScale = _scale; - computeVolume(); + updateVolume(); } const Eigen::Vector3d& MeshShape::getScale() const { @@ -291,7 +291,7 @@ Eigen::Matrix3d MeshShape::computeInertia(double _mass) const { return inertia; } -void MeshShape::computeVolume() { +void MeshShape::updateVolume() { // Use bounding box to represent the mesh double l = mScale[0] * mBoundingBoxDim[0]; double h = mScale[1] * mBoundingBoxDim[1]; diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 1e02cc2328bd5..c1905342bfa5b 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -139,11 +139,11 @@ class MeshShape : public Shape { const std::string& _uri, const common::ResourceRetrieverPtr& _retriever); // Documentation inherited. - Eigen::Matrix3d computeInertia(double _mass) const override; + Eigen::Matrix3d computeInertia(double mass) const override; protected: // Documentation inherited. - void computeVolume() override; + void updateVolume() override; private: /// \brief diff --git a/dart/dynamics/PlaneShape.cpp b/dart/dynamics/PlaneShape.cpp index a1d3fc6213adc..eb57d36ef6ef9 100644 --- a/dart/dynamics/PlaneShape.cpp +++ b/dart/dynamics/PlaneShape.cpp @@ -77,10 +77,9 @@ void PlaneShape::draw(renderer::RenderInterface* _ri, } //============================================================================== -Eigen::Matrix3d PlaneShape::computeInertia(double _mass) const +Eigen::Matrix3d PlaneShape::computeInertia(double /*mass*/) const { - Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero(); - return inertia; + return Eigen::Matrix3d::Zero(); } //============================================================================== @@ -136,7 +135,7 @@ double PlaneShape::computeSignedDistance(const Eigen::Vector3d& _point) const } //============================================================================== -void PlaneShape::computeVolume() +void PlaneShape::updateVolume() { mVolume = 0.0; } diff --git a/dart/dynamics/PlaneShape.h b/dart/dynamics/PlaneShape.h index e077dc500b15e..b5e4798d5964d 100644 --- a/dart/dynamics/PlaneShape.h +++ b/dart/dynamics/PlaneShape.h @@ -59,7 +59,7 @@ class PlaneShape : public Shape bool _default = true) const; // Documentation inherited. - virtual Eigen::Matrix3d computeInertia(double _mass) const; + Eigen::Matrix3d computeInertia(double mass) const override; /// Set plane normal void setNormal(const Eigen::Vector3d& _normal); @@ -88,7 +88,7 @@ class PlaneShape : public Shape private: // Documentation inherited. - void computeVolume(); + void updateVolume() override; /// Plane normal Eigen::Vector3d mNormal; diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 2ab0f08ce10ac..a9b8822683088 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -43,6 +43,7 @@ #include #include "dart/math/Geometry.h" +#include "dart/common/Deprecated.h" #include "dart/common/Subject.h" #include "dart/dynamics/SmartPointer.h" @@ -140,7 +141,18 @@ class Shape : public virtual common::Subject Eigen::Vector3d getOffset() const; /// \brief - virtual Eigen::Matrix3d computeInertia(double _mass) const = 0; + virtual Eigen::Matrix3d computeInertia(double mass) const = 0; + + Eigen::Matrix3d computeInertiaFromDensity(double density) const + { +// return computeInertiaFromMass(density * computeVolume()); + return Eigen::Matrix3d(); + } + + virtual Eigen::Matrix3d computeInertiaFromMass(double density) const + { + return Eigen::Matrix3d(); + } /// \brief Get volume of this shape. /// The volume will be automatically calculated by the sub-classes @@ -189,10 +201,13 @@ class Shape : public virtual common::Subject bool isHidden() const; protected: - /// \brief - virtual void computeVolume() = 0; + DEPRECATED(6.0) + virtual void computeVolume() { updateVolume(); } - /// \brief + /// \brief Update volume + virtual void updateVolume() = 0; + + DEPRECATED(6.0) virtual void initMeshes() {} /// \brief Dimensions for bounding box. diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp index 1c7673ff08298..c22f5cfa56c93 100644 --- a/dart/dynamics/SoftMeshShape.cpp +++ b/dart/dynamics/SoftMeshShape.cpp @@ -71,9 +71,11 @@ const SoftBodyNode* SoftMeshShape::getSoftBodyNode() const return mSoftBodyNode; } -Eigen::Matrix3d SoftMeshShape::computeInertia(double _mass) const +Eigen::Matrix3d SoftMeshShape::computeInertia(double mass) const { + dtwarn << "[SoftMeshShape::computeInertia] Not implemented yet.\n"; // TODO(JS): Not implemented. + return Eigen::Matrix3d(); } @@ -84,7 +86,7 @@ void SoftMeshShape::draw(renderer::RenderInterface* _ri, // TODO(JS): Not implemented. } -void SoftMeshShape::computeVolume() +void SoftMeshShape::updateVolume() { // TODO(JS): Not implemented. } diff --git a/dart/dynamics/SoftMeshShape.h b/dart/dynamics/SoftMeshShape.h index 2d5b07cd286ad..36f2439ae5e67 100644 --- a/dart/dynamics/SoftMeshShape.h +++ b/dart/dynamics/SoftMeshShape.h @@ -66,7 +66,7 @@ class SoftMeshShape : public Shape void update(); // Documentation inherited. - virtual Eigen::Matrix3d computeInertia(double _mass) const; + Eigen::Matrix3d computeInertia(double mass) const override; // Documentation inherited. virtual void draw( @@ -76,7 +76,7 @@ class SoftMeshShape : public Shape protected: // Documentation inherited. - virtual void computeVolume(); + void updateVolume() override; private: /// \brief Build mesh using SoftBodyNode data diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index 29aad02f61090..9222d8b22278a 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -765,15 +765,15 @@ void generateShapes(const dynamics::SkeletonPtr& skel, parent->addVisualizationShape(shape); parent->addCollisionShape(shape); - parent->setLocalCOM(localTransform.translation()); - // TODO(JS): Inertia should support local transform rather than just offset. - // Update mass - const double density = 2000.0; - // TODO: Add static function that computes mass from density and radii of - // an ellipsoid. - double mass = density * size[0] * size[1] * size[2]; - parent->setMass(mass); + // Update inertia + const double density = 2.0e+3; + const double mass = shape->getVolume() * density; + const Eigen::Matrix3d moi = shape->computeInertia(mass); + const dynamics::Inertia inertia(mass, localTransform.translation(), moi); + // TODO(JS): Once Inertia supports transform, pass the localTransform to the + // inertia. See #234. + parent->setInertia(inertia); } // Generate shpae for bodies with no shape @@ -794,11 +794,31 @@ void generateShapes(const dynamics::SkeletonPtr& skel, bodyNode->addCollisionShape(shape); // Update mass - const double density = 2000.0; - // TODO: Add static function that computes mass from density and radii of - // an ellipsoid. - double mass = density * size[0] * size[1] * size[2]; - bodyNode->setMass(mass); + const double density = 1.0e+3; + const double mass = shape->getVolume() * density; + const Eigen::Matrix3d moi = shape->computeInertia(mass); + const dynamics::Inertia inertia(mass, Eigen::Vector3d::Zero(), moi); + bodyNode->setInertia(inertia); + } + + // Update mass and moments of inertia of the bodies based on the generated + // shapes into them. + double density = 1.0e+3; + for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) + { + dynamics::BodyNode* bodyNode = skel->getBodyNode(i); + + // Now all the bodies should have at least one shape + assert(bodyNode->getNumVisualizationShapes() > 0); + + double mass = 0.0; + Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); + auto shapes = bodyNode->getVisualizationShapes(); + for (const dynamics::ShapePtr& shape : shapes) + { + mass += density * shape->getVolume(); + inertia += shape->computeInertia(mass); + } } } diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index eab756a542ebb..afcf7414d6da9 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -48,7 +48,7 @@ namespace utils { class VskParser { public: - /// Read Skeleton from skel file + /// Read Skeleton from VSK file static dynamics::SkeletonPtr readSkeleton( const common::Uri& fileUri, const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr); diff --git a/dart/utils/XmlHelpers.h b/dart/utils/XmlHelpers.h index 58f0477f7e09a..9213b59ed80e1 100644 --- a/dart/utils/XmlHelpers.h +++ b/dart/utils/XmlHelpers.h @@ -136,8 +136,8 @@ class TemplatedElementEnumerator { protected: - using ElementPtr = typename std::add_pointer::type; - using ElementRef = typename std::add_lvalue_reference::type; + using ElementPtr = ElementType*; + using ElementRef = ElementType&; public: From 37cd00ac426f2e4427fdcdf1430cf8bdcb1e514a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 28 Nov 2015 01:00:16 -0500 Subject: [PATCH 47/79] Add color property to Marker --- dart/dynamics/Marker.cpp | 154 +++++++++++++++++++++++++-------------- dart/dynamics/Marker.h | 88 ++++++++++++---------- dart/utils/VskParser.cpp | 46 +++++++----- 3 files changed, 180 insertions(+), 108 deletions(-) diff --git a/dart/dynamics/Marker.cpp b/dart/dynamics/Marker.cpp index cfbc43a99f9e5..ca8f52ee6e735 100644 --- a/dart/dynamics/Marker.cpp +++ b/dart/dynamics/Marker.cpp @@ -46,118 +46,164 @@ namespace dynamics { int Marker::msMarkerCount = 0; -Marker::Properties::Properties(const std::string& _name, - const Eigen::Vector3d& _offset, - ConstraintType _type) - : mName(_name), - mOffset(_offset), - mType(_type) +//============================================================================== +Marker::Properties::Properties(const std::string& name, + const Eigen::Vector3d& offset, + const Eigen::Vector4d& color, + ConstraintType type) + : mName(name), mOffset(offset), mColor(color), mType(type) { // Do nothing } -Marker::Marker(const std::string& _name, const Eigen::Vector3d& _offset, - BodyNode* _bodyNode, ConstraintType _type) - : mProperties(_name, _offset, _type), - mBodyNode(_bodyNode), - mSkelIndex(0) +//============================================================================== +Marker::Marker(const std::string& name, + const Eigen::Vector3d& offset, + const Eigen::Vector4d& color, + BodyNode* bodyNode, + ConstraintType type) + : mProperties(name, offset, color, type), + mBodyNode(bodyNode), + mSkelIndex(0), + mID(Marker::msMarkerCount++) { - mID = Marker::msMarkerCount++; + // Do nothing } +//============================================================================== Marker::~Marker() { // Do nothing } -void Marker::draw(renderer::RenderInterface* _ri, bool _offset, - const Eigen::Vector4d& _color, bool _useDefaultColor) const { - if (!_ri) +//============================================================================== +void Marker::draw(renderer::RenderInterface* ri, + bool offset, + const Eigen::Vector4d& color, + bool useDefaultColor) const +{ + if (!ri) return; - _ri->pushName(getID()); + ri->pushName(getID()); - if (mProperties.mType == HARD) { - _ri->setPenColor(Eigen::Vector3d(1, 0, 0)); - } else if (mProperties.mType == SOFT) { - _ri->setPenColor(Eigen::Vector3d(0, 1, 0)); - } else { - if (_useDefaultColor) - _ri->setPenColor(Eigen::Vector3d(0, 0, 1)); + if (mProperties.mType == HARD) + { + ri->setPenColor(Color::Red(1.0)); + } + else if (mProperties.mType == SOFT) + { + ri->setPenColor(Color::Green(1.0)); + } + else + { + if (useDefaultColor) + ri->setPenColor(mProperties.mColor); else - _ri->setPenColor(Eigen::Vector4d( - _color[0], _color[1], _color[2], _color[3])); + ri->setPenColor(color); } - if (_offset) { - _ri->pushMatrix(); - _ri->translate(mProperties.mOffset); - _ri->drawEllipsoid(Eigen::Vector3d(0.01, 0.01, 0.01)); - _ri->popMatrix(); - } else { - _ri->drawEllipsoid(Eigen::Vector3d(0.01, 0.01, 0.01)); + if (offset) + { + ri->pushMatrix(); + ri->translate(mProperties.mOffset); + ri->drawEllipsoid(Eigen::Vector3d::Constant(0.01)); + ri->popMatrix(); + } + else + { + ri->drawEllipsoid(Eigen::Vector3d::Constant(0.01)); } - _ri->popName(); + ri->popName(); } +//============================================================================== BodyNode* Marker::getBodyNode() { return mBodyNode; } -const BodyNode* Marker::getBodyNode() const { +//============================================================================== +const BodyNode* Marker::getBodyNode() const +{ return mBodyNode; } -const Eigen::Vector3d& Marker::getLocalPosition() const { +//============================================================================== +const Eigen::Vector3d& Marker::getLocalPosition() const +{ return mProperties.mOffset; } -void Marker::setLocalPosition(const Eigen::Vector3d& _offset) { - mProperties.mOffset = _offset; +//============================================================================== +void Marker::setLocalPosition(const Eigen::Vector3d& offset) +{ + mProperties.mOffset = offset; } -Eigen::Vector3d Marker::getWorldPosition() const { +//============================================================================== +Eigen::Vector3d Marker::getWorldPosition() const +{ return mBodyNode->getTransform() * mProperties.mOffset; } -int Marker::getIndexInSkeleton() const { - return mSkelIndex; +//============================================================================== +void Marker::setSkeletonIndex(int index) +{ + setIndexInSkeleton(index); } -void Marker::setSkeletonIndex(int _idx) { - mSkelIndex = _idx; +//============================================================================== +void Marker::setIndexInSkeleton(int index) +{ + mSkelIndex = index; +} + +//============================================================================== +int Marker::getIndexInSkeleton() const +{ + return mSkelIndex; } -int Marker::getID() const { +//============================================================================== +int Marker::getID() const +{ return mID; } -void Marker::setName(const std::string& _name) +//============================================================================== +void Marker::setName(const std::string& name) { - mProperties.mName = _name; + mProperties.mName = name; } +//============================================================================== const std::string& Marker::getName() const { return mProperties.mName; } -Marker::ConstraintType Marker::getConstraintType() const { - return mProperties.mType; +//============================================================================== +void Marker::setConstraintType(Marker::ConstraintType type) +{ + mProperties.mType = type; } -void Marker::setConstraintType(Marker::ConstraintType _type) { - mProperties.mType = _type; +//============================================================================== +Marker::ConstraintType Marker::getConstraintType() const +{ + return mProperties.mType; } -Marker::Marker(const Properties& _properties, BodyNode* _parent) - : mProperties(_properties), - mBodyNode(_parent), - mSkelIndex(0) +//============================================================================== +Marker::Marker(const Properties& properties, BodyNode* parent) + : mProperties(properties), + mBodyNode(parent), + mSkelIndex(0), + mID(Marker::msMarkerCount++) { - mID = Marker::msMarkerCount++; + // Do nothing } } // namespace dynamics diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index fbfc86d3f5b85..92bce37560741 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -38,8 +38,8 @@ #define DART_DYNAMICS_MARKER_H_ #include - #include +#include "dart/math/Helpers.h" namespace dart { namespace renderer { @@ -52,9 +52,12 @@ namespace dynamics { class BodyNode; -class Marker { +class Marker +{ public: - enum ConstraintType { + + enum ConstraintType + { NO, HARD, SOFT @@ -64,24 +67,33 @@ class Marker { { std::string mName; Eigen::Vector3d mOffset; + Eigen::Vector4d mColor; ConstraintType mType; - Properties(const std::string& _name = "", - const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(), - ConstraintType _type = NO); + Properties(const std::string& name = "", + const Eigen::Vector3d& offset = Eigen::Vector3d::Zero(), + const Eigen::Vector4d& color = Color::White(1.0), + ConstraintType type = NO); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - /// \brief - Marker(const std::string& _name, const Eigen::Vector3d& _offset, - BodyNode* _bodyNode, ConstraintType _type = NO); + /// Constructor + Marker(const std::string& name, + const Eigen::Vector3d& offset, + const Eigen::Vector4d& color, + BodyNode* bodyNode, + ConstraintType type = NO); - /// \brief + /// Destructor virtual ~Marker(); - /// \brief - void draw(renderer::RenderInterface* _ri = nullptr, bool _offset = true, - const Eigen::Vector4d& _color = Eigen::Vector4d::Identity(), - bool _useDefaultColor = true) const; + /// Render this marker + void draw(renderer::RenderInterface* ri = nullptr, + bool offset = true, + const Eigen::Vector4d& color = Color::White(1.0), + bool useDefaultColor = true) const; /// Get the BodyNode this Marker belongs to BodyNode* getBodyNode(); @@ -89,36 +101,41 @@ class Marker { /// Get the (const) BodyNode this Marker belongs to const BodyNode* getBodyNode() const; - /// \brief + /// Get position of this marker in the parent body node coordinates const Eigen::Vector3d& getLocalPosition() const; - /// \brief - void setLocalPosition(const Eigen::Vector3d& _offset); + /// Set position of this marker in the parent body node coordinates + void setLocalPosition(const Eigen::Vector3d& offset); - /// \brief Get position w.r.t. world frame + /// Get position in the world coordinates Eigen::Vector3d getWorldPosition() const; - /// \brief - int getIndexInSkeleton() const; + /// Deprecated; please use setIndexInSkeleton() instead + DEPRECATED(6.0) + void setSkeletonIndex(int index); - /// \brief - void setSkeletonIndex(int _idx); + /// Set index in skeleton this marker is belongs to + void setIndexInSkeleton(int index); + // TODO(JS): This function is not called by any. Remove? - /// \brief + /// Get index in skeleton this marker is belongs to + int getIndexInSkeleton() const; + // TODO(JS): This function is not called by any. Remove? + + /// Get global unique ID int getID() const; - /// \brief - void setName(const std::string&); + /// Set name of this marker + void setName(const std::string& name); - /// \brief + /// Get name of this marker const std::string& getName() const; - // useful for IK - /// \brief - ConstraintType getConstraintType() const; + /// Set constraint type. which will be useful for inverse kinematics + void setConstraintType(ConstraintType type); - /// \brief - void setConstraintType(ConstraintType _type); + /// Get constraint type. which will be useful for inverse kinematics + ConstraintType getConstraintType() const; friend class Skeleton; friend class BodyNode; @@ -126,7 +143,7 @@ class Marker { protected: /// Constructor used by BodyNode - Marker(const Properties& _properties, BodyNode* _parent); + Marker(const Properties& properties, BodyNode* parent); /// \brief Properties of this Marker Properties mProperties; @@ -138,15 +155,12 @@ class Marker { int mSkelIndex; private: - /// \brief a unique ID of this marker globally. + /// Unique ID of this marker globally. int mID; - /// \brief counts the number of markers globally. + /// Counts the number of markers globally. static int msMarkerCount; -public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace dynamics diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index 9222d8b22278a..46f7016e8790f 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -43,10 +43,6 @@ #include #include -// TinyXML-2 Library -// http://www.grinninglizard.com/tinyxml2/index.html -#include - #include // Local Files @@ -126,7 +122,7 @@ bool readJointDummy(const tinyxml2::XMLElement* jointEle, const ParameterMap& parameterMap); template -std::pair createJointAndNodePair( +std::pair createJointAndBodyNodePair( const dynamics::SkeletonPtr& skeleton, dynamics::BodyNode* parentBodyNode, const dynamics::Joint::Properties* jointProperties, @@ -433,31 +429,31 @@ bool readSegment(const tinyxml2::XMLElement* segment, if (__jointType == "JointFree") { - auto pair = createJointAndNodePair( + auto pair = createJointAndBodyNodePair( skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); bodyNode = pair.second; } else if (__jointType == "JointBall") { - auto pair = createJointAndNodePair( + auto pair = createJointAndBodyNodePair( skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); bodyNode = pair.second; } else if (__jointType == "JointHardySpicer") { - auto pair = createJointAndNodePair( + auto pair = createJointAndBodyNodePair( skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); bodyNode = pair.second; } else if (__jointType == "JointHinge") { - auto pair = createJointAndNodePair( + auto pair = createJointAndBodyNodePair( skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); bodyNode = pair.second; } else if (__jointType == "JointDummy") { - auto pair = createJointAndNodePair( + auto pair = createJointAndBodyNodePair( skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); bodyNode = pair.second; } @@ -547,6 +543,10 @@ bool readJointBall(const tinyxml2::XMLElement* jointEle, properties.mT_ParentBodyToJoint = tfFromParent; properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + properties.mDampingCoefficients = Eigen::Vector3d::Constant(0.5); + properties.mPositionLowerLimits = Eigen::Vector3d::Constant(-0.5 * DART_PI); + properties.mPositionUpperLimits = Eigen::Vector3d::Constant(+0.5 * DART_PI); + properties.mIsPositionLimited = true; jointProperties = Eigen::make_aligned_shared( @@ -578,6 +578,10 @@ bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); properties.mAxis[0] = axis1; properties.mAxis[1] = axis2; + properties.mDampingCoefficients = Eigen::Vector2d::Constant(0.5); + properties.mPositionLowerLimits = Eigen::Vector2d::Constant(-0.5 * DART_PI); + properties.mPositionUpperLimits = Eigen::Vector2d::Constant(+0.5 * DART_PI); + properties.mIsPositionLimited = true; jointProperties = Eigen::make_aligned_shared( @@ -602,6 +606,10 @@ bool readJointHinge(const tinyxml2::XMLElement* jointEle, properties.mT_ParentBodyToJoint = tfFromParent; properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); properties.mAxis = axis; + properties.mDampingCoefficient = 0.5; + properties.mPositionLowerLimit = -0.5 * DART_PI; + properties.mPositionUpperLimit = +0.5 * DART_PI; + properties.mIsPositionLimited = true; jointProperties = Eigen::make_aligned_shared( @@ -683,12 +691,15 @@ bool readMarker(const tinyxml2::XMLElement* markerEle, // = getAttribute(markerEle, "COVARIANCE"); // Attribute: RGB - // Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); - // if (hasAttribute(markerEle, "RGB")) - // { - // rgb = getAttributeVector3d(markerEle, "RGB"); - // rgb /= 255.0; - // } + Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); + if (hasAttribute(markerEle, "RGB")) + { + rgb = getAttributeVector3d(markerEle, "RGB"); + rgb /= 255.0; + } + Eigen::Vector4d rgba; + rgba.head<3>() = rgb; + rgba[3] = 1.0; // Attribute: RADIUS // double radius = 0.01; @@ -704,7 +715,8 @@ bool readMarker(const tinyxml2::XMLElement* markerEle, return false; } - dynamics::Marker* marker = new dynamics::Marker(name, position, bodyNode); + dynamics::Marker* marker = new dynamics::Marker(name, position, rgba, + bodyNode); bodyNode->addMarker(marker); return true; From ff27c7bff4f5249a57ca8be826623dfe09572f09 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 28 Nov 2015 01:02:25 -0500 Subject: [PATCH 48/79] Add Skeleton::getNumMarkers() --- dart/dynamics/Skeleton.cpp | 6 ++++++ dart/dynamics/Skeleton.h | 3 +++ unittests/testVskParser.cpp | 10 +++++++--- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index a60d4bf4dc390..4e30a27aa6588 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -728,6 +728,12 @@ void Skeleton::clearIK() mWholeBodyIK = nullptr; } +//============================================================================== +size_t Skeleton::getNumMarkers() const +{ + return mNameMgrForMarkers.getCount(); +} + //============================================================================== Marker* Skeleton::getMarker(const std::string& _name) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index b022dcac8257d..47fb247accda7 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -367,6 +367,9 @@ class Skeleton : public MetaSkeleton /// nullptr void clearIK(); + /// Get total number of markers in this Skeleton + size_t getNumMarkers() const; + /// Get marker whose name is _name Marker* getMarker(const std::string& _name); diff --git a/unittests/testVskParser.cpp b/unittests/testVskParser.cpp index b03d5b02abdc2..a82c3c6c5b54c 100644 --- a/unittests/testVskParser.cpp +++ b/unittests/testVskParser.cpp @@ -58,12 +58,13 @@ using namespace utils; TEST(VskParser, EmptySkeleton) { WorldPtr world(new World()); + EXPECT_TRUE(world != nullptr); + SkeletonPtr skeleton = VskParser::readSkeleton(DART_DATA_PATH"vsk/test/empty.vsk"); - world->addSkeleton(skeleton); - - EXPECT_TRUE(world != nullptr); EXPECT_TRUE(skeleton == nullptr); + + world->addSkeleton(skeleton); EXPECT_EQ(world->getNumSkeletons(), 0u); world->step(); @@ -78,14 +79,17 @@ TEST(VskParser, SingleStepSimulations) SkeletonPtr nick = VskParser::readSkeleton(DART_DATA_PATH"vsk/Nick01.vsk"); EXPECT_NE(nick , nullptr); + EXPECT_EQ(nick->getNumMarkers(), 53u); SkeletonPtr sehoon = VskParser::readSkeleton(DART_DATA_PATH"vsk/SehoonVSK3.vsk"); EXPECT_NE(sehoon, nullptr); + EXPECT_EQ(nick->getNumMarkers(), 53u); SkeletonPtr yuting = VskParser::readSkeleton(DART_DATA_PATH"vsk/Yuting.vsk"); EXPECT_NE(yuting, nullptr); + EXPECT_EQ(nick->getNumMarkers(), 53u); world->removeAllSkeletons(); world->addSkeleton(nick); From 172d8d09d5cc8e98e4ba963405e4bb636fc7af39 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 28 Nov 2015 05:07:15 -0500 Subject: [PATCH 49/79] Draw markers when SimWindow::mShowMarkers is true --- dart/gui/SimWindow.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 43478206ddb7b..363d84a1175b1 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -81,9 +81,16 @@ void SimWindow::timeStepping() { mWorld->step(); } -void SimWindow::drawSkels() { - for (size_t i = 0; i < mWorld->getNumSkeletons(); i++) +//============================================================================== +void SimWindow::drawSkels() +{ + for (size_t i = 0; i < mWorld->getNumSkeletons(); ++i) + { mWorld->getSkeleton(i)->draw(mRI); + + if (mShowMarkers) + mWorld->getSkeleton(i)->drawMarkers(mRI); + } } void SimWindow::drawEntities() From ffc5e7b0d7a331f9c76c7a4fd43642362e8def31 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 28 Nov 2015 17:22:23 -0500 Subject: [PATCH 50/79] Add options to VSK parser --- dart/utils/VskParser.cpp | 356 ++++++++++++++++++++++----------------- dart/utils/VskParser.h | 50 +++++- 2 files changed, 248 insertions(+), 158 deletions(-) diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index 46f7016e8790f..d0185021199c2 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -62,14 +62,13 @@ using BodyPropPtr = std::shared_ptr; using JointPropPtr = std::shared_ptr; using ParameterMap = std::map; -using SegmentIndexMap = std::map; using BodyNodeColorMap = std::map; struct VskData { - ParameterMap parameterMap; - SegmentIndexMap segmentIndexMap; - BodyNodeColorMap bodyNodeColorMap; + ParameterMap parameterMap; + BodyNodeColorMap bodyNodeColorMap; + VskParser::Options options; }; const double vsk_scale = 1.0e-3; @@ -94,45 +93,44 @@ bool readJoint(const std::string& jointType, const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); bool readJointFree(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); bool readJointBall(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); bool readJointHinge(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); bool readJointDummy(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap); + const VskData& vskData); + +Eigen::Vector3d readColorAttribute(const tinyxml2::XMLElement* element, + const ParameterMap& parameterMap, + const Eigen::Vector3d& defaultColor); template std::pair createJointAndBodyNodePair( + const std::string& jointType, const dynamics::SkeletonPtr& skeleton, dynamics::BodyNode* parentBodyNode, const dynamics::Joint::Properties* jointProperties, - const dynamics::BodyNode::Properties& bodyNodeProperties) -{ - return skeleton->createJointAndBodyNodePair( - parentBodyNode, - *static_cast(jointProperties), - bodyNodeProperties); -} + const dynamics::BodyNode::Properties& bodyNodeProperties); bool readMarkerSet(const tinyxml2::XMLElement* markerSetEle, const dynamics::SkeletonPtr& skel, @@ -158,10 +156,30 @@ void tokenize(const std::string& str, } // anonymous namespace +//============================================================================== +VskParser::Options::Options(const Eigen::Vector3d& newDefaultEllipsoidSize, + double newThicknessRatio, + double newDensity, + double newJointPositionLowerLimit, + double newJointPositionUpperLimit, + double newJointDampingCoefficient, + double newJointFriction) + : defaultEllipsoidSize(newDefaultEllipsoidSize), + thicknessRatio(newThicknessRatio), + density(newDensity), + jointPositionLowerLimit(newJointPositionLowerLimit), + jointPositionUpperLimit(newJointPositionUpperLimit), + jointDampingCoefficient(newJointDampingCoefficient), + jointFriction(newJointFriction) +{ + // Do nothing +} + //============================================================================== dynamics::SkeletonPtr VskParser::readSkeleton( - const common::Uri& fileUri, - const common::ResourceRetrieverPtr& retrieverOrNullptr) + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr, + const Options options) { const common::ResourceRetrieverPtr retriever = getRetriever(retrieverOrNullptr); @@ -192,6 +210,7 @@ dynamics::SkeletonPtr VskParser::readSkeleton( } VskData vskData; + vskData.options = options; // Read element tinyxml2::XMLElement* parametersEle @@ -272,8 +291,6 @@ bool readSkeletonElement(const tinyxml2::XMLElement* skeletonEle, dynamics::SkeletonPtr& skel, VskData& vskData) { - // Read skeleton and fill the Skeleton* and segmentindex - skel = dynamics::Skeleton::create(); // Read all segments @@ -377,96 +394,54 @@ bool readSegment(const tinyxml2::XMLElement* segment, bounds = readAttributeVector<6>(segment, "BOUNDS", vskData.parameterMap); // Attribute: RGB - Eigen::Vector3d rgb = Eigen::Vector3d(0.5, 0.5, 0.5); - if (hasAttribute(segment, "RGB")) - { - rgb = readAttributeVector<3>(segment, "RGB", vskData.parameterMap); - rgb /= 255.0; - } + Eigen::Vector3d rgb = readColorAttribute(segment, vskData.parameterMap, + Eigen::Vector3d::Constant(0.5)); dynamics::BodyNode::Properties bodyNodeProperties; bodyNodeProperties.mName = name; Eigen::Isometry3d tfFromParent; tfFromParent.translation() = position; - tfFromParent.linear() = math::expMapRot(orientation); + tfFromParent.linear() = math::expMapRot(orientation); // Joint const tinyxml2::XMLElement* jointEle = nullptr; - bool found = false; JointPropPtr jointProperties; - std::string __jointType; + std::string foundJointType; - for (auto jointType : vskJointTypes) + for (const auto& jointType : vskJointTypes) { jointEle = segment->FirstChildElement(jointType.c_str()); if (jointEle) { - found = true; - __jointType = jointType; - readJoint(jointType, jointEle, jointProperties, tfFromParent, - vskData.parameterMap); + foundJointType = jointType; + const bool res = readJoint(jointType, jointEle, jointProperties, + tfFromParent, vskData); + + if (!res) + { + dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; + return false; + } + break; } } - if (!found) - { - dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; - return false; - } - jointProperties->mName = "Joint-" + bodyNodeProperties.mName; -// // add to the model -// skel->addNode(blink); -// // _segmentindex[sname]=blink->getModelID(); -// segmentindex[name]=blink->getSkelIndex(); - - // Create joint and body node dynamics::BodyNode* bodyNode = nullptr; - if (__jointType == "JointFree") - { - auto pair = createJointAndBodyNodePair( - skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); - bodyNode = pair.second; - } - else if (__jointType == "JointBall") - { - auto pair = createJointAndBodyNodePair( - skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); - bodyNode = pair.second; - } - else if (__jointType == "JointHardySpicer") - { - auto pair = createJointAndBodyNodePair( - skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); - bodyNode = pair.second; - } - else if (__jointType == "JointHinge") - { - auto pair = createJointAndBodyNodePair( - skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); - bodyNode = pair.second; - } - else if (__jointType == "JointDummy") - { - auto pair = createJointAndBodyNodePair( - skel, parentBodyNode, jointProperties.get(), bodyNodeProperties); - bodyNode = pair.second; - } - else - { - dtwarn << "[ParserVsk::readSegment] Attempting to parse unsupported joint " - << "type.\n"; - return false; - } + auto pair = createJointAndBodyNodePair( + foundJointType, skel, parentBodyNode, jointProperties.get(), + bodyNodeProperties); + bodyNode = pair.second; + assert(bodyNode != nullptr); vskData.bodyNodeColorMap[bodyNode] = rgb; - // marker the subtree + // Read the subtree segments ConstElementEnumerator childSegment(segment, "Segment"); while (childSegment.next()) { @@ -481,32 +456,32 @@ bool readJoint(const std::string& jointType, const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& vskData) { if (jointType == "JointFree") { return readJointFree(jointEle, jointProperties, tfFromParent, - parameterMap); + vskData); } else if (jointType == "JointBall") { return readJointBall(jointEle, jointProperties, tfFromParent, - parameterMap); + vskData); } else if (jointType == "JointHardySpicer") { return readJointHardySpicer(jointEle, jointProperties, tfFromParent, - parameterMap); + vskData); } else if (jointType == "JointHinge") { return readJointHinge(jointEle, jointProperties, tfFromParent, - parameterMap); + vskData); } else if (jointType == "JointDummy") { return readJointDummy(jointEle, jointProperties, tfFromParent, - parameterMap); + vskData); } else { @@ -516,10 +491,10 @@ bool readJoint(const std::string& jointType, } //============================================================================== -bool readJointFree(const tinyxml2::XMLElement* jointEle, +bool readJointFree(const tinyxml2::XMLElement* /*jointEle*/, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& /*vskData*/) { dynamics::FreeJoint::Properties properties; @@ -537,16 +512,21 @@ bool readJointFree(const tinyxml2::XMLElement* jointEle, bool readJointBall(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& vskData) { dynamics::BallJoint::Properties properties; properties.mT_ParentBodyToJoint = tfFromParent; properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); - properties.mDampingCoefficients = Eigen::Vector3d::Constant(0.5); - properties.mPositionLowerLimits = Eigen::Vector3d::Constant(-0.5 * DART_PI); - properties.mPositionUpperLimits = Eigen::Vector3d::Constant(+0.5 * DART_PI); + properties.mDampingCoefficients = Eigen::Vector3d::Constant( + vskData.options.jointDampingCoefficient); + properties.mPositionLowerLimits = Eigen::Vector3d::Constant( + vskData.options.jointPositionLowerLimit); + properties.mPositionUpperLimits = Eigen::Vector3d::Constant( + vskData.options.jointPositionUpperLimit); properties.mIsPositionLimited = true; + properties.mFrictions = Eigen::Vector3d::Constant( + vskData.options.jointFriction); jointProperties = Eigen::make_aligned_shared( @@ -559,7 +539,7 @@ bool readJointBall(const tinyxml2::XMLElement* jointEle, bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& vskData) { dynamics::UniversalJoint::Properties properties; @@ -569,7 +549,7 @@ bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, if (hasAttribute(jointEle, "AXIS-PAIR")) { Eigen::Vector6d axisPair - = readAttributeVector<6>(jointEle, "AXIS-PAIR", parameterMap); + = readAttributeVector<6>(jointEle, "AXIS-PAIR", vskData.parameterMap); axis1 = axisPair.head<3>(); axis2 = axisPair.tail<3>(); } @@ -578,10 +558,15 @@ bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); properties.mAxis[0] = axis1; properties.mAxis[1] = axis2; - properties.mDampingCoefficients = Eigen::Vector2d::Constant(0.5); - properties.mPositionLowerLimits = Eigen::Vector2d::Constant(-0.5 * DART_PI); - properties.mPositionUpperLimits = Eigen::Vector2d::Constant(+0.5 * DART_PI); + properties.mDampingCoefficients = Eigen::Vector2d::Constant( + vskData.options.jointDampingCoefficient); + properties.mPositionLowerLimits = Eigen::Vector2d::Constant( + vskData.options.jointPositionLowerLimit); + properties.mPositionUpperLimits = Eigen::Vector2d::Constant( + vskData.options.jointPositionUpperLimit); properties.mIsPositionLimited = true; + properties.mFrictions = Eigen::Vector2d::Constant( + vskData.options.jointFriction); jointProperties = Eigen::make_aligned_shared( @@ -594,22 +579,23 @@ bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, bool readJointHinge(const tinyxml2::XMLElement* jointEle, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& vskData) { dynamics::RevoluteJoint::Properties properties; // Attribute: AXIS Eigen::Vector3d axis = Eigen::Vector3d::UnitX(); if (hasAttribute(jointEle, "AXIS")) - axis = readAttributeVector<3>(jointEle, "AXIS", parameterMap); + axis = readAttributeVector<3>(jointEle, "AXIS", vskData.parameterMap); properties.mT_ParentBodyToJoint = tfFromParent; properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); properties.mAxis = axis; - properties.mDampingCoefficient = 0.5; - properties.mPositionLowerLimit = -0.5 * DART_PI; - properties.mPositionUpperLimit = +0.5 * DART_PI; + properties.mDampingCoefficient = vskData.options.jointDampingCoefficient; + properties.mPositionLowerLimit = vskData.options.jointPositionLowerLimit; + properties.mPositionUpperLimit = vskData.options.jointPositionUpperLimit; properties.mIsPositionLimited = true; + properties.mFriction = vskData.options.jointFriction; jointProperties = Eigen::make_aligned_shared( @@ -619,10 +605,10 @@ bool readJointHinge(const tinyxml2::XMLElement* jointEle, } //============================================================================== -bool readJointDummy(const tinyxml2::XMLElement* jointEle, +bool readJointDummy(const tinyxml2::XMLElement* /*jointEle*/, JointPropPtr& jointProperties, const Eigen::Isometry3d& tfFromParent, - const ParameterMap& parameterMap) + const VskData& /*vskData*/) { dynamics::WeldJoint::Properties properties; @@ -636,6 +622,81 @@ bool readJointDummy(const tinyxml2::XMLElement* jointEle, return true; } +//============================================================================== +Eigen::Vector3d readColorAttribute(const tinyxml2::XMLElement* element, + const ParameterMap& parameterMap, + const Eigen::Vector3d& defaultColor) +{ + if (hasAttribute(element, "RGB")) + return readAttributeVector<3>(element, "RGB", parameterMap) / 255.0; + else + return defaultColor; +} + +//============================================================================== +template +std::pair createJointAndBodyNodePair( + const std::string& jointType, + const dynamics::SkeletonPtr& skeleton, + dynamics::BodyNode* parentBodyNode, + const dynamics::Joint::Properties* jointProperties, + const dynamics::BodyNode::Properties& bodyNodeProperties) +{ + if (jointType == "JointFree") + { + return skeleton->createJointAndBodyNodePair< + dynamics::FreeJoint, dynamics::BodyNode>( + parentBodyNode, + *static_cast( + jointProperties), + bodyNodeProperties); + } + else if (jointType == "JointBall") + { + return skeleton->createJointAndBodyNodePair< + dynamics::BallJoint, dynamics::BodyNode>( + parentBodyNode, + *static_cast( + jointProperties), + bodyNodeProperties); + } + else if (jointType == "JointHardySpicer") + { + return skeleton->createJointAndBodyNodePair< + dynamics::UniversalJoint, dynamics::BodyNode>( + parentBodyNode, + *static_cast( + jointProperties), + bodyNodeProperties); + } + else if (jointType == "JointHinge") + { + return skeleton->createJointAndBodyNodePair< + dynamics::RevoluteJoint, dynamics::BodyNode>( + parentBodyNode, + *static_cast( + jointProperties), + bodyNodeProperties); + } + else if (jointType == "JointDummy") + { + return skeleton->createJointAndBodyNodePair< + dynamics::WeldJoint, dynamics::BodyNode>( + parentBodyNode, + *static_cast( + jointProperties), + bodyNodeProperties); + } + else + { + dtwarn << "[ParserVsk::readSegment] Attempting to parse unsupported joint " + << "type.\n"; + + return std::pair(nullptr, + nullptr); + } +} + //============================================================================== bool readMarkerSet(const tinyxml2::XMLElement* markerSetEle, const dynamics::SkeletonPtr& skel, @@ -691,15 +752,9 @@ bool readMarker(const tinyxml2::XMLElement* markerEle, // = getAttribute(markerEle, "COVARIANCE"); // Attribute: RGB - Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); - if (hasAttribute(markerEle, "RGB")) - { - rgb = getAttributeVector3d(markerEle, "RGB"); - rgb /= 255.0; - } - Eigen::Vector4d rgba; - rgba.head<3>() = rgb; - rgba[3] = 1.0; + Eigen::Vector4d rgba = Eigen::Vector4d::Ones(); + rgba.head<3>() = readColorAttribute(markerEle, vskData.parameterMap, + Eigen::Vector3d::Constant(0.5)); // Attribute: RADIUS // double radius = 0.01; @@ -723,27 +778,22 @@ bool readMarker(const tinyxml2::XMLElement* markerEle, } //============================================================================== -bool readStick(const tinyxml2::XMLElement* stickEle, - const dynamics::SkeletonPtr& skel, - VskData& vskData) +bool readStick(const tinyxml2::XMLElement* /*stickEle*/, + const dynamics::SkeletonPtr& /*skel*/, + VskData& /*vskData*/) { - std::string marker1 = getAttributeString(stickEle, "MARKER1"); - std::string marker2 = getAttributeString(stickEle, "MARKER2"); + // std::string marker1 = getAttributeString(stickEle, "MARKER1"); + // std::string marker2 = getAttributeString(stickEle, "MARKER2"); // Attribute: RGB - Eigen::Vector3d rgb = Eigen::Vector3d::Constant(0.5); - if (hasAttribute(stickEle, "RGB")) - { - rgb = getAttributeVector3d(stickEle, "RGB"); - rgb /= 255.0; - } + // Eigen::Vector3d rgb = readColorAttribute(segment, vskData.parameterMap, + // Eigen::Vector3d(0.5, 0.5, 0.5)); return true; } //============================================================================== -void generateShapes(const dynamics::SkeletonPtr& skel, - VskData& vskData) +void generateShapes(const dynamics::SkeletonPtr& skel, VskData& vskData) { // Generate shapes for bodies that have their parents for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) @@ -763,7 +813,7 @@ void generateShapes(const dynamics::SkeletonPtr& skel, // diameters are 35% of the distance. Eigen::Vector3d size; size[0] = tf.translation().norm(); - size[1] = size[2] = 0.35 * size[0]; + size[1] = size[2] = vskData.options.thicknessRatio * size[0]; // Determine the local transform of the shape Eigen::Isometry3d localTransform = Eigen::Isometry3d::Identity(); @@ -777,15 +827,6 @@ void generateShapes(const dynamics::SkeletonPtr& skel, parent->addVisualizationShape(shape); parent->addCollisionShape(shape); - - // Update inertia - const double density = 2.0e+3; - const double mass = shape->getVolume() * density; - const Eigen::Matrix3d moi = shape->computeInertia(mass); - const dynamics::Inertia inertia(mass, localTransform.translation(), moi); - // TODO(JS): Once Inertia supports transform, pass the localTransform to the - // inertia. See #234. - parent->setInertia(inertia); } // Generate shpae for bodies with no shape @@ -797,25 +838,17 @@ void generateShapes(const dynamics::SkeletonPtr& skel, continue; // Use hard-coded size ellipsoid - Eigen::Vector3d size = Eigen::Vector3d::Constant(0.05); + const Eigen::Vector3d& size = vskData.options.defaultEllipsoidSize; dynamics::ShapePtr shape(new dynamics::EllipsoidShape(size)); shape->setColor(vskData.bodyNodeColorMap[bodyNode]); bodyNode->addVisualizationShape(shape); bodyNode->addCollisionShape(shape); - - // Update mass - const double density = 1.0e+3; - const double mass = shape->getVolume() * density; - const Eigen::Matrix3d moi = shape->computeInertia(mass); - const dynamics::Inertia inertia(mass, Eigen::Vector3d::Zero(), moi); - bodyNode->setInertia(inertia); } - // Update mass and moments of inertia of the bodies based on the generated - // shapes into them. - double density = 1.0e+3; + // Update mass and moments of inertia of the bodies based on the their shapes + const double& density = vskData.options.density; for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) { dynamics::BodyNode* bodyNode = skel->getBodyNode(i); @@ -823,14 +856,25 @@ void generateShapes(const dynamics::SkeletonPtr& skel, // Now all the bodies should have at least one shape assert(bodyNode->getNumVisualizationShapes() > 0); - double mass = 0.0; - Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); - auto shapes = bodyNode->getVisualizationShapes(); + double totalMass = 0.0; + Eigen::Matrix3d totalMoi = Eigen::Matrix3d::Zero(); + const auto& shapes = bodyNode->getVisualizationShapes(); + for (const dynamics::ShapePtr& shape : shapes) { - mass += density * shape->getVolume(); - inertia += shape->computeInertia(mass); + const double mass = density * shape->getVolume(); + const Eigen::Isometry3d& localTf = shape->getLocalTransform(); + const Eigen::Matrix3d moi = shape->computeInertia(mass); + + totalMass += mass; + totalMoi += math::parallelAxisTheorem(moi, localTf.translation(), mass); + // TODO(JS): We should take the orientation of the inertia into account, + // but Inertia class doens't support it for now. See #234. } + + const dynamics::Inertia inertia(totalMass, Eigen::Vector3d::Zero(), + totalMoi); + bodyNode->setInertia(inertia); } } diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index afcf7414d6da9..6b6092142ab01 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -48,10 +48,56 @@ namespace utils { class VskParser { public: + + /// Options struct is additional information that helps building a skeleton + /// that can be used in kinematics or dynamics simulation. VSK file format + /// itself doesn't provide essential properties for it such as body's shape, + /// mass, and inertia. + struct Options + { + /// The default shape for body node is ellipsoid. The size of ellipsoid of + /// each body node are determined by the relative transformation from a body + /// node and its child body node. defaultEllipsoidSize is used for body + /// nodes that don't have child body node. + Eigen::Vector3d defaultEllipsoidSize; + + /// Ratio of shorter radii of each ellipsoid to the longest radius where + /// the longest radius is the distance between a body and its child body + /// node. + double thicknessRatio; + + /// Density of each ellipsoid that are used to compute mass. + double density; + + /// Lower limit of joint position + double jointPositionLowerLimit; + + /// Upper limit of joint position + double jointPositionUpperLimit; + + /// Joint damping coefficient + double jointDampingCoefficient; + + /// Joint Coulomb friction + double jointFriction; + + /// Constructor + Options(const Eigen::Vector3d& defaultEllipsoidSize + = Eigen::Vector3d::Constant(0.05), + double thicknessRatio = 0.35, + double density = 1e+3, + double jointPositionLowerLimit = -DART_PI, + double jointPositionUpperLimit = +DART_PI, + double jointDampingCoefficient = 0.1, + double jointFriction = 0.0); + }; + /// Read Skeleton from VSK file static dynamics::SkeletonPtr readSkeleton( - const common::Uri& fileUri, - const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr); + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr, + const Options options = Options()); + }; } // namespace utils From 958c181a4e6b53b525f5741921108441df700122 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 28 Nov 2015 21:19:11 -0500 Subject: [PATCH 51/79] Add back Eigen aligned new operator to Marker class --- dart/dynamics/Marker.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index 92bce37560741..5314002404973 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -161,6 +161,10 @@ class Marker /// Counts the number of markers globally. static int msMarkerCount; +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace dynamics From d8ee39cc537296779a98bdcee83cddef83569ce4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 29 Nov 2015 03:02:14 -0500 Subject: [PATCH 52/79] Add servo motor constraint --- dart/constraint/ConstraintSolver.cpp | 31 +++ dart/constraint/ConstraintSolver.h | 6 +- dart/constraint/ServoMotorConstraint.cpp | 294 +++++++++++++++++++++++ dart/constraint/ServoMotorConstraint.h | 150 ++++++++++++ dart/dynamics/Joint.h | 1 - unittests/TestHelpers.h | 66 +++++ unittests/testJoints.cpp | 185 ++++++++++++++ 7 files changed, 731 insertions(+), 2 deletions(-) create mode 100644 dart/constraint/ServoMotorConstraint.cpp create mode 100644 dart/constraint/ServoMotorConstraint.h diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index fc13796744e3d..d12ec9459c5ba 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -50,6 +50,7 @@ #include "dart/constraint/ContactConstraint.h" #include "dart/constraint/SoftContactConstraint.h" #include "dart/constraint/JointLimitConstraint.h" +#include "dart/constraint/ServoMotorConstraint.h" #include "dart/constraint/JointCoulombFrictionConstraint.h" #include "dart/constraint/DantzigLCPSolver.h" #include "dart/constraint/PGSLCPSolver.h" @@ -424,6 +425,36 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(jointLimitConstraint); } + //---------------------------------------------------------------------------- + // Update automatic constraints: servo motor constraints + //---------------------------------------------------------------------------- + // Destroy previous joint limit constraints + for (const auto& servoMotorConstraint : mServoMotorConstraints) + delete servoMotorConstraint; + mServoMotorConstraints.clear(); + + // Create new joint limit constraints + for (const auto& skel : mSkeletons) + { + const size_t numJoints = skel->getNumJoints(); + for (size_t i = 0; i < numJoints; i++) + { + dynamics::Joint* joint = skel->getJoint(i); + + if (joint->getActuatorType() == dynamics::Joint::SERVO) + mServoMotorConstraints.push_back(new ServoMotorConstraint(joint)); + } + } + + // Add active servo motor constraints + for (auto& servoMotorConstraint : mServoMotorConstraints) + { + servoMotorConstraint->update(); + + if (servoMotorConstraint->isActive()) + mActiveConstraints.push_back(servoMotorConstraint); + } + //---------------------------------------------------------------------------- // Update automatic constraints: joint Coulomb friction constraints //---------------------------------------------------------------------------- diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index e1ae909797c0d..abb669e10c0e3 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -58,6 +58,7 @@ class ClosedLoopConstraint; class ContactConstraint; class SoftContactConstraint; class JointLimitConstraint; +class ServoMotorConstraint; class JointCoulombFrictionConstraint; class JointConstraint; class LCPSolver; @@ -165,7 +166,10 @@ class ConstraintSolver /// Joint limit constraints those are automatically created std::vector mJointLimitConstraints; - /// Joint limit constraints those are automatically created + /// Servo motor constraints those are automatically created + std::vector mServoMotorConstraints; + + /// Joint Coulomb friction constraints those are automatically created std::vector mJointCoulombFrictionConstraints; /// Constraints that manually added diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp new file mode 100644 index 0000000000000..6f8aea62a296c --- /dev/null +++ b/dart/constraint/ServoMotorConstraint.cpp @@ -0,0 +1,294 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include "dart/constraint/ServoMotorConstraint.h" + +#include + +#include "dart/common/Console.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Joint.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/lcpsolver/lcp.h" + +#define DART_CFM 1e-9 + +namespace dart { +namespace constraint { + +double ServoMotorConstraint::mConstraintForceMixing = DART_CFM; + +//============================================================================== +ServoMotorConstraint::ServoMotorConstraint(dynamics::Joint* joint) + : ConstraintBase(), + mJoint(joint), + mBodyNode(joint->getChildBodyNode()), + mAppliedImpulseIndex(0) +{ + assert(joint); + assert(mBodyNode); + + mLifeTime[0] = 0; + mLifeTime[1] = 0; + mLifeTime[2] = 0; + mLifeTime[3] = 0; + mLifeTime[4] = 0; + mLifeTime[5] = 0; + + mActive[0] = false; + mActive[1] = false; + mActive[2] = false; + mActive[3] = false; + mActive[4] = false; + mActive[5] = false; +} + +//============================================================================== +ServoMotorConstraint::~ServoMotorConstraint() +{ +} + +//============================================================================== +void ServoMotorConstraint::setConstraintForceMixing(double cfm) +{ + // Clamp constraint force mixing parameter if it is out of the range + if (cfm < 1e-9) + { + dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] " + << "Constraint force mixing parameter[" << cfm + << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl; + mConstraintForceMixing = 1e-9; + } + if (cfm > 1.0) + { + dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] " + << "Constraint force mixing parameter[" << cfm + << "] is greater than 1.0. " << "It is set to 1.0." << std::endl; + mConstraintForceMixing = 1.0; + } + + mConstraintForceMixing = cfm; +} + +//============================================================================== +double ServoMotorConstraint::getConstraintForceMixing() +{ + return mConstraintForceMixing; +} + +//============================================================================== +void ServoMotorConstraint::update() +{ + // Reset dimention + mDim = 0; + + size_t dof = mJoint->getNumDofs(); + for (size_t i = 0; i < dof; ++i) + { + mNegativeVelocityError[i] = mJoint->getCommand(i) - mJoint->getVelocity(i); + + if (mNegativeVelocityError[i] != 0.0) + { + double timeStep = mJoint->getSkeleton()->getTimeStep(); + // TODO: There are multiple ways to get time step (or its inverse). + // - ContactConstraint get it from the constructor parameter + // - Skeleton has it itself. + // - ConstraintBase::getInformation() passes ConstraintInfo structure + // that contains reciprocal time step. + // We might need to pick one way and remove the others to get rid of + // redundancy. + + // Note that we are computing impulse not force + mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep; + mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep; + + if (mActive[i]) + { + ++(mLifeTime[i]); + } + else + { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + } + else + { + mActive[i] = false; + } + } +} + +//============================================================================== +void ServoMotorConstraint::getInformation(ConstraintInfo* lcp) +{ + size_t index = 0; + size_t dof = mJoint->getNumDofs(); + for (size_t i = 0; i < dof; ++i) + { + if (mActive[i] == false) + continue; + + assert(lcp->w[index] == 0.0); + + lcp->b[index] = mNegativeVelocityError[i]; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; + + assert(lcp->findex[index] == -1); + + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; + } +} + +//============================================================================== +void ServoMotorConstraint::applyUnitImpulse(size_t index) +{ + assert(index < mDim && "Invalid Index."); + + size_t localIndex = 0; + const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); + + size_t dof = mJoint->getNumDofs(); + for (size_t i = 0; i < dof; ++i) + { + if (mActive[i] == false) + continue; + + if (localIndex == index) + { + skeleton->clearConstraintImpulses(); + mJoint->setConstraintImpulse(i, 1.0); + skeleton->updateBiasImpulse(mBodyNode); + skeleton->updateVelocityChange(); + mJoint->setConstraintImpulse(i, 0.0); + } + + ++localIndex; + } + + mAppliedImpulseIndex = index; +} + +//============================================================================== +void ServoMotorConstraint::getVelocityChange(double* delVel, bool withCfm) +{ + assert(delVel != nullptr && "Null pointer is not allowed."); + + size_t localIndex = 0; + size_t dof = mJoint->getNumDofs(); + for (size_t i = 0; i < dof ; ++i) + { + if (mActive[i] == false) + continue; + + if (mJoint->getSkeleton()->isImpulseApplied()) + delVel[localIndex] = mJoint->getVelocityChange(i); + else + delVel[localIndex] = 0.0; + + ++localIndex; + } + + // Add small values to diagnal to keep it away from singular, similar to cfm + // varaible in ODE + if (withCfm) + { + delVel[mAppliedImpulseIndex] += delVel[mAppliedImpulseIndex] + * mConstraintForceMixing; + } + + assert(localIndex == mDim); +} + +//============================================================================== +void ServoMotorConstraint::excite() +{ + mJoint->getSkeleton()->setImpulseApplied(true); +} + +//============================================================================== +void ServoMotorConstraint::unexcite() +{ + mJoint->getSkeleton()->setImpulseApplied(false); +} + +//============================================================================== +void ServoMotorConstraint::applyImpulse(double* lambda) +{ + size_t localIndex = 0; + size_t dof = mJoint->getNumDofs(); + for (size_t i = 0; i < dof ; ++i) + { + if (mActive[i] == false) + continue; + + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + // TODO(JS): consider to add Joint::addConstraintImpulse() + + mOldX[i] = lambda[localIndex]; + + ++localIndex; + } +} + +//============================================================================== +dynamics::SkeletonPtr ServoMotorConstraint::getRootSkeleton() const +{ + return mJoint->getSkeleton()->mUnionRootSkeleton.lock(); +} + +//============================================================================== +bool ServoMotorConstraint::isActive() const +{ + // Since we are not allowed to set the joint actuator type per each + // DegreeOfFreedom, we just check if the whole joint is SERVO actuator. + if (mJoint->getActuatorType() == dynamics::Joint::SERVO) + return true; + + return false; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/ServoMotorConstraint.h b/dart/constraint/ServoMotorConstraint.h new file mode 100644 index 0000000000000..f23e73b820d95 --- /dev/null +++ b/dart/constraint/ServoMotorConstraint.h @@ -0,0 +1,150 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_CONSTRAINT_SERVOMOTORCONSTRAINT_H_ +#define DART_CONSTRAINT_SERVOMOTORCONSTRAINT_H_ + +#include "dart/constraint/ConstraintBase.h" + +namespace dart { + +namespace dynamics { +class BodyNode; +class Joint; +} // namespace dynamics + +namespace constraint { + +/// Servo motor constraint +class ServoMotorConstraint : public ConstraintBase +{ +public: + /// Constructor + explicit ServoMotorConstraint(dynamics::Joint* joint); + + /// Destructor + virtual ~ServoMotorConstraint(); + + //---------------------------------------------------------------------------- + // Property settings + //---------------------------------------------------------------------------- + + /// Set global constraint force mixing parameter + static void setConstraintForceMixing(double cfm); + + /// Get global constraint force mixing parameter + static double getConstraintForceMixing(); + + //---------------------------------------------------------------------------- + // Friendship + //---------------------------------------------------------------------------- + + friend class ConstraintSolver; + friend class ConstrainedGroup; + +protected: + //---------------------------------------------------------------------------- + // Constraint virtual functions + //---------------------------------------------------------------------------- + + // Documentation inherited + virtual void update(); + + // Documentation inherited + virtual void getInformation(ConstraintInfo* lcp); + + // Documentation inherited + virtual void applyUnitImpulse(size_t index); + + // Documentation inherited + virtual void getVelocityChange(double* delVel, bool withCfm); + + // Documentation inherited + virtual void excite(); + + // Documentation inherited + virtual void unexcite(); + + // Documentation inherited + virtual void applyImpulse(double* lambda); + + // Documentation inherited + virtual dynamics::SkeletonPtr getRootSkeleton() const; + + // Documentation inherited + virtual bool isActive() const; + +private: + /// + dynamics::Joint* mJoint; + + /// + dynamics::BodyNode* mBodyNode; + + /// Index of applied impulse + size_t mAppliedImpulseIndex; + + /// + size_t mLifeTime[6]; + // TODO(JS): Lifetime should be considered only when we use iterative lcp + // solver + + /// + bool mActive[6]; + + /// + double mNegativeVelocityError[6]; + + /// + double mOldX[6]; + + /// + double mUpperBound[6]; + + /// + double mLowerBound[6]; + + /// Global constraint force mixing parameter in the range of [1e-9, 1]. The + /// default is 1e-5 + /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0 + static double mConstraintForceMixing; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_H_ + diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index c1f2f177a214f..82639c3a47bea 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -100,7 +100,6 @@ class Joint : public virtual common::Subject /// The constraint solver will try to track the desired velocity within the /// joint force limit. All the joint constarints are valid. SERVO, - // TODO: Not implemented yet. /// Command input is joint acceleration, and the output is joint force. /// diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 1934eea0a37e6..86ed1b7890dce 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -296,6 +296,72 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, return robot; } +//============================================================================== +/// Creates a N link pendulum with the given dimensions where each joint is +/// the specified type. The each offset from the joint position to the child +/// body is specified. +SkeletonPtr createNLinkPendulum(size_t numBodyNodes, + const Vector3d& dim, + TypeOfDOF type, + const Vector3d& offset, + bool finished = false) +{ + assert(numBodyNodes > 0); + + SkeletonPtr robot = Skeleton::create(); + robot->disableSelfCollision(); + + // Create the first link, the joint with the ground and its shape + BodyNode::Properties node(std::string("link1")); + node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); + std::shared_ptr shape(new BoxShape(dim)); + node.mVizShapes.push_back(shape); + node.mColShapes.push_back(shape); + + std::pair pair1 = add1DofJoint( + robot, nullptr, node, "joint1", 0.0, -DART_PI, DART_PI, type); + + Joint* joint = pair1.first; + Eigen::Isometry3d T = joint->getTransformFromChildBodyNode(); + T.translation() = offset; + joint->setTransformFromChildBodyNode(T); + joint->setDampingCoefficient(0, 0.01); + + BodyNode* parent_node = pair1.second; + + // Create links iteratively + for (size_t i = 1; i < numBodyNodes; ++i) + { + std::ostringstream ssLink; + std::ostringstream ssJoint; + ssLink << "link" << i; + ssJoint << "joint" << i; + + node = BodyNode::Properties(ssLink.str()); + node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); + shape = std::shared_ptr(new BoxShape(dim)); + node.mVizShapes.push_back(shape); + node.mColShapes.push_back(shape); + + std::pair newPair = add1DofJoint( + robot, parent_node, node, ssJoint.str(), 0.0, -DART_PI, DART_PI, type); + + Joint* joint = newPair.first; + Eigen::Isometry3d T = joint->getTransformFromChildBodyNode(); + T.translation() = offset; + joint->setTransformFromChildBodyNode(T); + joint->setDampingCoefficient(0, 0.01); + + parent_node = newPair.second; + } + + // If finished, initialize the skeleton + if(finished) + addEndEffector(robot, parent_node, dim); + + return robot; +} + //============================================================================== SkeletonPtr createGround( const Eigen::Vector3d& _size, diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 7d6ec24e0f138..91c52cd85a0b3 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -624,6 +624,191 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION) testJointCoulombFrictionForce(timeStep); } +//============================================================================== +SkeletonPtr createPendulum(Joint::ActuatorType actType) +{ + Vector3d dim(1, 1, 1); + Vector3d offset(0, 0, 2); + + SkeletonPtr pendulum = createNLinkPendulum(1, dim, DOF_ROLL, offset); + EXPECT_NE(pendulum, nullptr); + + pendulum->disableSelfCollision(); + + for (size_t i = 0; i < pendulum->getNumBodyNodes(); ++i) + { + auto bodyNode = pendulum->getBodyNode(i); + bodyNode->removeAllCollisionShapes(); + } + + // Joint common setting + dynamics::Joint* joint = pendulum->getJoint(0); + EXPECT_NE(joint, nullptr); + + joint->setActuatorType(actType); + joint->setPosition(0, 90.0 * DART_RADIAN); + joint->setDampingCoefficient(0, 0.0); + joint->setSpringStiffness(0, 0.0); + joint->setPositionLimitEnforced(true); + joint->setCoulombFriction(0, 0.0); + + return pendulum; +} + +//============================================================================== +void testServoMotor() +{ + size_t numPendulums = 7; + double timestep = 1e-3; + double tol = 1e-9; + double posUpperLimit = 90.0 * DART_RADIAN; + double posLowerLimit = 45.0 * DART_RADIAN; + double sufficient_force = 1e+5; + double insufficient_force = 1e-1; + + // World + simulation::WorldPtr world(new simulation::World); + EXPECT_TRUE(world != nullptr); + + world->setGravity(Eigen::Vector3d(0, 0, -9.81)); + world->setTimeStep(timestep); + + // Each pendulum has servo motor in the joint but also have different + // expectations depending on the property values. + // + // Pendulum0: + // - Condition: Zero desired velocity and sufficient servo motor force limits + // - Expectation: The desired velocity should be achieved. + // Pendulum 1: + // - Condition: Nonzero desired velocity and sufficient servo motor force + // limits + // - Expectation: The desired velocity should be achieved. + // Pendulum 2: + // - Condition: Nonzero desired velocity and insufficient servo motor force + // limits + // - Expectation: The desired velocity can be achieve or not. But when it's + // not achieved, the servo motor force is reached to the lower + // or upper limit. + // Pendulum 3: + // - Condition: Nonzero desired velocity, finite servo motor force limits, + // and position limits + // - Expectation: The desired velocity should be achieved unless joint + // position is reached to lower or upper position limit. + // Pendulum 4: + // - Condition: Nonzero desired velocity, infinite servo motor force limits, + // and position limits + // - Expectation: Same as the Pendulum 3's expectation. + // Pendulum 5: + // - Condition: Nonzero desired velocity, finite servo motor force limits, + // and infinite Coulomb friction + // - Expectation: The the pendulum shouldn't move at all due to the infinite + // friction. + // Pendulum 6: + // - Condition: Nonzero desired velocity, infinite servo motor force limits, + // and infinite Coulomb friction + // - Expectation: The the pendulum shouldn't move at all due to the friction. + // TODO(JS): Should a servo motor dominent Coulomb friction in this case? + + std::vector pendulums(numPendulums); + std::vector joints(numPendulums); + for (size_t i = 0; i < numPendulums; ++i) + { + pendulums[i] = createPendulum(Joint::SERVO); + joints[i] = pendulums[i]->getJoint(0); + } + + joints[0]->setForceUpperLimit(0, sufficient_force); + joints[0]->setForceLowerLimit(0, -sufficient_force); + + joints[1]->setForceUpperLimit(0, sufficient_force); + joints[1]->setForceLowerLimit(0, -sufficient_force); + + joints[2]->setForceUpperLimit(0, insufficient_force); + joints[2]->setForceLowerLimit(0, -insufficient_force); + + joints[3]->setForceUpperLimit(0, sufficient_force); + joints[3]->setForceLowerLimit(0, -sufficient_force); + joints[3]->setPositionUpperLimit(0, posUpperLimit); + joints[3]->setPositionLowerLimit(0, posLowerLimit); + + joints[4]->setForceUpperLimit(0, DART_DBL_INF); + joints[4]->setForceLowerLimit(0, -DART_DBL_INF); + joints[4]->setPositionUpperLimit(0, posUpperLimit); + joints[4]->setPositionLowerLimit(0, posLowerLimit); + + joints[5]->setForceUpperLimit(0, sufficient_force); + joints[5]->setForceLowerLimit(0, -sufficient_force); + joints[5]->setCoulombFriction(0, DART_DBL_INF); + + joints[6]->setForceUpperLimit(0, DART_DBL_INF); + joints[6]->setForceLowerLimit(0, -DART_DBL_INF); + joints[6]->setCoulombFriction(0, DART_DBL_INF); + + for (auto pendulum : pendulums) + world->addSkeleton(pendulum); + +#ifndef NDEBUG // Debug mode + double simTime = 0.2; +#else + double simTime = 2.0; +#endif // ------- Debug mode + double timeStep = world->getTimeStep(); + int nSteps = simTime / timeStep; + + // Two seconds with lower control forces than the friction + for (int i = 0; i < nSteps; i++) + { + const double expected_vel = std::sin(world->getTime()); + + joints[0]->setCommand(0, 0.0); + joints[1]->setCommand(0, expected_vel); + joints[2]->setCommand(0, expected_vel); + joints[3]->setCommand(0, expected_vel); + joints[4]->setCommand(0, expected_vel); + joints[5]->setCommand(0, expected_vel); + joints[6]->setCommand(0, expected_vel); + + world->step(); + + std::vector jointVels(numPendulums); + for (size_t j = 0; j < numPendulums; ++j) + jointVels[j] = joints[j]->getVelocity(0); + + EXPECT_NEAR(jointVels[0], 0.0, tol); + EXPECT_NEAR(jointVels[1], expected_vel, tol); + bool result2 = std::abs(jointVels[2] - expected_vel) < tol + || std::abs(joints[2]->getConstraintImpulse(0) / timeStep + - insufficient_force) < tol + || std::abs(joints[2]->getConstraintImpulse(0) / timeStep + + insufficient_force) < tol; + EXPECT_TRUE(result2); + EXPECT_LE(joints[3]->getPosition(0), + posUpperLimit + expected_vel * timeStep); + EXPECT_GE(joints[3]->getPosition(0), + posLowerLimit - expected_vel * timeStep); + // EXPECT_LE(joints[4]->getPosition(0), + // posUpperLimit + expected_vel * timeStep); + // EXPECT_GE(joints[4]->getPosition(0), + // posLowerLimit - expected_vel * timeStep); + // TODO(JS): Position limits and servo motor with infinite force limits + // doesn't work together because they compete against each other to achieve + // different joint velocities with their infinit force limits. In this case, + // the position limit constraint should dominent the servo motor constraint. + // EXPECT_NEAR(jointVels[5], 0.0, tol * 1e+2); + // EXPECT_NEAR(jointVels[6], 0.0, tol * 1e+2); + // TODO(JS): Servo motor with infinite force limits and infinite Coulomb + // friction doesn't work because they compete against each other to achieve + // different joint velocities with their infinit force limits. In this case, + // the friction constraints should dominent the servo motor constraints. + } +} + +//============================================================================== +TEST_F(JOINTS, SERVO_MOTOR) +{ + testServoMotor(); +} + //============================================================================== template Eigen::Matrix random_vec(double limit=100) From c65383570c364ff7877ece13ac1f5840282671d6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 29 Nov 2015 03:17:06 -0500 Subject: [PATCH 53/79] Fix incorrect applying constraints on a joint with different constraints - Each joint constraint impulse is added up to the joint instead of overwriting the total impulse everytime. - This should resolve #317 as well. --- dart/constraint/JointCoulombFrictionConstraint.cpp | 4 +++- dart/constraint/JointLimitConstraint.cpp | 4 +++- dart/dynamics/BodyNode.h | 3 ++- dart/dynamics/Skeleton.h | 5 +++-- dart/simulation/World.cpp | 1 - unittests/testJoints.cpp | 2 +- 6 files changed, 12 insertions(+), 7 deletions(-) diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp index fb255292c72ea..9234143ab9928 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.cpp +++ b/dart/constraint/JointCoulombFrictionConstraint.cpp @@ -200,6 +200,7 @@ void JointCoulombFrictionConstraint::applyUnitImpulse(size_t _index) mJoint->setConstraintImpulse(i, 1.0); skeleton->updateBiasImpulse(mBodyNode); skeleton->updateVelocityChange(); + mJoint->setConstraintImpulse(i, 0.0); } ++localIndex; @@ -262,7 +263,8 @@ void JointCoulombFrictionConstraint::applyImpulse(double* _lambda) if (mActive[i] == false) continue; - mJoint->setConstraintImpulse(i, _lambda[localIndex]); + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]); mOldX[i] = _lambda[localIndex]; diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index 6ae4c9c9aefd7..802138dfa94c0 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -309,6 +309,7 @@ void JointLimitConstraint::applyUnitImpulse(size_t _index) mJoint->setConstraintImpulse(i, 1.0); skeleton->updateBiasImpulse(mBodyNode); skeleton->updateVelocityChange(); + mJoint->setConstraintImpulse(i, 0.0); } ++localIndex; @@ -370,7 +371,8 @@ void JointLimitConstraint::applyImpulse(double* _lambda) if (mActive[i] == false) continue; - mJoint->setConstraintImpulse(i, _lambda[localIndex]); + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]); mOldX[i] = _lambda[localIndex]; diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 357f15556d90e..7aea341b40aee 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -764,7 +764,8 @@ class BodyNode : bool _isImpulseLocal = false, bool _isOffsetLocal = true); - /// Clear constraint impulse + /// Clear constraint impulses and cache data used for impulse-based forward + /// dynamics algorithm virtual void clearConstraintImpulse(); /// Return constraint impulse diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index b022dcac8257d..32d2e21df3d47 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -514,8 +514,9 @@ class Skeleton : public MetaSkeleton // Impulse-based dynamics algorithms //---------------------------------------------------------------------------- - /// Clear constraint impulses: (a) spatial constraints on BodyNode and - /// (b) generalized constraints on Joint + /// Clear constraint impulses and cache data used for impulse-based forward + /// dynamics algorithm, where the constraint impulses are spatial constraints + /// on the BodyNodes and generalized constraints on the Joints. void clearConstraintImpulses(); /// Update bias impulses diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 6857d1e5576a1..b6980962f2dc3 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -184,7 +184,6 @@ void World::step(bool _resetCommand) { skel->clearInternalForces(); skel->clearExternalForces(); -// skel->clearConstraintImpulses(); skel->resetCommands(); } } diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 91c52cd85a0b3..33f393eb73113 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -794,7 +794,7 @@ void testServoMotor() // doesn't work together because they compete against each other to achieve // different joint velocities with their infinit force limits. In this case, // the position limit constraint should dominent the servo motor constraint. - // EXPECT_NEAR(jointVels[5], 0.0, tol * 1e+2); + EXPECT_NEAR(jointVels[5], 0.0, tol * 1e+2); // EXPECT_NEAR(jointVels[6], 0.0, tol * 1e+2); // TODO(JS): Servo motor with infinite force limits and infinite Coulomb // friction doesn't work because they compete against each other to achieve From 074b654f7412bbc1fa2f9b42ca2fe554f1ce582f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 29 Nov 2015 03:19:27 -0500 Subject: [PATCH 54/79] Minor improvement on generating joint constraints in ConstraintSolver --- dart/constraint/ConstraintSolver.cpp | 92 +++++++++------------------- 1 file changed, 30 insertions(+), 62 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index d12ec9459c5ba..19e3b6a847003 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -396,23 +396,46 @@ void ConstraintSolver::updateConstraints() } //---------------------------------------------------------------------------- - // Update automatic constraints: joint limit constraints + // Update automatic constraints: joint constraints //---------------------------------------------------------------------------- - // Destroy previous joint limit constraints + // Destroy previous joint constraints for (const auto& jointLimitConstraint : mJointLimitConstraints) delete jointLimitConstraint; + for (const auto& servoMotorConstraint : mServoMotorConstraints) + delete servoMotorConstraint; + for (const auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) + delete jointFrictionConstraint; mJointLimitConstraints.clear(); + mServoMotorConstraints.clear(); + mJointCoulombFrictionConstraints.clear(); - // Create new joint limit constraints + // Create new joint constraints for (const auto& skel : mSkeletons) { - const size_t numBodyNodes = skel->getNumBodyNodes(); - for (size_t i = 0; i < numBodyNodes; i++) + const size_t numJoints = skel->getNumJoints(); + for (size_t i = 0; i < numJoints; i++) { - dynamics::Joint* joint = skel->getBodyNode(i)->getParentJoint(); + dynamics::Joint* joint = skel->getJoint(i); - if (joint->isDynamic() && joint->isPositionLimitEnforced()) + if (joint->isKinematic()) + continue; + + const size_t dof = joint->getNumDofs(); + for (size_t j = 0; j < dof; ++j) + { + if (joint->getCoulombFriction(j) != 0.0) + { + mJointCoulombFrictionConstraints.push_back( + new JointCoulombFrictionConstraint(joint)); + break; + } + } + + if (joint->isPositionLimitEnforced()) mJointLimitConstraints.push_back(new JointLimitConstraint(joint)); + + if (joint->getActuatorType() == dynamics::Joint::SERVO) + mServoMotorConstraints.push_back(new ServoMotorConstraint(joint)); } } @@ -425,28 +448,6 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(jointLimitConstraint); } - //---------------------------------------------------------------------------- - // Update automatic constraints: servo motor constraints - //---------------------------------------------------------------------------- - // Destroy previous joint limit constraints - for (const auto& servoMotorConstraint : mServoMotorConstraints) - delete servoMotorConstraint; - mServoMotorConstraints.clear(); - - // Create new joint limit constraints - for (const auto& skel : mSkeletons) - { - const size_t numJoints = skel->getNumJoints(); - for (size_t i = 0; i < numJoints; i++) - { - dynamics::Joint* joint = skel->getJoint(i); - - if (joint->getActuatorType() == dynamics::Joint::SERVO) - mServoMotorConstraints.push_back(new ServoMotorConstraint(joint)); - } - } - - // Add active servo motor constraints for (auto& servoMotorConstraint : mServoMotorConstraints) { servoMotorConstraint->update(); @@ -455,39 +456,6 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(servoMotorConstraint); } - //---------------------------------------------------------------------------- - // Update automatic constraints: joint Coulomb friction constraints - //---------------------------------------------------------------------------- - // Destroy previous joint limit constraints - for (const auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) - delete jointFrictionConstraint; - mJointCoulombFrictionConstraints.clear(); - - // Create new joint limit constraints - for (const auto& skel : mSkeletons) - { - const size_t numBodyNodes = skel->getNumBodyNodes(); - for (size_t i = 0; i < numBodyNodes; i++) - { - dynamics::Joint* joint = skel->getBodyNode(i)->getParentJoint(); - - if (joint->isDynamic()) - { - const size_t dof = joint->getNumDofs(); - for (size_t i = 0; i < dof; ++i) - { - if (joint->getCoulombFriction(i) != 0.0) - { - mJointCoulombFrictionConstraints.push_back( - new JointCoulombFrictionConstraint(joint)); - break; - } - } - } - } - } - - // Add active joint limit for (auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) { jointFrictionConstraint->update(); From 86d8d4642a58c19c9613edcffa1b8382c4002b71 Mon Sep 17 00:00:00 2001 From: mklingen Date: Mon, 30 Nov 2015 16:55:40 -0500 Subject: [PATCH 55/79] added DEPRECATED flag --- dart/dynamics/Shape.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index aa51a9ddd955e..96ad15200e333 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -45,6 +45,7 @@ #include "dart/math/Geometry.h" #include "dart/common/Subject.h" #include "dart/dynamics/SmartPointer.h" +#include "dart/common/Deprecated.h" namespace dart { namespace renderer { @@ -122,10 +123,8 @@ class Shape : public virtual common::Subject /// \brief Get dimensions of bounding box. /// The dimension will be automatically determined by the sub-classes /// such as BoxShape, EllipsoidShape, CylinderShape, and MeshShape. - // TODO(JS): Single Vector3d does not fit to represent bounding box for - // biased mesh shape. Two Vector3ds might be better; one is for - // minimum verterx, and the other is for maximum verterx of the - // bounding box. + /// \deprecated Please use getBoundingBox() instead + DEPRECATED(5.2) Eigen::Vector3d getBoundingBoxDim() const; /// \brief Set local transformation of the shape w.r.t. parent frame. From 95d019c61db1edf9ac07c6999a21697dd1c3b996 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 4 Dec 2015 15:28:04 -0500 Subject: [PATCH 56/79] clarified comment --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 632b026481199..4d0c7a59ae23f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,7 +174,7 @@ if(ASSIMP_FOUND) "missing symbols for the constructor and/or destructor of " "aiScene. DART will use its own implementations of these " "functions. We recommend using a version of ASSIMP that " - "does not have this issue.") + "does not have this issue, once one becomes available.") endif(NOT ASSIMP_AISCENE_CTOR_DTOR_DEFINED) check_cxx_source_compiles( @@ -194,7 +194,7 @@ if(ASSIMP_FOUND) "missing symbols for the constructor and/or destructor of " "aiMaterial. DART will use its own implementations of " "these functions. We recommend using a version of ASSIMP " - "that does not have this issue.") + "that does not have this issue, once one becomes available.") endif(NOT ASSIMP_AIMATERIAL_CTOR_DTOR_DEFINED) unset(CMAKE_REQUIRED_INCLUDES) From 97f0cd14d3d0b4de5d711ebe2017862a184a91db Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Dec 2015 03:16:53 -0500 Subject: [PATCH 57/79] Change VskParser from a class to a namespace --- dart/utils/VskParser.cpp | 7 +++++-- dart/utils/VskParser.h | 10 ++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index d0185021199c2..a9999751f18e4 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -40,7 +40,6 @@ // Standard Library #include -#include #include #include @@ -56,6 +55,8 @@ namespace dart { namespace utils { +namespace VskParser { + namespace { using BodyPropPtr = std::shared_ptr; @@ -176,7 +177,7 @@ VskParser::Options::Options(const Eigen::Vector3d& newDefaultEllipsoidSize, } //============================================================================== -dynamics::SkeletonPtr VskParser::readSkeleton( +dynamics::SkeletonPtr readSkeleton( const common::Uri& fileUri, const common::ResourceRetrieverPtr& retrieverOrNullptr, const Options options) @@ -914,6 +915,8 @@ void tokenize(const std::string& str, } // anonymous namespace +} // namespace VskParser + } // namespace utils } // namespace dart diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index 6b6092142ab01..ec2bbe5233c4b 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -38,17 +38,15 @@ #ifndef DART_UTILS_VSKPARSER_H_ #define DART_UTILS_VSKPARSER_H_ -#include "dart/common/LocalResourceRetriever.h" +#include "dart/common/ResourceRetriever.h" #include "dart/common/Uri.h" #include "dart/dynamics/Skeleton.h" namespace dart { namespace utils { -class VskParser +namespace VskParser { -public: - /// Options struct is additional information that helps building a skeleton /// that can be used in kinematics or dynamics simulation. VSK file format /// itself doesn't provide essential properties for it such as body's shape, @@ -93,12 +91,12 @@ class VskParser }; /// Read Skeleton from VSK file - static dynamics::SkeletonPtr readSkeleton( + dynamics::SkeletonPtr readSkeleton( const common::Uri& fileUri, const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr, const Options options = Options()); -}; +} // namespace VskParser } // namespace utils } // namespace dart From 16c325db18ebe04be83197075f4bfe1e246f844f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Dec 2015 03:30:55 -0500 Subject: [PATCH 58/79] Make a note that Marker should be refactored into a Node class --- dart/dynamics/Marker.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index 5314002404973..51869f26cd05a 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -166,6 +166,8 @@ class Marker EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +// TODO: Marker class should be refactored into a Node once pull request #531 is +// finished. } // namespace dynamics } // namespace dart From 9946d62877f7c06c71b43415a42cacb5ba945829 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Dec 2015 14:12:52 -0500 Subject: [PATCH 59/79] Fix #554 --- dart/common/detail/NameManager.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/common/detail/NameManager.h b/dart/common/detail/NameManager.h index 164b274bdd4be..224f2d0fe6ad7 100644 --- a/dart/common/detail/NameManager.h +++ b/dart/common/detail/NameManager.h @@ -200,7 +200,7 @@ template void NameManager::removeEntries(const std::string& _name, const T& _obj) { removeObject(_obj); - removeName(_name, false); + removeName(_name); } //============================================================================== From e0fdd9f9469222b4d93eba1fd578fbe72031741e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Dec 2015 14:13:22 -0500 Subject: [PATCH 60/79] Add regression test for #554 --- unittests/testNameManagement.cpp | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index f0b4c6181ba29..860bb9f19f91b 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -216,6 +216,32 @@ TEST(NameManagement, SetPattern) EXPECT_TRUE( test_mgr.getObject("(2)-Entity") == entity2 ); } +//============================================================================== +TEST(NameManagement, Regression554) +{ + dart::common::NameManager< std::shared_ptr > test_mgr("test", "name"); + + std::shared_ptr int0(new int(0)); + std::shared_ptr int1(new int(1)); + std::shared_ptr int2(new int(2)); + std::shared_ptr int_another0(new int(2)); + + test_mgr.issueNewNameAndAdd(std::to_string(*int0), int0); + test_mgr.issueNewNameAndAdd(std::to_string(*int1), int1); + test_mgr.issueNewNameAndAdd(std::to_string(*int2), int2); + + EXPECT_TRUE( test_mgr.getObject("0") == int0 ); + EXPECT_TRUE( test_mgr.getObject("1") == int1 ); + EXPECT_TRUE( test_mgr.getObject("2") == int2 ); + + bool res1 = test_mgr.addName(std::to_string(*int_another0), int_another0); + EXPECT_FALSE( res1 ); + + test_mgr.removeEntries(std::to_string(*int_another0), int_another0); + bool res2 = test_mgr.addName(std::to_string(*int_another0), int_another0); + EXPECT_TRUE( res2 ); +} + //============================================================================== TEST(NameManagement, WorldSkeletons) { From d72a371271fbe18db08cbdf544e27a1b4353d9aa Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Dec 2015 14:27:57 -0500 Subject: [PATCH 61/79] Revise regression test --- unittests/testNameManagement.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index 860bb9f19f91b..774acb745c0a4 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -224,7 +224,7 @@ TEST(NameManagement, Regression554) std::shared_ptr int0(new int(0)); std::shared_ptr int1(new int(1)); std::shared_ptr int2(new int(2)); - std::shared_ptr int_another0(new int(2)); + std::shared_ptr int_another0(new int(0)); test_mgr.issueNewNameAndAdd(std::to_string(*int0), int0); test_mgr.issueNewNameAndAdd(std::to_string(*int1), int1); @@ -240,6 +240,10 @@ TEST(NameManagement, Regression554) test_mgr.removeEntries(std::to_string(*int_another0), int_another0); bool res2 = test_mgr.addName(std::to_string(*int_another0), int_another0); EXPECT_TRUE( res2 ); + + EXPECT_TRUE( test_mgr.getObject("0") == int_another0 ); + EXPECT_TRUE( test_mgr.getObject("1") == int1 ); + EXPECT_TRUE( test_mgr.getObject("2") == int2 ); } //============================================================================== From cbbc97555d9cab59c5b92915d54e04b1228b8dd8 Mon Sep 17 00:00:00 2001 From: jiayeli Date: Sat, 19 Dec 2015 16:55:51 +0800 Subject: [PATCH 62/79] add more hint to Pendulum.cpp, revise the typo error in collision.md --- docs/readthedocs/tutorials/collisions.md | 2 +- tutorials/tutorialMultiPendulum.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 5f545909437af..5c32d7f8e1876 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -575,7 +575,7 @@ And now we can add it to the world without any complaints: mWorld->addSkeleton(object); ``` -### Lesson 3d: Compute collisions +### Lesson 3c: Compute collisions Now that we've added the Skeleton to the world, we want to make sure that it wasn't actually placed inside of something accidentally. If an object in a diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 9f1fe50381fc1..2862d2c1e7e66 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -107,17 +107,17 @@ class MyWindow : public dart::gui::SimWindow mForceCountDown[index] = default_countdown; } - void changeRestPosition(double) + void changeRestPosition(double /*delta*/) { // Lesson 2a } - void changeStiffness(double) + void changeStiffness(double /*delta*/) { // Lesson 2b } - void changeDamping(double) + void changeDamping(double /*delta*/) { // Lesson 2c } From c4295f04498a488aabd6289331838ba1b15482b2 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 19 Dec 2015 17:05:41 +0100 Subject: [PATCH 63/79] Fix control file for missing urdfdom-dev dep --- debian/control | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/debian/control b/debian/control index 5f94f1a88134c..1ae442ca3a304 100644 --- a/debian/control +++ b/debian/control @@ -13,7 +13,7 @@ Build-Depends: debhelper (>= 9), libtinyxml-dev, libtinyxml2-dev, liburdfdom-dev, - libboost-dev (>= 1.54.0.1ubuntu1), + libboost-dev (>= 1.54.0.1ubuntu1), libboost-system-dev (>= 1.54.0-4ubuntu3), libboost-regex-dev (>= 1.54.0-4ubuntu3), libopenthreads-dev, @@ -71,7 +71,8 @@ Depends: ${misc:Depends}, libxmu-dev, libtinyxml2-dev, libopenthreads-dev, - libopenscenegraph-dev + libopenscenegraph-dev, + liburdfdom-dev Description: Dynamic Animation and Robotics Toolkit, development files DART is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data From 220ea220fc8fc239ed38dcc7273f53ed4f5d17e6 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 19 Dec 2015 17:40:16 +0100 Subject: [PATCH 64/79] Updated changelog --- debian/changelog | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/debian/changelog b/debian/changelog index 02b9cfe984ccf..9de4ace26f9ea 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,7 @@ +dart (6.0.0) unstable; urgency=medium + + * Added missing 'liburdfdom-dev' dependency on Ubuntu package + dart (5.1.1) unstable; urgency=medium * Add bullet dependency to package.xml @@ -129,7 +133,7 @@ dart (4.3.0) unstable; urgency=low * Added name manager for efficient name look-up and unique naming * Added all-inclusive header and namespace headers - * Added DegreeOfFreedom class for getting/setting data of individual generalized coordinates + * Added DegreeOfFreedom class for getting/setting data of individual generalized coordinates * Added hybrid dynamics * Added joint actuator types * Added Coulomb joint friction @@ -209,4 +213,3 @@ dart (3.0.0) unstable; urgency=low * Add "common" namespace -- Tobias Kunz Fri, 11 Oct 2013 22:00:00 -0400 - From d2427a342be7b80c43ebb2e4e0b987e89209c0c9 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 19 Dec 2015 17:59:45 +0100 Subject: [PATCH 65/79] Updated changelogs --- Changelog.md | 8 ++++++-- debian/changelog | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Changelog.md b/Changelog.md index c0f6952c8c7aa..652f764a977fa 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,8 @@ +### Version 6.0.0 (2015-12-19) + +1. Added missing `liburdfdom-dev` dependency in Ubuntu package + * [Pull request #574](https://github.com/dartsim/dart/pull/574) + ### Version 5.1.1 (2015-11-06) 1. Add bullet dependency to package.xml @@ -321,7 +326,7 @@ * [Pull request #277](https://github.com/dartsim/dart/pull/277) 1. Added all-inclusive header and namespace headers * [Pull request #278](https://github.com/dartsim/dart/pull/278) -1. Added DegreeOfFreedom class for getting/setting data of individual generalized coordinates +1. Added DegreeOfFreedom class for getting/setting data of individual generalized coordinates * [Pull request #288](https://github.com/dartsim/dart/pull/288) 1. Added hybrid dynamics * [Pull request #298](https://github.com/dartsim/dart/pull/298) @@ -474,4 +479,3 @@ 1. Clean-up of the Robot class 1. Removed Object class 1. More robust build and installation process on Linux - diff --git a/debian/changelog b/debian/changelog index 9de4ace26f9ea..7a50f63eaffbe 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,6 +1,6 @@ dart (6.0.0) unstable; urgency=medium - * Added missing 'liburdfdom-dev' dependency on Ubuntu package + * Added missing 'liburdfdom-dev' dependency in Ubuntu package dart (5.1.1) unstable; urgency=medium From e6ae75b4f7c2d3c41b0340864a433578665381c3 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Dec 2015 14:39:53 -0500 Subject: [PATCH 66/79] created unittest to trigger issue #576 --- unittests/testConcurrency.cpp | 76 +++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 unittests/testConcurrency.cpp diff --git a/unittests/testConcurrency.cpp b/unittests/testConcurrency.cpp new file mode 100644 index 0000000000000..072debcdca49c --- /dev/null +++ b/unittests/testConcurrency.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#include +#include + +#include + +#include "dart/simulation/World.h" + +#include "TestHelpers.h" + +using namespace dart; +using namespace dynamics; + +//============================================================================== +void createAndDestroyFrames(int threadNum) +{ + for(size_t i=0; i < 100; ++i) + { + SimpleFrame someFrame(Frame::World(), + "Frame_"+std::to_string(threadNum)+std::to_string(i)); + } +} + +//============================================================================== +TEST(Concurrency, FrameDeletion) +{ + std::vector> futures; + for(size_t i=0; i < 10; ++i) + futures.push_back(std::async(std::launch::async, + &createAndDestroyFrames, i)); + + for(size_t i=0; i < futures.size(); ++i) + futures[i].get(); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 51f23e0c2b9388dcdcb52a95e4bb6e11190e88d9 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Dec 2015 14:40:48 -0500 Subject: [PATCH 67/79] fixed concurrency issue which will hopefully address issue #576 --- dart/dynamics/Entity.cpp | 11 ++++++++--- dart/dynamics/Frame.cpp | 5 +++-- unittests/testConcurrency.cpp | 1 + 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index a26cc603a34da..167d57ff6a16b 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -416,7 +416,7 @@ void Entity::changeParentFrame(Frame* _newParentFrame) const Frame* oldParentFrame = mParentFrame; - if (!mAmQuiet && nullptr != mParentFrame) + if (!mAmQuiet && nullptr != mParentFrame && !mParentFrame->isWorld()) { // If this entity has a parent Frame, tell that parent that it is losing // this child @@ -432,8 +432,13 @@ void Entity::changeParentFrame(Frame* _newParentFrame) if (!mAmQuiet && nullptr != mParentFrame) { - mParentFrame->mChildEntities.insert(this); - mParentFrame->processNewEntity(this); + if(!mParentFrame->isWorld()) + { + // The WorldFrame should not keep track of its children, or else we get + // concurrency issues (race conditions). + mParentFrame->mChildEntities.insert(this); + mParentFrame->processNewEntity(this); + } notifyTransformUpdate(); } diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 66d8078889896..1e50a5c1cf576 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -555,7 +555,7 @@ void Frame::changeParentFrame(Frame* _newParentFrame) } } - if(mParentFrame) + if(mParentFrame && !mParentFrame->isWorld()) { FramePtrSet::iterator it = mParentFrame->mChildFrames.find(this); if(it != mParentFrame->mChildFrames.end()) @@ -568,8 +568,9 @@ void Frame::changeParentFrame(Frame* _newParentFrame) return; } - if(!mAmQuiet) + if(!mAmQuiet && !_newParentFrame->isWorld()) _newParentFrame->mChildFrames.insert(this); + Entity::changeParentFrame(_newParentFrame); } diff --git a/unittests/testConcurrency.cpp b/unittests/testConcurrency.cpp index 072debcdca49c..81f8333691f1a 100644 --- a/unittests/testConcurrency.cpp +++ b/unittests/testConcurrency.cpp @@ -59,6 +59,7 @@ void createAndDestroyFrames(int threadNum) //============================================================================== TEST(Concurrency, FrameDeletion) { + // Regression test for issue #576 std::vector> futures; for(size_t i=0; i < 10; ++i) futures.push_back(std::async(std::launch::async, From 99491af6292e22ff8752ee40a4ab797e8c5b0e90 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Dec 2015 14:48:26 -0500 Subject: [PATCH 68/79] added tests to ensure that the world frame is not gaining any children --- unittests/testConcurrency.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/unittests/testConcurrency.cpp b/unittests/testConcurrency.cpp index 81f8333691f1a..4cb0ac38e555d 100644 --- a/unittests/testConcurrency.cpp +++ b/unittests/testConcurrency.cpp @@ -51,8 +51,14 @@ void createAndDestroyFrames(int threadNum) { for(size_t i=0; i < 100; ++i) { + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); + SimpleFrame someFrame(Frame::World(), "Frame_"+std::to_string(threadNum)+std::to_string(i)); + + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); } } @@ -66,7 +72,14 @@ TEST(Concurrency, FrameDeletion) &createAndDestroyFrames, i)); for(size_t i=0; i < futures.size(); ++i) + { + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); futures[i].get(); + } + + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); } //============================================================================== From b0f4ee72db2e81701eba3c1f73c44a02f880f10f Mon Sep 17 00:00:00 2001 From: Donny Date: Sat, 2 Jan 2016 10:24:34 -0800 Subject: [PATCH 69/79] Allow CMake to generate for VS 2015. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d0c7a59ae23f..4dd8b9cbc49a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -471,7 +471,7 @@ endif() #=============================================================================== if(MSVC) # Visual Studio enables c++11 support by default - if(NOT MSVC12) + if(NOT MSVC12 AND NOT MSVC14) message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP4") From 8d23ecdf98106946507d3ae297e594fd8ccdfa9d Mon Sep 17 00:00:00 2001 From: Donny Date: Sat, 2 Jan 2016 11:34:15 -0800 Subject: [PATCH 70/79] Include to define std::cout. --- dart/utils/urdf/urdf_world_parser.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/utils/urdf/urdf_world_parser.cpp b/dart/utils/urdf/urdf_world_parser.cpp index a41429df6066d..2fd7d67053e01 100644 --- a/dart/utils/urdf/urdf_world_parser.cpp +++ b/dart/utils/urdf/urdf_world_parser.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include From 7318372d294121bbd92bf7b0e473d88aa0025372 Mon Sep 17 00:00:00 2001 From: Donny Date: Sat, 2 Jan 2016 11:40:15 -0800 Subject: [PATCH 71/79] Forward declare structs as structs, not classes. --- dart/constraint/ConstrainedGroup.h | 2 +- dart/dynamics/Skeleton.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h index 1ba17fe549c62..e9ea79ab22ca8 100644 --- a/dart/constraint/ConstrainedGroup.h +++ b/dart/constraint/ConstrainedGroup.h @@ -49,7 +49,7 @@ class Skeleton; namespace constraint { -class ConstraintInfo; +struct ConstraintInfo; class ConstraintBase; class ConstraintSolver; diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 47fb247accda7..47e63c54bc59e 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -833,7 +833,7 @@ class Skeleton : public MetaSkeleton friend class EndEffector; protected: - class DataCache; + struct DataCache; /// Constructor called by create() Skeleton(const Properties& _properties); From bec9a8d517482e0d010634a23721a008aa98192e Mon Sep 17 00:00:00 2001 From: Donny Date: Sat, 2 Jan 2016 11:41:15 -0800 Subject: [PATCH 72/79] Resolve Visual Studio error C4800. 'int': forcing value to bool 'true' or 'false'. Visual Studio 2015 won't allow the conversion from int to bool. --- dart/math/Helpers.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index aef0273027604..5409d7c8cf0ba 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -182,7 +182,7 @@ inline bool isInt(double _x) { /// \brief Returns whether _v is a NaN (Not-A-Number) value inline bool isNan(double _v) { #ifdef _WIN32 - return _isnan(_v); + return _isnan(_v) != 0; #else return std::isnan(_v); #endif From 2305827c413f602dfd46bcc041d6deefff5cc2e2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 12 Jan 2016 13:55:26 -0500 Subject: [PATCH 73/79] Updating the constraint namespace to C++11. Note: API changes --- apps/atlasSimbicon/Controller.cpp | 6 +- apps/atlasSimbicon/Controller.h | 6 +- apps/jointConstraints/MyWindow.cpp | 2 +- apps/jointConstraints/MyWindow.h | 2 +- apps/rigidLoop/Main.cpp | 3 +- dart/common/SmartPointer.h | 52 ++++++++++++++++ dart/constraint/ConstrainedGroup.cpp | 14 +++-- dart/constraint/ConstrainedGroup.h | 14 +++-- dart/constraint/ConstraintSolver.cpp | 39 ++++++------ dart/constraint/ConstraintSolver.h | 34 ++++------- dart/constraint/DantzigLCPSolver.cpp | 7 +-- dart/constraint/PGSLCPSolver.cpp | 7 +-- dart/constraint/SmartPointer.h | 64 ++++++++++++++++++++ dart/dynamics/SmartPointer.h | 45 ++++++-------- docs/readthedocs/tutorials/collisions.md | 2 +- docs/readthedocs/tutorials/multi-pendulum.md | 7 +-- tutorials/tutorialCollisions-Finished.cpp | 9 +-- tutorials/tutorialCollisions.cpp | 7 ++- tutorials/tutorialMultiPendulum-Finished.cpp | 6 +- 19 files changed, 212 insertions(+), 114 deletions(-) create mode 100644 dart/common/SmartPointer.h create mode 100644 dart/constraint/SmartPointer.h diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index 740376a5bdcb9..0cda32dae1ce8 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -231,7 +231,7 @@ void Controller::harnessPelvis() return; BodyNode* bd = mAtlasRobot->getBodyNode("pelvis"); - mWeldJointConstraintPelvis = new WeldJointConstraint(bd); + mWeldJointConstraintPelvis = std::make_shared(bd); mConstratinSolver->addConstraint(mWeldJointConstraintPelvis); mPelvisHarnessOn = true; @@ -257,7 +257,7 @@ void Controller::harnessLeftFoot() return; BodyNode* bd = mAtlasRobot->getBodyNode("l_foot"); - mWeldJointConstraintLeftFoot = new WeldJointConstraint(bd); + mWeldJointConstraintLeftFoot = std::make_shared(bd); mLeftFootHarnessOn = true; dtmsg << "Left foot is harnessed." << std::endl; @@ -282,7 +282,7 @@ void Controller::harnessRightFoot() return; BodyNode* bd = mAtlasRobot->getBodyNode("r_foot"); - mWeldJointConstraintRightFoot = new WeldJointConstraint(bd); + mWeldJointConstraintRightFoot = std::make_shared(bd); mRightFootHarnessOn = true; dtmsg << "Right foot is harnessed." << std::endl; diff --git a/apps/atlasSimbicon/Controller.h b/apps/atlasSimbicon/Controller.h index 7e3c080e6df17..4f30135a70b83 100644 --- a/apps/atlasSimbicon/Controller.h +++ b/apps/atlasSimbicon/Controller.h @@ -172,13 +172,13 @@ class Controller dart::dynamics::BodyNode* _getRightFoot() const; /// \brief Weld joint constraint for pelvis harnessing - dart::constraint::WeldJointConstraint* mWeldJointConstraintPelvis; + dart::constraint::WeldJointConstraintPtr mWeldJointConstraintPelvis; /// \brief Weld joint constraint for left foot harnessing - dart::constraint::WeldJointConstraint* mWeldJointConstraintLeftFoot; + dart::constraint::WeldJointConstraintPtr mWeldJointConstraintLeftFoot; /// \brief Weld joint constraint for right foot harnessing - dart::constraint::WeldJointConstraint* mWeldJointConstraintRightFoot; + dart::constraint::WeldJointConstraintPtr mWeldJointConstraintRightFoot; /// \brief Initial state of the robot Eigen::VectorXd mInitialState; diff --git a/apps/jointConstraints/MyWindow.cpp b/apps/jointConstraints/MyWindow.cpp index fff10381130a7..6235bea1ba0e5 100644 --- a/apps/jointConstraints/MyWindow.cpp +++ b/apps/jointConstraints/MyWindow.cpp @@ -142,7 +142,7 @@ void MyWindow::keyboard(unsigned char key, int x, int y) if (mHarnessOn) { BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis"); - mWeldJoint = new WeldJointConstraint(bd); + mWeldJoint = std::make_shared(bd); mWorld->getConstraintSolver()->addConstraint(mWeldJoint); } else diff --git a/apps/jointConstraints/MyWindow.h b/apps/jointConstraints/MyWindow.h index 98b49b591da8e..b75000e684224 100644 --- a/apps/jointConstraints/MyWindow.h +++ b/apps/jointConstraints/MyWindow.h @@ -72,7 +72,7 @@ class MyWindow : public dart::gui::SimWindow private: Eigen::Vector3d mForce; Controller* mController; - dart::constraint::WeldJointConstraint* mWeldJoint; + dart::constraint::WeldJointConstraintPtr mWeldJoint; int mImpulseDuration; bool mHarnessOn; diff --git a/apps/rigidLoop/Main.cpp b/apps/rigidLoop/Main.cpp index 831c754656cf7..044c3d3cc4b01 100644 --- a/apps/rigidLoop/Main.cpp +++ b/apps/rigidLoop/Main.cpp @@ -74,7 +74,8 @@ int main(int argc, char* argv[]) bd2->getVisualizationShape(0)->setColor(Eigen::Vector3d(1.0, 0.0, 0.0)); Eigen::Vector3d offset(0.0, 0.025, 0.0); Eigen::Vector3d jointPos = bd1->getTransform() * offset; - BallJointConstraint *cl = new BallJointConstraint( bd1, bd2, jointPos); + BallJointConstraintPtr cl = + std::make_shared( bd1, bd2, jointPos); //WeldJointConstraint *cl = new WeldJointConstraint(bd1, bd2); myWorld->getConstraintSolver()->addConstraint(cl); diff --git a/dart/common/SmartPointer.h b/dart/common/SmartPointer.h new file mode 100644 index 0000000000000..06476c038e17d --- /dev/null +++ b/dart/common/SmartPointer.h @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_COMMON_SMARTPOINTER_H_ +#define DART_COMMON_SMARTPOINTER_H_ + +#include + +// -- Standard shared/weak pointers -- +// Define a typedef for const and non-const version of shared_ptr and weak_ptr +// for the class X +#define DART_COMMON_MAKE_SHARED_WEAK( X )\ + class X ;\ + typedef std::shared_ptr< X > X ## Ptr;\ + typedef std::shared_ptr< const X > Const ## X ## Ptr;\ + typedef std::weak_ptr< X > Weak ## X ## Ptr;\ + typedef std::weak_ptr< const X > WeakConst ## X ## Ptr; + +#endif // DART_COMMON_SMARTPOINTER_H_ diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index cd60fda2ed6e2..07b9d4396f55c 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -57,7 +57,7 @@ ConstrainedGroup::~ConstrainedGroup() } //============================================================================== -void ConstrainedGroup::addConstraint(ConstraintBase* _constraint) +void ConstrainedGroup::addConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint != nullptr && "Null constraint pointer is now allowed."); assert(containConstraint(_constraint) == false @@ -74,14 +74,14 @@ size_t ConstrainedGroup::getNumConstraints() const } //============================================================================== -ConstraintBase* ConstrainedGroup::getConstraint(size_t _index) const +ConstraintBasePtr ConstrainedGroup::getConstraint(size_t _index) const { assert(_index < mConstraints.size()); return mConstraints[_index]; } //============================================================================== -void ConstrainedGroup::removeConstraint(ConstraintBase* _constraint) +void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint != nullptr && "Null constraint pointer is now allowed."); assert(containConstraint(_constraint) == true @@ -108,16 +108,20 @@ void ConstrainedGroup::removeAllConstraints() } //============================================================================== -bool ConstrainedGroup::containConstraint(ConstraintBase* _constraint) const +bool ConstrainedGroup::containConstraint( + const ConstConstraintBasePtr& _constraint) const { // std::cout << "CommunityTEST::_containConstraint(): Not implemented." // << std::endl; + // TODO(MXG): Is there any reason these functions are not implemented yet? + return false; } //============================================================================== -bool ConstrainedGroup::checkAndAddConstraint(ConstraintBase* _constraint) +bool ConstrainedGroup::checkAndAddConstraint( + const ConstraintBasePtr& _constraint) { std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented." << std::endl; diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h index e9ea79ab22ca8..4e163a5fb773a 100644 --- a/dart/constraint/ConstrainedGroup.h +++ b/dart/constraint/ConstrainedGroup.h @@ -41,6 +41,8 @@ #include #include +#include "dart/constraint/SmartPointer.h" + namespace dart { namespace dynamics { @@ -74,16 +76,16 @@ class ConstrainedGroup //---------------------------------------------------------------------------- /// Add constraint - void addConstraint(ConstraintBase* _constraint); + void addConstraint(const ConstraintBasePtr& _constraint); /// Return number of constraints in this constrained group size_t getNumConstraints() const; /// Return a constraint - ConstraintBase* getConstraint(size_t _index) const; + ConstraintBasePtr getConstraint(size_t _index) const; /// Remove constraint - void removeConstraint(ConstraintBase* _constraint); + void removeConstraint(const ConstraintBasePtr& _constraint); /// Remove all constraints void removeAllConstraints(); @@ -99,13 +101,13 @@ class ConstrainedGroup private: /// Return true if _constraint is contained - bool containConstraint(ConstraintBase* _constraint) const; + bool containConstraint(const ConstConstraintBasePtr& _constraint) const; /// Return true and add the constraint if _constraint is contained - bool checkAndAddConstraint(ConstraintBase* _constraint); + bool checkAndAddConstraint(const ConstraintBasePtr& _constraint); /// List of constraints - std::vector mConstraints; + std::vector mConstraints; /// std::shared_ptr mRootSkeleton; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 19e3b6a847003..8c940f7be5b9c 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -182,7 +182,7 @@ void ConstraintSolver::removeAllSkeletons() } //============================================================================== -void ConstraintSolver::addConstraint(ConstraintBase* _constraint) +void ConstraintSolver::addConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint); @@ -197,7 +197,7 @@ void ConstraintSolver::addConstraint(ConstraintBase* _constraint) } //============================================================================== -void ConstraintSolver::removeConstraint(ConstraintBase* _constraint) +void ConstraintSolver::removeConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint); @@ -306,7 +306,8 @@ bool ConstraintSolver::checkAndAddSkeleton(const SkeletonPtr& _skeleton) } //============================================================================== -bool ConstraintSolver::containConstraint(const ConstraintBase* _constraint) const +bool ConstraintSolver::containConstraint( + const ConstConstraintBasePtr& _constraint) const { return std::find(mManualConstraints.begin(), mManualConstraints.end(), @@ -314,7 +315,8 @@ bool ConstraintSolver::containConstraint(const ConstraintBase* _constraint) cons } //============================================================================== -bool ConstraintSolver::checkAndAddConstraint(ConstraintBase* _constraint) +bool ConstraintSolver::checkAndAddConstraint( + const ConstraintBasePtr& _constraint) { if (!containConstraint(_constraint)) { @@ -352,13 +354,9 @@ void ConstraintSolver::updateConstraints() mCollisionDetector->detectCollision(true, true); // Destroy previous contact constraints - for (const auto& contactConstraint : mContactConstraints) - delete contactConstraint; mContactConstraints.clear(); // Destroy previous soft contact constraints - for (const auto& softContactConstraint : mSoftContactConstraints) - delete softContactConstraint; mSoftContactConstraints.clear(); // Create new contact constraints @@ -369,11 +367,12 @@ void ConstraintSolver::updateConstraints() if (isSoftContact(ct)) { mSoftContactConstraints.push_back( - new SoftContactConstraint(ct, mTimeStep)); + std::make_shared(ct, mTimeStep)); } else { - mContactConstraints.push_back(new ContactConstraint(ct, mTimeStep)); + mContactConstraints.push_back( + std::make_shared(ct, mTimeStep)); } } @@ -399,12 +398,6 @@ void ConstraintSolver::updateConstraints() // Update automatic constraints: joint constraints //---------------------------------------------------------------------------- // Destroy previous joint constraints - for (const auto& jointLimitConstraint : mJointLimitConstraints) - delete jointLimitConstraint; - for (const auto& servoMotorConstraint : mServoMotorConstraints) - delete servoMotorConstraint; - for (const auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) - delete jointFrictionConstraint; mJointLimitConstraints.clear(); mServoMotorConstraints.clear(); mJointCoulombFrictionConstraints.clear(); @@ -426,16 +419,18 @@ void ConstraintSolver::updateConstraints() if (joint->getCoulombFriction(j) != 0.0) { mJointCoulombFrictionConstraints.push_back( - new JointCoulombFrictionConstraint(joint)); + std::make_shared(joint)); break; } } if (joint->isPositionLimitEnforced()) - mJointLimitConstraints.push_back(new JointLimitConstraint(joint)); + mJointLimitConstraints.push_back( + std::make_shared(joint)); if (joint->getActuatorType() == dynamics::Joint::SERVO) - mServoMotorConstraints.push_back(new ServoMotorConstraint(joint)); + mServoMotorConstraints.push_back( + std::make_shared(joint)); } } @@ -478,7 +473,7 @@ void ConstraintSolver::buildConstrainedGroups() //---------------------------------------------------------------------------- // Unite skeletons according to constraints's relationships //---------------------------------------------------------------------------- - for (std::vector::iterator it = mActiveConstraints.begin(); + for (std::vector::iterator it = mActiveConstraints.begin(); it != mActiveConstraints.end(); ++it) { (*it)->uniteSkeletons(); @@ -487,7 +482,7 @@ void ConstraintSolver::buildConstrainedGroups() //---------------------------------------------------------------------------- // Build constraint groups //---------------------------------------------------------------------------- - for (std::vector::const_iterator it = mActiveConstraints.begin(); + for (std::vector::const_iterator it = mActiveConstraints.begin(); it != mActiveConstraints.end(); ++it) { bool found = false; @@ -514,7 +509,7 @@ void ConstraintSolver::buildConstrainedGroups() } // Add active constraints to constrained groups - for (std::vector::const_iterator it = mActiveConstraints.begin(); + for (std::vector::const_iterator it = mActiveConstraints.begin(); it != mActiveConstraints.end(); ++it) { dynamics::SkeletonPtr skel = (*it)->getRootSkeleton(); diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h index abb669e10c0e3..830d46a7fccfd 100644 --- a/dart/constraint/ConstraintSolver.h +++ b/dart/constraint/ConstraintSolver.h @@ -41,6 +41,7 @@ #include +#include "dart/constraint/SmartPointer.h" #include "dart/constraint/ConstraintBase.h" #include "dart/collision/CollisionDetector.h" @@ -52,17 +53,6 @@ class Skeleton; namespace constraint { -class ConstrainedGroup; -class ConstraintBase; -class ClosedLoopConstraint; -class ContactConstraint; -class SoftContactConstraint; -class JointLimitConstraint; -class ServoMotorConstraint; -class JointCoulombFrictionConstraint; -class JointConstraint; -class LCPSolver; - // TODO: // - RootSkeleton concept @@ -97,10 +87,10 @@ class ConstraintSolver void removeAllSkeletons(); /// Add a constraint - void addConstraint(ConstraintBase* _constraint); + void addConstraint(const ConstraintBasePtr& _constraint); /// Remove a constraint - void removeConstraint(ConstraintBase* _constraint); + void removeConstraint(const ConstraintBasePtr& _constraint); /// Remove all constraints void removeAllConstraints(); @@ -128,10 +118,10 @@ class ConstraintSolver bool checkAndAddSkeleton(const dynamics::SkeletonPtr& _skeleton); /// Check if the constraint is contained in this solver - bool containConstraint(const ConstraintBase* _constraint) const; + bool containConstraint(const ConstConstraintBasePtr& _constraint) const; /// Add constraint if the constraint is not contained in this solver - bool checkAndAddConstraint(ConstraintBase* _constraint); + bool checkAndAddConstraint(const ConstraintBasePtr& _constraint); /// Update constraints void updateConstraints(); @@ -158,25 +148,25 @@ class ConstraintSolver std::vector mSkeletons; /// Contact constraints those are automatically created - std::vector mContactConstraints; + std::vector mContactConstraints; /// Soft contact constraints those are automatically created - std::vector mSoftContactConstraints; + std::vector mSoftContactConstraints; /// Joint limit constraints those are automatically created - std::vector mJointLimitConstraints; + std::vector mJointLimitConstraints; /// Servo motor constraints those are automatically created - std::vector mServoMotorConstraints; + std::vector mServoMotorConstraints; /// Joint Coulomb friction constraints those are automatically created - std::vector mJointCoulombFrictionConstraints; + std::vector mJointCoulombFrictionConstraints; /// Constraints that manually added - std::vector mManualConstraints; + std::vector mManualConstraints; /// Active constraints - std::vector mActiveConstraints; + std::vector mActiveConstraints; /// Constraint group list std::vector mConstrainedGroups; diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index 1c3c6d069e416..3e051b782c9e1 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/dart/constraint/DantzigLCPSolver.cpp @@ -92,7 +92,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) // std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl; for (size_t i = 1; i < numConstraints; ++i) { - ConstraintBase* constraint = _group->getConstraint(i - 1); + const ConstraintBasePtr& constraint = _group->getConstraint(i - 1); assert(constraint->getDimension() > 0); offset[i] = offset[i - 1] + constraint->getDimension(); // std::cout << "offset[" << i << "]: " << offset[i] << std::endl; @@ -101,10 +101,9 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) // For each constraint ConstraintInfo constInfo; constInfo.invTimeStep = 1.0 / mTimeStep; - ConstraintBase* constraint; for (size_t i = 0; i < numConstraints; ++i) { - constraint = _group->getConstraint(i); + const ConstraintBasePtr& constraint = _group->getConstraint(i); constInfo.x = x + offset[i]; constInfo.lo = lo + offset[i]; @@ -173,7 +172,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) // Apply constraint impulses for (size_t i = 0; i < numConstraints; ++i) { - constraint = _group->getConstraint(i); + const ConstraintBasePtr& constraint = _group->getConstraint(i); constraint->applyImpulse(x + offset[i]); constraint->excite(); } diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index 50ea58047fd5c..3e443ce05c50d 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -92,7 +92,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) // std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl; for (size_t i = 1; i < numConstraints; ++i) { - ConstraintBase* constraint = _group->getConstraint(i - 1); + const ConstraintBasePtr& constraint = _group->getConstraint(i - 1); assert(constraint->getDimension() > 0); offset[i] = offset[i - 1] + constraint->getDimension(); // std::cout << "offset[" << i << "]: " << offset[i] << std::endl; @@ -101,10 +101,9 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) // For each constraint ConstraintInfo constInfo; constInfo.invTimeStep = 1.0 / mTimeStep; - ConstraintBase* constraint; for (size_t i = 0; i < numConstraints; ++i) { - constraint = _group->getConstraint(i); + const ConstraintBasePtr& constraint = _group->getConstraint(i); constInfo.x = x + offset[i]; constInfo.lo = lo + offset[i]; @@ -176,7 +175,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) // Apply constraint impulses for (size_t i = 0; i < numConstraints; ++i) { - constraint = _group->getConstraint(i); + const ConstraintBasePtr& constraint = _group->getConstraint(i); constraint->applyImpulse(x + offset[i]); constraint->excite(); } diff --git a/dart/constraint/SmartPointer.h b/dart/constraint/SmartPointer.h new file mode 100644 index 0000000000000..49b31cdc107e4 --- /dev/null +++ b/dart/constraint/SmartPointer.h @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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. + */ + +#ifndef DART_CONSTRAINT_SMARTPOINTER_H_ +#define DART_CONSTRAINT_SMARTPOINTER_H_ + +#include "dart/common/SmartPointer.h" + +namespace dart { +namespace constraint { + +DART_COMMON_MAKE_SHARED_WEAK(ConstrainedGroup) +DART_COMMON_MAKE_SHARED_WEAK(ConstraintBase) +DART_COMMON_MAKE_SHARED_WEAK(ClosedLoopConstraint) +DART_COMMON_MAKE_SHARED_WEAK(ContactConstraint) +DART_COMMON_MAKE_SHARED_WEAK(SoftContactConstraint) +DART_COMMON_MAKE_SHARED_WEAK(JointLimitConstraint) +DART_COMMON_MAKE_SHARED_WEAK(ServoMotorConstraint) +DART_COMMON_MAKE_SHARED_WEAK(JointCoulombFrictionConstraint) +DART_COMMON_MAKE_SHARED_WEAK(JointConstraint) +DART_COMMON_MAKE_SHARED_WEAK(LCPSolver) + +DART_COMMON_MAKE_SHARED_WEAK(BallJointConstraint) +DART_COMMON_MAKE_SHARED_WEAK(WeldJointConstraint) + +DART_COMMON_MAKE_SHARED_WEAK(BalanceConstraint) + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_SMARTPOINTER_H_ diff --git a/dart/dynamics/SmartPointer.h b/dart/dynamics/SmartPointer.h index 2562d5e0d9794..08519a68ad975 100644 --- a/dart/dynamics/SmartPointer.h +++ b/dart/dynamics/SmartPointer.h @@ -37,6 +37,7 @@ #ifndef DART_DYNAMICS_SMARTPOINTER_H_ #define DART_DYNAMICS_SMARTPOINTER_H_ +#include "dart/common/SmartPointer.h" #include "dart/dynamics/detail/BodyNodePtr.h" #include "dart/dynamics/detail/JointPtr.h" #include "dart/dynamics/detail/DegreeOfFreedomPtr.h" @@ -52,22 +53,12 @@ namespace dart { namespace dynamics { -// -- Standard shared/weak pointers -- -// Define a typedef for const and non-const version of shared_ptr and weak_ptr -// for the class X -#define DART_DYNAMICS_MAKE_SHARED_WEAK( X ) \ - class X ; \ - typedef std::shared_ptr< X > X ## Ptr; \ - typedef std::shared_ptr< const X > Const ## X ## Ptr; \ - typedef std::weak_ptr< X > Weak ## X ## Ptr; \ - typedef std::weak_ptr< const X > WeakConst ## X ## Ptr; - -DART_DYNAMICS_MAKE_SHARED_WEAK(SimpleFrame) +DART_COMMON_MAKE_SHARED_WEAK(SimpleFrame) //----------------------------------------------------------------------------- // Skeleton Smart Pointers //----------------------------------------------------------------------------- -DART_DYNAMICS_MAKE_SHARED_WEAK(Skeleton) +DART_COMMON_MAKE_SHARED_WEAK(Skeleton) // These pointers will take the form of: // std::shared_ptr --> SkeletonPtr // std::shared_ptr --> ConstSkeletonPtr @@ -75,29 +66,29 @@ DART_DYNAMICS_MAKE_SHARED_WEAK(Skeleton) // std::weak_ptr --> WeakConstSkeletonPtr // MetaSkeleton smart pointers -DART_DYNAMICS_MAKE_SHARED_WEAK(MetaSkeleton) +DART_COMMON_MAKE_SHARED_WEAK(MetaSkeleton) // ReferentialSkeleton smart pointers -DART_DYNAMICS_MAKE_SHARED_WEAK(ReferentialSkeleton) +DART_COMMON_MAKE_SHARED_WEAK(ReferentialSkeleton) -DART_DYNAMICS_MAKE_SHARED_WEAK(Group) -DART_DYNAMICS_MAKE_SHARED_WEAK(Linkage) -DART_DYNAMICS_MAKE_SHARED_WEAK(Branch) -DART_DYNAMICS_MAKE_SHARED_WEAK(Chain) +DART_COMMON_MAKE_SHARED_WEAK(Group) +DART_COMMON_MAKE_SHARED_WEAK(Linkage) +DART_COMMON_MAKE_SHARED_WEAK(Branch) +DART_COMMON_MAKE_SHARED_WEAK(Chain) //----------------------------------------------------------------------------- // Shape Smart Pointers //----------------------------------------------------------------------------- -DART_DYNAMICS_MAKE_SHARED_WEAK(Shape) -DART_DYNAMICS_MAKE_SHARED_WEAK(ArrowShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(BoxShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(CylinderShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(EllipsoidShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(LineSegmentShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(MeshShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(PlaneShape) -DART_DYNAMICS_MAKE_SHARED_WEAK(SoftMeshShape) +DART_COMMON_MAKE_SHARED_WEAK(Shape) +DART_COMMON_MAKE_SHARED_WEAK(ArrowShape) +DART_COMMON_MAKE_SHARED_WEAK(BoxShape) +DART_COMMON_MAKE_SHARED_WEAK(CylinderShape) +DART_COMMON_MAKE_SHARED_WEAK(EllipsoidShape) +DART_COMMON_MAKE_SHARED_WEAK(LineSegmentShape) +DART_COMMON_MAKE_SHARED_WEAK(MeshShape) +DART_COMMON_MAKE_SHARED_WEAK(PlaneShape) +DART_COMMON_MAKE_SHARED_WEAK(SoftMeshShape) //----------------------------------------------------------------------------- diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 5c32d7f8e1876..118c0882ef169 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -820,7 +820,7 @@ tail BodyNode. Now we have everything we need to construct the constraint: ```cpp -auto constraint = new dart::constraint::BallJointConstraint( +auto constraint = std::make_shared( head, tail, offset); ``` diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index 64e95b70d1ad5..c96ea9fc33def 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -364,7 +364,8 @@ Eigen::Vector3d location = Now we can create the BallJointConstraint: ```cpp -mBallConstraint = new dart::constraint::BallJointConstraint(tip, location); +mBallConstraint = + std::make_shared(tip, location); ``` And then add it to the world: @@ -378,12 +379,10 @@ Now we also want to be able to remove this constraint. In the function ```cpp mWorld->getConstraintSolver()->removeConstraint(mBallConstraint); -delete mBallConstraint; mBallConstraint = nullptr; ``` -Currently DART does not use smart pointers for dynamic constraints, so they -need to be explicitly deleted. This may be revised in a later version of DART. +Setting mBallConstraint to a nullptr will allow its smart pointer to delete it. **Now you are ready to run the demo!** diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 4b349cb8e292d..f2a63f7fcb539 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -291,7 +291,7 @@ class MyWindow : public dart::gui::SimWindow // Compute the offset where the JointConstraint should be located Eigen::Vector3d offset = Eigen::Vector3d(0, 0, default_shape_height / 2.0); offset = tail->getWorldTransform() * offset; - auto constraint = new dart::constraint::BallJointConstraint( + auto constraint = std::make_shared( head, tail, offset); mWorld->getConstraintSolver()->addConstraint(constraint); @@ -304,13 +304,14 @@ class MyWindow : public dart::gui::SimWindow { for(size_t i=0; igetBodyNode1()->getSkeleton() == skel || constraint->getBodyNode2()->getSkeleton() == skel) { mWorld->getConstraintSolver()->removeConstraint(constraint); mJointConstraints.erase(mJointConstraints.begin()+i); - delete constraint; break; // There should only be one constraint per skeleton } } @@ -328,7 +329,7 @@ class MyWindow : public dart::gui::SimWindow /// History of the active JointConstraints so that we can properly delete them /// when a Skeleton gets removed - std::vector mJointConstraints; + std::vector mJointConstraints; /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 35ec5b1120725..5d83d1f5469e9 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -224,13 +224,14 @@ class MyWindow : public SimWindow { for(size_t i=0; igetBodyNode1()->getSkeleton() == skel || constraint->getBodyNode2()->getSkeleton() == skel) { mWorld->getConstraintSolver()->removeConstraint(constraint); mJointConstraints.erase(mJointConstraints.begin()+i); - delete constraint; break; // There should only be one constraint per skeleton } } @@ -248,7 +249,7 @@ class MyWindow : public SimWindow /// History of the active JointConstraints so that we can properly delete them /// when a Skeleton gets removed - std::vector mJointConstraints; + std::vector mJointConstraints; /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index c4fc738c39f96..0aa3bee614248 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -160,7 +160,8 @@ class MyWindow : public dart::gui::SimWindow // Attach the last link to the world Eigen::Vector3d location = tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height); - mBallConstraint = new dart::constraint::BallJointConstraint(tip, location); + mBallConstraint = + std::make_shared(tip, location); mWorld->getConstraintSolver()->addConstraint(mBallConstraint); } @@ -168,7 +169,6 @@ class MyWindow : public dart::gui::SimWindow void removeConstraint() { mWorld->getConstraintSolver()->removeConstraint(mBallConstraint); - delete mBallConstraint; mBallConstraint = nullptr; } @@ -331,7 +331,7 @@ class MyWindow : public dart::gui::SimWindow SkeletonPtr mPendulum; /// Pointer to the ball constraint that we will be turning on and off - dart::constraint::BallJointConstraint* mBallConstraint; + dart::constraint::BallJointConstraintPtr mBallConstraint; /// Number of iterations before clearing a force entry std::vector mForceCountDown; From ec269b5f21425950ecf7c6018ea0f7d41164c889 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 12 Jan 2016 14:30:04 -0500 Subject: [PATCH 74/79] Fixed segmentation fault for null meshes --- dart/dynamics/ArrowShape.cpp | 3 +++ dart/dynamics/MeshShape.cpp | 8 ++++++++ dart/dynamics/MeshShape.h | 2 -- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/ArrowShape.cpp b/dart/dynamics/ArrowShape.cpp index bb7d82a2f4c83..4a77c9c6f09fd 100644 --- a/dart/dynamics/ArrowShape.cpp +++ b/dart/dynamics/ArrowShape.cpp @@ -235,6 +235,9 @@ void ArrowShape::configureArrow(const Eigen::Vector3d& _tail, for(size_t i=0; i<4; ++i) for(size_t j=0; j<4; ++j) node->mTransformation[i][j] = tf(i,j); + + _updateBoundingBoxDim(); + updateVolume(); } //============================================================================== diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 4bbd273157116..77af9e0398fe1 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -299,6 +299,14 @@ void MeshShape::updateVolume() { } void MeshShape::_updateBoundingBoxDim() { + + if(!mMesh) + { + mBoundingBox.setMin(Eigen::Vector3d::Zero()); + mBoundingBox.setMax(Eigen::Vector3d::Zero()); + return; + } + double max_X = -std::numeric_limits::infinity(); double max_Y = -std::numeric_limits::infinity(); double max_Z = -std::numeric_limits::infinity(); diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index c1905342bfa5b..6c9068375b23d 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -145,11 +145,9 @@ class MeshShape : public Shape { // Documentation inherited. void updateVolume() override; -private: /// \brief void _updateBoundingBoxDim(); -protected: /// \brief const aiScene* mMesh; From 34a2c1bcf814838d023c40042ca37fdee0fa52e7 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 15 Jan 2016 11:04:37 -0500 Subject: [PATCH 75/79] Implement missing implementations in ConstrainedGroup --- dart/constraint/ConstrainedGroup.cpp | 35 ++++++++++------------------ 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 07b9d4396f55c..0563446a52688 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -36,6 +36,7 @@ #include "dart/constraint/ConstrainedGroup.h" +#include #include #include @@ -59,9 +60,9 @@ ConstrainedGroup::~ConstrainedGroup() //============================================================================== void ConstrainedGroup::addConstraint(const ConstraintBasePtr& _constraint) { - assert(_constraint != nullptr && "Null constraint pointer is now allowed."); + assert(_constraint != nullptr && "Attempted to add nullptr."); assert(containConstraint(_constraint) == false - && "Don't try to add same constraint multiple times into Community."); + && "Attempted to add a duplicate constraint."); assert(_constraint->isActive()); mConstraints.push_back(_constraint); @@ -83,9 +84,9 @@ ConstraintBasePtr ConstrainedGroup::getConstraint(size_t _index) const //============================================================================== void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint) { - assert(_constraint != nullptr && "Null constraint pointer is now allowed."); - assert(containConstraint(_constraint) == true - && "Don't try to remove a constraint not contained in Community."); + assert(_constraint != nullptr && "Attempted to add nullptr."); + assert(containConstraint(_constraint) + && "Attempted to remove not existing constraint."); mConstraints.erase( remove(mConstraints.begin(), mConstraints.end(), _constraint), @@ -95,15 +96,6 @@ void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint) //============================================================================== void ConstrainedGroup::removeAllConstraints() { -// dtwarn << "ConstrainedGroup::removeAllConstraints(): " -// << "Not implemented." << std::endl; - - // TODO(JS): Temporary implementation -// for (size_t i = 0; i < mConstraints.size(); ++i) -// { -// delete mConstraints[i]; -// } - mConstraints.clear(); } @@ -111,22 +103,19 @@ void ConstrainedGroup::removeAllConstraints() bool ConstrainedGroup::containConstraint( const ConstConstraintBasePtr& _constraint) const { -// std::cout << "CommunityTEST::_containConstraint(): Not implemented." -// << std::endl; - - // TODO(MXG): Is there any reason these functions are not implemented yet? - - return false; + return std::find(mConstraints.begin(), mConstraints.end(), _constraint) + != mConstraints.end(); } //============================================================================== bool ConstrainedGroup::checkAndAddConstraint( const ConstraintBasePtr& _constraint) { - std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented." - << std::endl; + if (containConstraint(_constraint)) + return false; - return false; + addConstraint(_constraint); + return true; } //============================================================================== From 43585eacf6bc1f11a9608c6955a8a8cf80f9251f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 15 Jan 2016 11:17:34 -0500 Subject: [PATCH 76/79] Remove a private method ConstrainedGroup::checkAndAddConstraint since it's not being called anywhere, and compile ConstrainedGroup::containConstraint() only in debug mode --- dart/constraint/ConstrainedGroup.cpp | 13 +------------ dart/constraint/ConstrainedGroup.h | 5 ++--- 2 files changed, 3 insertions(+), 15 deletions(-) diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 0563446a52688..0347a71097990 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -61,7 +61,7 @@ ConstrainedGroup::~ConstrainedGroup() void ConstrainedGroup::addConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint != nullptr && "Attempted to add nullptr."); - assert(containConstraint(_constraint) == false + assert(!containConstraint(_constraint) && "Attempted to add a duplicate constraint."); assert(_constraint->isActive()); @@ -107,17 +107,6 @@ bool ConstrainedGroup::containConstraint( != mConstraints.end(); } -//============================================================================== -bool ConstrainedGroup::checkAndAddConstraint( - const ConstraintBasePtr& _constraint) -{ - if (containConstraint(_constraint)) - return false; - - addConstraint(_constraint); - return true; -} - //============================================================================== size_t ConstrainedGroup::getTotalDimension() const { diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h index 4e163a5fb773a..50a23a387f3db 100644 --- a/dart/constraint/ConstrainedGroup.h +++ b/dart/constraint/ConstrainedGroup.h @@ -100,11 +100,10 @@ class ConstrainedGroup friend class ConstraintSolver; private: +#ifndef NDEBUG /// Return true if _constraint is contained bool containConstraint(const ConstConstraintBasePtr& _constraint) const; - - /// Return true and add the constraint if _constraint is contained - bool checkAndAddConstraint(const ConstraintBasePtr& _constraint); +#endif /// List of constraints std::vector mConstraints; From 8c994dc2d4f145fcba1cbc21c2a4e4622c7b1b7f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 15 Jan 2016 14:02:19 -0500 Subject: [PATCH 77/79] Hide definition of ConstrainedGroup::containConstraint() when it's not in debug mode --- dart/constraint/ConstrainedGroup.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 0347a71097990..e294a3c07780f 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -100,12 +100,14 @@ void ConstrainedGroup::removeAllConstraints() } //============================================================================== +#ifndef NDEBUG bool ConstrainedGroup::containConstraint( const ConstConstraintBasePtr& _constraint) const { return std::find(mConstraints.begin(), mConstraints.end(), _constraint) != mConstraints.end(); } +#endif //============================================================================== size_t ConstrainedGroup::getTotalDimension() const From 2194f99d179487454434f2c60436ca2ce1c3798e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jan 2016 22:16:58 -0500 Subject: [PATCH 78/79] Make Visual Studio 2015 the minimum requirement - also cmake 3.1.3 is now the minimum requirement for Visual Studio since MSVC14 variable is supported since that cmake version. --- CMakeLists.txt | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4dd8b9cbc49a7..8ceaa98f96cee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,11 @@ #=============================================================================== # CMake settings #=============================================================================== -cmake_minimum_required(VERSION 2.8.6 FATAL_ERROR) +if(MSVC) + cmake_minimum_required(VERSION 3.1.3) +else() + cmake_minimum_required(VERSION 2.8.6) +endif() # Use MACOSX_RPATH by default on OS X. This was added in CMake 2.8.12 and # became default in CMake 3.0. Explicitly setting this policy is necessary to @@ -218,7 +222,7 @@ endif() # Boost set(DART_MIN_BOOST_VERSION 1.46.0 CACHE INTERNAL "Boost min version requirement" FORCE) -if(MSVC OR MSVC90 OR MSVC10) +if(MSVC) add_definitions(-DBOOST_ALL_NO_LIB) endif() add_definitions(-DBOOST_TEST_DYN_LINK) @@ -471,8 +475,8 @@ endif() #=============================================================================== if(MSVC) # Visual Studio enables c++11 support by default - if(NOT MSVC12 AND NOT MSVC14) - message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") + if(NOT MSVC14) + message(FATAL_ERROR "${PROJECT_NAME} requires VS 2015 or greater.") endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP4") set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/LTCG /INCREMENTAL:NO") From b10b4a3b5de9aeaee4aa533c3ac249264ddfc29f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 22 Jan 2016 07:47:10 -0500 Subject: [PATCH 79/79] Use constexpr without checking compiler as Visual Studio 2015 became the minimum requirement --- dart/dynamics/InvalidIndex.h | 5 ----- dart/math/Helpers.h | 15 --------------- 2 files changed, 20 deletions(-) diff --git a/dart/dynamics/InvalidIndex.h b/dart/dynamics/InvalidIndex.h index 9040a563ef8f3..d5b42b86f2358 100644 --- a/dart/dynamics/InvalidIndex.h +++ b/dart/dynamics/InvalidIndex.h @@ -42,12 +42,7 @@ namespace dart { namespace dynamics { -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -const size_t INVALID_INDEX = static_cast(-1); -#else constexpr size_t INVALID_INDEX = static_cast(-1); -#endif } // namespace dynamics } // namespace dart diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index 5409d7c8cf0ba..0ceaa896b53c6 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -66,34 +66,19 @@ inline int delta(int _i, int _j) { return 0; } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x, std::false_type) { return static_cast(0) < x; } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x, std::true_type) { return (static_cast(0) < x) - (x < static_cast(0)); } -#if defined(_MSC_VER) -// TODO: Change to constexpr once Visual Studio supports it -template inline -#else template inline constexpr -#endif int sign(T x) { return sign(x, std::is_signed());