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/CMakeLists.txt b/CMakeLists.txt index 50d8efdca191d..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 @@ -41,7 +45,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") @@ -62,6 +66,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() @@ -150,6 +155,55 @@ 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 "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, once one becomes available.") + 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 "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, once one becomes available.") + 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.") @@ -168,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) @@ -410,7 +464,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) @@ -420,14 +474,16 @@ 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.") + if(NOT MSVC14) + message(FATAL_ERROR "${PROJECT_NAME} requires VS 2015 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_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") + endif(NOT DART_MSVC_DEFAULT_OPTIONS) elseif(CMAKE_COMPILER_IS_GNUCXX) set(CMAKE_CXX_FLAGS "-Wall -msse2 -fPIC") execute_process( diff --git a/Changelog.md b/Changelog.md index 5b4f589742a08..652f764a977fa 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,37 @@ +### 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 + * [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) + +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 @@ -292,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) @@ -445,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/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/appveyor.yml b/appveyor.yml index 8ada23ea80804..38f6ebdbe3460 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -11,8 +11,9 @@ platform: # specify custom environment variables environment: + MSVC_DEFAULT_OPTIONS: ON BOOST_ROOT: C:\Libraries\boost - BOOST_LIBRARYDIR: C:\Libraries\boost\stage\lib + 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: @@ -43,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 @@ -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 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) - 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/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/utils/ParserVsk.h_ b/dart/common/SmartPointer.h similarity index 71% rename from dart/utils/ParserVsk.h_ rename to dart/common/SmartPointer.h index e4fe1e6f0f758..06476c038e17d 100644 --- a/dart/utils/ParserVsk.h_ +++ b/dart/common/SmartPointer.h @@ -1,9 +1,8 @@ /* - * Copyright (c) 2011, Georgia Tech Research Corporation + * Copyright (c) 2016, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Sehoon Ha - * Date: 06/12/2011 + * Author(s): Michael X. Grey * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -35,17 +34,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_KINEMATICS_PARSER_VSK_H -#define DART_KINEMATICS_PARSER_VSK_H +#ifndef DART_COMMON_SMARTPOINTER_H_ +#define DART_COMMON_SMARTPOINTER_H_ -namespace kinematics { - class Skeleton; -} // namespace kinematics +#include -#define VERBOSE false -#define VSK_OK 0 -#define VSK_ERROR 1 -int readVSKFile(const char* const filename, kinematics::Skeleton* skel); - -#endif // #ifndef DART_KINEMATICS_PARSER_VSK_H +// -- 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/common/detail/NameManager.h b/dart/common/detail/NameManager.h index 120f7d619f8ec..0e55febebb23b 100644 --- a/dart/common/detail/NameManager.h +++ b/dart/common/detail/NameManager.h @@ -194,7 +194,7 @@ template void NameManager::removeEntries(const std::string& _name, const T& _obj) { removeObject(_obj); - removeName(_name, false); + removeName(_name); } //============================================================================== diff --git a/dart/config.h.in b/dart/config.h.in index 0128e0e5613ae..588c24f91bfc9 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 @@ -41,4 +50,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/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/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index cd60fda2ed6e2..e294a3c07780f 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -36,6 +36,7 @@ #include "dart/constraint/ConstrainedGroup.h" +#include #include #include @@ -57,11 +58,11 @@ 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 - && "Don't try to add same constraint multiple times into Community."); + assert(_constraint != nullptr && "Attempted to add nullptr."); + assert(!containConstraint(_constraint) + && "Attempted to add a duplicate constraint."); assert(_constraint->isActive()); mConstraints.push_back(_constraint); @@ -74,18 +75,18 @@ 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 - && "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,35 +96,18 @@ void ConstrainedGroup::removeConstraint(ConstraintBase* _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(); } //============================================================================== -bool ConstrainedGroup::containConstraint(ConstraintBase* _constraint) const -{ -// std::cout << "CommunityTEST::_containConstraint(): Not implemented." -// << std::endl; - - return false; -} - -//============================================================================== -bool ConstrainedGroup::checkAndAddConstraint(ConstraintBase* _constraint) +#ifndef NDEBUG +bool ConstrainedGroup::containConstraint( + const ConstConstraintBasePtr& _constraint) const { - std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented." - << std::endl; - - return false; + return std::find(mConstraints.begin(), mConstraints.end(), _constraint) + != mConstraints.end(); } +#endif //============================================================================== size_t ConstrainedGroup::getTotalDimension() const diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h index 1ba17fe549c62..50a23a387f3db 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 { @@ -49,7 +51,7 @@ class Skeleton; namespace constraint { -class ConstraintInfo; +struct ConstraintInfo; class ConstraintBase; class ConstraintSolver; @@ -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(); @@ -98,14 +100,13 @@ class ConstrainedGroup friend class ConstraintSolver; private: +#ifndef NDEBUG /// Return true if _constraint is contained - bool containConstraint(ConstraintBase* _constraint) const; - - /// Return true and add the constraint if _constraint is contained - bool checkAndAddConstraint(ConstraintBase* _constraint); + bool containConstraint(const ConstConstraintBasePtr& _constraint) const; +#endif /// 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 fc13796744e3d..8c940f7be5b9c 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" @@ -181,7 +182,7 @@ void ConstraintSolver::removeAllSkeletons() } //============================================================================== -void ConstraintSolver::addConstraint(ConstraintBase* _constraint) +void ConstraintSolver::addConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint); @@ -196,7 +197,7 @@ void ConstraintSolver::addConstraint(ConstraintBase* _constraint) } //============================================================================== -void ConstraintSolver::removeConstraint(ConstraintBase* _constraint) +void ConstraintSolver::removeConstraint(const ConstraintBasePtr& _constraint) { assert(_constraint); @@ -305,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(), @@ -313,7 +315,8 @@ bool ConstraintSolver::containConstraint(const ConstraintBase* _constraint) cons } //============================================================================== -bool ConstraintSolver::checkAndAddConstraint(ConstraintBase* _constraint) +bool ConstraintSolver::checkAndAddConstraint( + const ConstraintBasePtr& _constraint) { if (!containConstraint(_constraint)) { @@ -351,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 @@ -368,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)); } } @@ -395,23 +395,42 @@ void ConstraintSolver::updateConstraints() } //---------------------------------------------------------------------------- - // Update automatic constraints: joint limit constraints + // Update automatic constraints: joint constraints //---------------------------------------------------------------------------- - // Destroy previous joint limit constraints - for (const auto& jointLimitConstraint : mJointLimitConstraints) - delete jointLimitConstraint; + // Destroy previous joint constraints 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()) - mJointLimitConstraints.push_back(new JointLimitConstraint(joint)); + 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( + std::make_shared(joint)); + break; + } + } + + if (joint->isPositionLimitEnforced()) + mJointLimitConstraints.push_back( + std::make_shared(joint)); + + if (joint->getActuatorType() == dynamics::Joint::SERVO) + mServoMotorConstraints.push_back( + std::make_shared(joint)); } } @@ -424,39 +443,14 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(jointLimitConstraint); } - //---------------------------------------------------------------------------- - // 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) + for (auto& servoMotorConstraint : mServoMotorConstraints) { - const size_t numBodyNodes = skel->getNumBodyNodes(); - for (size_t i = 0; i < numBodyNodes; i++) - { - dynamics::Joint* joint = skel->getBodyNode(i)->getParentJoint(); + servoMotorConstraint->update(); - 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; - } - } - } - } + if (servoMotorConstraint->isActive()) + mActiveConstraints.push_back(servoMotorConstraint); } - // Add active joint limit for (auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) { jointFrictionConstraint->update(); @@ -479,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(); @@ -488,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; @@ -515,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 b6acac0bd2d3d..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,16 +53,6 @@ class Skeleton; namespace constraint { -class ConstrainedGroup; -class ConstraintBase; -class ClosedLoopConstraint; -class ContactConstraint; -class SoftContactConstraint; -class JointLimitConstraint; -class JointCoulombFrictionConstraint; -class JointConstraint; -class LCPSolver; - // TODO: // - RootSkeleton concept @@ -96,13 +87,10 @@ class ConstraintSolver void removeAllSkeletons(); /// Add a constraint - void addConstraint(ConstraintBase* _constraint); - - /// Return the number of constraints - size_t getNumConstraints() const; + void addConstraint(const ConstraintBasePtr& _constraint); /// Remove a constraint - void removeConstraint(ConstraintBase* _constraint); + void removeConstraint(const ConstraintBasePtr& _constraint); /// Remove all constraints void removeAllConstraints(); @@ -130,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(); @@ -160,22 +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; - /// Joint limit constraints those are automatically created - std::vector mJointCoulombFrictionConstraints; + /// 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 - 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/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/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/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/renderer/OpenGLCamera.h b/dart/constraint/SmartPointer.h similarity index 57% rename from dart/renderer/OpenGLCamera.h rename to dart/constraint/SmartPointer.h index 458b4b4f8a605..49b31cdc107e4 100644 --- a/dart/renderer/OpenGLCamera.h +++ b/dart/constraint/SmartPointer.h @@ -1,8 +1,8 @@ /* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation + * Copyright (c) 2016, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jie (Jay) Tan + * Author(s): Michael X. Grey * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -26,43 +26,39 @@ * 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 + * 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 +#ifndef DART_CONSTRAINT_SMARTPOINTER_H_ +#define DART_CONSTRAINT_SMARTPOINTER_H_ -#include -#include "dart/renderer/Camera.h" +#include "dart/common/SmartPointer.h" namespace dart { -namespace renderer { +namespace constraint { -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) {} +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) - 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) {} +DART_COMMON_MAKE_SHARED_WEAK(BalanceConstraint) -}; - -} // namespace renderer +} // namespace constraint } // namespace dart -#endif // #ifndef DART_RENDERER_OPENGLCAMERA_H +#endif // DART_CONSTRAINT_SMARTPOINTER_H_ 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/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 983b1cc894880..70ea9b4e45dd8 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -454,8 +454,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 4afea264bf46d..4b7d8df8f340d 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -254,7 +254,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; @@ -814,7 +814,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/BoxShape.cpp b/dart/dynamics/BoxShape.cpp index 11ef3fa0d0d03..16903678c6975 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 { @@ -48,21 +48,41 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size) assert(_size[0] > 0.0); assert(_size[1] > 0.0); assert(_size[2] > 0.0); - mBoundingBoxDim = _size; - initMeshes(); - computeVolume(); + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); + 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(); + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); + updateVolume(); } const Eigen::Vector3d& BoxShape::getSize() const { @@ -80,22 +100,20 @@ void BoxShape::draw(renderer::RenderInterface* _ri, _ri->setPenColor(mColor); _ri->pushMatrix(); _ri->transform(mTransform); - _ri->drawCube(mBoundingBoxDim); + _ri->drawCube(mBoundingBox.computeFullExtents()); _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..fbc5deaf04617 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,23 +88,41 @@ 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; - 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/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/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/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp index 4e7efb9231fdb..d71cf9a329e67 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() { @@ -56,8 +55,9 @@ void EllipsoidShape::setSize(const Eigen::Vector3d& _size) { assert(_size[1] > 0.0); assert(_size[2] > 0.0); mSize = _size; - mBoundingBoxDim = _size; - computeVolume(); + mBoundingBox.setMin(-_size * 0.5); + mBoundingBox.setMax(_size * 0.5); + updateVolume(); } const Eigen::Vector3d&EllipsoidShape::getSize() const { @@ -77,19 +77,36 @@ void EllipsoidShape::draw(renderer::RenderInterface* _ri, _ri->setPenColor(mColor); _ri->pushMatrix(); _ri->transform(mTransform); - _ri->drawEllipsoid(mBoundingBoxDim); + _ri->drawEllipsoid(mBoundingBox.computeFullExtents()); _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 +114,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/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/EulerJoint.h b/dart/dynamics/EulerJoint.h index e261b8461e410..391c403d39f33 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -56,6 +56,8 @@ class EulerJoint : public detail::EulerJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, EulerJointAddon) + EulerJoint(const EulerJoint&) = delete; + /// Destructor virtual ~EulerJoint(); diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 6e61ca5063182..c3dcf70bcf64d 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -565,7 +565,7 @@ void Frame::changeParentFrame(Frame* _newParentFrame) } } - if(mParentFrame) + if(mParentFrame && !mParentFrame->isWorld()) { FramePtrSet::iterator it = mParentFrame->mChildFrames.find(this); if(it != mParentFrame->mChildFrames.end()) @@ -578,8 +578,9 @@ void Frame::changeParentFrame(Frame* _newParentFrame) return; } - if(!mAmQuiet) + if(!mAmQuiet && !_newParentFrame->isWorld()) _newParentFrame->mChildFrames.insert(this); + Entity::changeParentFrame(_newParentFrame); } 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/Group.cpp b/dart/dynamics/Group.cpp index f9c551cf2ea30..9572220a93961 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 _includeJoints, bool _includeDofs) { - GroupPtr group(new Group(_name, _bodyNodes)); + GroupPtr group(new Group(_name, _bodyNodes, _includeJoints, _includeDofs)); group->mPtr = group; return group; } //============================================================================== GroupPtr Group::create(const std::string& _name, - const std::vector& _dofs) + const std::vector& _dofs, + bool _includeBodyNodes, bool _includeJoints) { - GroupPtr group(new Group(_name, _dofs)); + GroupPtr group(new Group(_name, _dofs, _includeBodyNodes, _includeJoints)); + 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; } @@ -166,8 +177,109 @@ void Group::swapDofIndices(size_t _index1, size_t _index2) } //============================================================================== -void Group::addBodyNode(BodyNode* _bn, bool _warning) +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); + + 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) +{ + 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); + + 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(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) @@ -176,26 +288,42 @@ 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; } //============================================================================== 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) @@ -209,8 +337,7 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) return false; } - unregisterBodyNode(_bn); - + unregisterBodyNode(_bn, false); return true; } @@ -218,86 +345,321 @@ 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::addJoint(Joint* _joint, bool _addDofs, bool _warning) { - if(INVALID_INDEX != getIndexOf(_dof, false)) + if(nullptr == _joint) { if(_warning) + { + 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, false)) + { + 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; + } + + // 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; } - - return; } - registerDegreeOfFreedom(_dof); + return added; } //============================================================================== -void 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) - 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()); + // Make sure the DegreeOfFreedom continues to exist for the duration of this + // scope + DegreeOfFreedomPtr hold(_dof); - return true; + 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), false) == 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 allGood = true; + bool removed = false; for(DegreeOfFreedom* dof : _dofs) - allGood &= removeDof(dof, _warning); + removed |= removeDof(dof, _cleanupJoint, _warning); - return allGood; + return removed; } //============================================================================== Group::Group(const std::string& _name, - const std::vector& _bodyNodes) + const std::vector& _bodyNodes, + bool _includeJoints, bool _includeDofs) { setName(_name); addBodyNodes(_bodyNodes); + + if(_includeDofs || _includeJoints) + { + for(size_t i=0; i < _bodyNodes.size(); ++i) + { + Joint* joint = _bodyNodes[i]->getParentJoint(); + + if(_includeJoints) + addJoint(joint, false); + + if(_includeDofs) + { + 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, bool _includeJoints) { setName(_name); - addDofs(_dofs); + addDofs(_dofs, _includeJoints); + + 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); + + 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 ec26e9fabdabb..a2f90da5283c6 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -46,15 +46,30 @@ class Group : public ReferentialSkeleton { public: - /// Create a Group out of a set of BodyNodes + /// 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()); + 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. 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 _includeJoints = 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; @@ -65,72 +80,169 @@ 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, + /// 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. + 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 - /// warned when you attempt to add the same DegreeOfFreedom twice, and an + /// 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. - void addDof(DegreeOfFreedom* _dof, bool _warning=true); + /// + /// 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 set of DegreesOfFreedom to this Group. Note: The BodyNodes of these - /// DegreesOfFreedom will also be added. If _warning is true, you will be + /// 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. - void addDofs(const std::vector& _dofs, bool _warning=true); + /// + /// This function will return false if the DegreeOfFreedom was already in the + /// Group. + bool addDof(DegreeOfFreedom* _dof, bool _addJoint=true, bool _warning=true); - /// Remove a DegreeOfFreedom from this Group. If _warning is true, you will be + /// 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 _addJoint = true, bool _warning=true); + + /// 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 already 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 in this + /// Group. + 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 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); + bool _cleanupJoint=true, bool _warning=true); 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 _includeJoints, + bool _includeDofs); /// Alternative constructor Group(const std::string& _name, - const std::vector& _dofs); + const std::vector& _dofs, + bool _includeBodyNodes, + bool _includeJoints); + + /// MetaSkeleton-based constructor + Group(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton); }; } // dynamics 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/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; diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index 707efc0a95eb8..a0a41ad97324e 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 @@ -776,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 diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index cee96cc3a7efe..5f8b63bb77781 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -103,7 +103,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/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/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/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..51869f26cd05a 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,16 +155,19 @@ 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 + }; +// TODO: Marker class should be refactored into a Node once pull request #531 is +// finished. } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index a78344ecc34b5..77af9e0398fe1 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 0.0); assert(_scale[2] > 0.0); mScale = _scale; - computeVolume(); + updateVolume(); + _updateBoundingBoxDim(); } const Eigen::Vector3d& MeshShape::getScale() const { @@ -278,9 +280,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); @@ -290,16 +293,20 @@ Eigen::Matrix3d MeshShape::computeInertia(double _mass) const { return inertia; } -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; +void MeshShape::updateVolume() { + Eigen::Vector3d bounds = mBoundingBox.computeFullExtents(); + mVolume = bounds.x() * bounds.y() * bounds.z(); } 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(); @@ -323,9 +330,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 * 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( diff --git a/dart/dynamics/MeshShape.h b/dart/dynamics/MeshShape.h index 1e02cc2328bd5..6c9068375b23d 100644 --- a/dart/dynamics/MeshShape.h +++ b/dart/dynamics/MeshShape.h @@ -139,17 +139,15 @@ 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 void _updateBoundingBoxDim(); -protected: /// \brief const aiScene* mMesh; diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 8d754129e06e2..3f0e429f6509e 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -73,6 +73,8 @@ class MultiDofJoint : DART_BAKE_SPECIALIZED_ADDON_IRREGULAR( MultiDofJoint::Addon, MultiDofJointAddon ) + MultiDofJoint(const MultiDofJoint&) = delete; + /// Destructor virtual ~MultiDofJoint(); diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 236965965366d..cf103efc4302b 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -61,6 +61,8 @@ class PlanarJoint : public detail::PlanarJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PlanarJointAddon) + PlanarJoint(const PlanarJoint&) = delete; + /// Destructor virtual ~PlanarJoint(); 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/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 2e7047c297977..bfc82c3e90b10 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -55,6 +55,8 @@ class PrismaticJoint : public detail::PrismaticJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PrismaticJointAddon) + PrismaticJoint(const PrismaticJoint&) = delete; + /// Destructor virtual ~PrismaticJoint(); diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 96d68a10b2eb7..6466941464b89 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -140,27 +140,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); } //============================================================================== @@ -191,7 +183,7 @@ size_t ReferentialSkeleton::getIndexOf(const Joint* _joint, bool _warning) const return INVALID_INDEX; } - return it->second.mBodyNodeIndex; + return it->second.mJointIndex; } //============================================================================== @@ -944,13 +936,81 @@ math::LinearJacobian ReferentialSkeleton::getCOMLinearJacobianDeriv( } //============================================================================== -void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) +void ReferentialSkeleton::registerComponent(BodyNode* _bn) { + registerBodyNode(_bn); + registerJoint(_bn->getParentJoint()); + 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(); +} + +//============================================================================== +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(); } //============================================================================== @@ -964,9 +1024,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); @@ -977,17 +1037,30 @@ 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); + unregisterJoint(_bn); +} + +//============================================================================== +void ReferentialSkeleton::unregisterBodyNode( + BodyNode* _bn, bool _unregisterDofs) { if(nullptr == _bn) { @@ -1011,27 +1084,88 @@ 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; igetParentJoint(); + + 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, bool removeBnIfEmpty) + BodyNode* _bn, size_t _localIndex) { if(nullptr == _bn) { @@ -1050,10 +1184,10 @@ void ReferentialSkeleton::unregisterDegreeOfFreedom( it->second.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; } @@ -1064,27 +1198,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(); } @@ -1131,5 +1253,31 @@ void ReferentialSkeleton::updateCaches() mFc = Eigen::VectorXd::Zero(nDofs); } +//============================================================================== +ReferentialSkeleton::IndexMap::IndexMap() + : mBodyNodeIndex(INVALID_INDEX), + mJointIndex(INVALID_INDEX) +{ + // Do nothing +} + +//============================================================================== +bool ReferentialSkeleton::IndexMap::isExpired() const +{ + if(INVALID_INDEX != mBodyNodeIndex) + return false; + + if(INVALID_INDEX != mJointIndex) + 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..f7baeb0f8231f 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -327,21 +327,37 @@ 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 Joint and + /// 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 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); - /// 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 DegreeOfFreedom from this ReferentialSkeleton. Only usable by + /// 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 Joint from this ReferentialSkeleton. This can only be used by /// derived classes. - void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex, - bool removeBnIfEmpty = true); + 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); /// Update the caches to match any changes to the structure of this /// ReferentialSkeleton @@ -357,8 +373,19 @@ 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 and mJointIndex 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 @@ -374,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/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 000c80fb33e23..127c7174b1c35 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -55,6 +55,8 @@ class RevoluteJoint : public detail::RevoluteJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, RevoluteJointAddon) + RevoluteJoint(const RevoluteJoint&) = delete; + /// Destructor virtual ~RevoluteJoint(); diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index c9fc2bd2b8c6a..2ffcc1b5607c9 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -55,6 +55,8 @@ class ScrewJoint : public detail::ScrewJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, ScrewJointAddon) + ScrewJoint(const ScrewJoint&) = delete; + /// Destructor virtual ~ScrewJoint(); 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..7f3d1029a1032 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -43,8 +43,10 @@ #include #include "dart/math/Geometry.h" +#include "dart/common/Deprecated.h" #include "dart/common/Subject.h" #include "dart/dynamics/SmartPointer.h" +#include "dart/common/Deprecated.h" namespace dart { namespace renderer { @@ -54,7 +56,6 @@ class RenderInterface; namespace dart { namespace dynamics { - /// \brief class Shape : public virtual common::Subject { @@ -114,14 +115,18 @@ 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. - // 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. - const Eigen::Vector3d& getBoundingBoxDim() const; + /// \deprecated Please use getBoundingBox() instead + DEPRECATED(5.2) + Eigen::Vector3d getBoundingBoxDim() const; /// \brief Set local transformation of the shape w.r.t. parent frame. void setLocalTransform(const Eigen::Isometry3d& _Transform); @@ -140,7 +145,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,14 +205,17 @@ 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. - 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/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 89f23ddfb9831..c010b2bdf5980 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -890,6 +890,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 cdce46f80208e..6ebc3c1f1f68a 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -492,6 +492,9 @@ class Skeleton : public virtual common::AddonManager, /// 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); @@ -641,8 +644,9 @@ class Skeleton : public virtual common::AddonManager, // 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 @@ -958,7 +962,7 @@ class Skeleton : public virtual common::AddonManager, friend class EndEffector; protected: - class DataCache; + struct DataCache; /// Constructor called by create() Skeleton(const Properties& _properties); diff --git a/dart/dynamics/SmartPointer.h b/dart/dynamics/SmartPointer.h index 66e15a828d2ca..ada68ca4c0577 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,14 @@ 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_COMMON_MAKE_SHARED_WEAK(SimpleFrame) -DART_DYNAMICS_MAKE_SHARED_WEAK(SimpleFrame) +DART_COMMON_MAKE_SHARED_WEAK(NodeDestructor) //----------------------------------------------------------------------------- // 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 +68,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) //----------------------------------------------------------------------------- @@ -176,7 +169,6 @@ DART_DYNAMICS_MAKE_NODEPTR(EndEffector) // TemplateWeakNodePtr --> WeakEndEffectorPtr // TemplateWeakNodePtr --> WeakConstEndEffectorPtr -DART_DYNAMICS_MAKE_SHARED_WEAK(NodeDestructor) //----------------------------------------------------------------------------- // InverseKinematics Smart Pointers 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/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 a41b784b3e43b..465d2720be20a 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -55,6 +55,8 @@ class UniversalJoint : public detail::UniversalJointBase DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, UniversalJointAddon) + 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(); diff --git a/dart/dynamics/detail/InverseKinematics.h b/dart/dynamics/detail/InverseKinematics.h index 6e8e6353fa1a2..ea72fc28fad21 100644 --- a/dart/dynamics/detail/InverseKinematics.h +++ b/dart/dynamics/detail/InverseKinematics.h @@ -81,7 +81,7 @@ void InverseKinematics::setDofs(const std::vector& _dofs) setDofs(indices); } -} // namespace dart } // namespace dynamics +} // namespace dart #endif // DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_H_ 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/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() diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index e1693fe9bba2f..b749c8da9962d 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(); - // 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(); + // Second axis + Eigen::Vector3d axis1 = axis0.cross(Eigen::Vector3d::UnitX()); + if (axis1.norm() < DART_EPSILON) + axis1 = axis0.cross(Eigen::Vector3d::UnitY()); + axis1.normalize(); - const Eigen::Vector3d& axisX = (axisY.cross(axisZ)).normalized(); + // Third axis + const Eigen::Vector3d axis2 = axis0.cross(axis1).normalized(); - // Assign orientation - result.linear().col(0) = axisX; - result.linear().col(1) = axisY; - result.linear().col(2) = axisZ; + // Assign the three axes + Eigen::Matrix3d result; + int index = axisType; + result.col(index) = axis0; + result.col(++index % 3) = axis1; + result.col(++index % 3) = axis2; - // Assign translation - result.translation() = _origin; + assert(verifyRotation(result)); + + return result; +} + +//============================================================================== +Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis, + const Eigen::Vector3d& translation, + AxisType axisType) +{ + Eigen::Isometry3d result = Eigen::Isometry3d::Identity(); + + 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, @@ -1951,5 +1975,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 bd292e9c079d8..42b89dcaf9271 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); @@ -428,20 +450,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 @@ -549,6 +562,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 diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index aef0273027604..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()); @@ -182,7 +167,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 diff --git a/dart/optimizer/Function.cpp b/dart/optimizer/Function.cpp index 0f0c3eb2171c0..83e8530f65585 100644 --- a/dart/optimizer/Function.cpp +++ b/dart/optimizer/Function.cpp @@ -81,9 +81,13 @@ 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()); + + DART_SUPPRESS_DEPRECATED_BEGIN return eval(temp); + DART_SUPPRESS_DEPRECATED_END } //============================================================================== @@ -101,9 +105,13 @@ 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()); + + DART_SUPPRESS_DEPRECATED_BEGIN evalGradient(temp, _grad); + DART_SUPPRESS_DEPRECATED_END } //============================================================================== diff --git a/dart/renderer/Camera.h b/dart/renderer/Camera.h deleted file mode 100644 index e173c6e9cbd06..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); - 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); - - 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/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 diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 6857d1e5576a1..4deecc9baacca 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(); } } @@ -277,8 +276,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/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; 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 diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp deleted file mode 100644 index 84076111f9c15..0000000000000 --- a/dart/utils/Parser.cpp +++ /dev/null @@ -1,710 +0,0 @@ -/* - * Copyright (c) 2013-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/utils/Parser.h" - -#include -#include -#include -#include - -#include "dart/common/Console.h" -#include "dart/math/Geometry.h" -#include "dart/common/LocalResourceRetriever.h" - -namespace dart { -namespace utils { - -std::string toString(bool _v) -{ - return boost::lexical_cast(_v); -} - -std::string toString(int _v) -{ - return boost::lexical_cast(_v); -} - -std::string toString(unsigned int _v) -{ - return boost::lexical_cast(_v); -} - -std::string toString(float _v) -{ - //if (std::isfinite(_v)) - return boost::lexical_cast(_v); - //else - // return std::string("0"); -} - -std::string toString(double _v) -{ - //if (std::isfinite(_v)) - return boost::lexical_cast(_v); - //else - // return std::string("0"); -} - -std::string toString(char _v) -{ - return boost::lexical_cast(_v); -} - -//============================================================================== -std::string toString(const Eigen::Vector2d& _v) -{ - return boost::lexical_cast(_v.transpose()); -} - -//============================================================================== -std::string toString(const Eigen::Vector3d& _v) -{ - return boost::lexical_cast(_v.transpose()); -} - -//============================================================================== -std::string toString(const Eigen::Vector3i& _v) -{ - return boost::lexical_cast(_v.transpose()); -} - -//============================================================================== -std::string toString(const Eigen::Vector6d& _v) -{ - return boost::lexical_cast(_v.transpose()); -} - -//============================================================================== -std::string toString(const Eigen::VectorXd& _v) -{ - return boost::lexical_cast(_v.transpose()); -} - -std::string toString(const Eigen::Isometry3d& _v) -{ - std::ostringstream ostr; - ostr.precision(6); - - Eigen::Vector3d xyz = math::matrixToEulerXYZ(_v.linear()); - - ostr << _v.translation()(0) << " " - << _v.translation()(1) << " " - << _v.translation()(2) << " "; - ostr << xyz[0] << " " << xyz[1] << " " << xyz[2]; - - return ostr.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; - } -} - -int toInt(const std::string& _str) -{ - return boost::lexical_cast(_str); -} - -unsigned int toUInt(const std::string& _str) -{ - return boost::lexical_cast(_str); -} - -float toFloat(const std::string& _str) -{ - return boost::lexical_cast(_str); -} - -double toDouble(const std::string& _str) -{ - return boost::lexical_cast(_str); -} - -char toChar(const std::string& _str) -{ - return boost::lexical_cast(_str); -} - -Eigen::Vector2d toVector2d(const std::string& _str) -{ - 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); - - for (size_t i = 0; i < pieces.size(); ++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; - } - } - } - - return ret; -} - -Eigen::Vector3d toVector3d(const std::string& _str) -{ - 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); - - for (size_t i = 0; i < pieces.size(); ++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; - } - } - } - - return ret; -} - -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); - assert(pieces.size() == 3); - - for (size_t i = 0; i < pieces.size(); ++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 int for Eigen::Vector3i[" - << i - << "]" - << std::endl; - } - } - } - - return ret; -} - -Eigen::Vector6d toVector6d(const std::string& _str) -{ - 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); - - for (size_t i = 0; i < pieces.size(); ++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; - } - } - } - - return ret; -} - -//============================================================================== -Eigen::VectorXd toVectorXd(const std::string& _str) -{ - 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() > 0); - - Eigen::VectorXd ret(pieces.size()); - - for (size_t i = 0; i < pieces.size(); ++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::VectorXd[" - << i - << "]" - << std::endl; - } - } - } - - return ret; -} - -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); - - for (size_t i = 0; i < pieces.size(); ++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; - } - } - } - - T.linear() = math::eulerXYZToMatrix(elements.tail<3>()); - T.translation() = elements.head<3>(); - return T; -} - -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); - assert(pieces.size() == 6); - - for (size_t i = 0; i < pieces.size(); ++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; - } - } - } - - Eigen::Vector3d reverseEulerAngles( - elements.tail<3>()[2], - elements.tail<3>()[1], - elements.tail<3>()[0]); - - T.linear() = math::eulerZYXToMatrix(reverseEulerAngles); - T.translation() = elements.head<3>(); - return T; -} - -std::string getValueString(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return str; -} - -bool getValueBool(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - 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; - } -} - -int getValueInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toInt(str); -} - -unsigned int getValueUInt(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toUInt(str); -} - -float getValueFloat(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toFloat(str); -} - -double getValueDouble(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toDouble(str); -} - -char getValueChar(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toChar(str); -} - -Eigen::Vector2d getValueVector2d(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVector2d(str); -} - -Eigen::Vector3d getValueVector3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVector3d(str); -} - -Eigen::Vector3i getValueVector3i(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVector3i(str); -} - -Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVector6d(str); -} - -//============================================================================== -Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, - const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVectorXd(str); -} - -Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toVector3d(str); -} - -Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toIsometry3d(str); -} - -Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); - - return toIsometry3dWithExtrinsicRotation(str); -} - -bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name) -{ - assert(_parentElement != nullptr); - assert(!_name.empty()); - - return _parentElement->FirstChildElement(_name.c_str()) == nullptr ? false : true; -} - -tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, - const std::string& _name) -{ - assert(!_name.empty()); - - return _parentElement->FirstChildElement(_name.c_str()); -} - -//============================================================================== -void openXMLFile( - tinyxml2::XMLDocument& doc, const common::Uri& uri, - const common::ResourceRetrieverPtr& _retriever) -{ - common::ResourceRetrieverPtr retriever; - if(_retriever) - retriever = _retriever; - else - retriever = std::make_shared(); - - const common::ResourcePtr resource = retriever->retrieve(uri); - if(!resource) - { - dtwarn << "[openXMLFile] Failed opening URI '" - << uri.toString() << "'.\n"; - throw std::runtime_error("Failed opening URI."); - } - - // C++11 guarantees that std::string has contiguous storage. - const size_t size = resource->getSize(); - std::string content; - content.resize(size); - if(resource->read(&content.front(), size, 1) != 1) - { - dtwarn << "[openXMLFile] Failed reading from URI '" - << uri.toString() << "'.\n"; - throw std::runtime_error("Failed reading from URI."); - } - - int const result = doc.Parse(&content.front()); - if(result != tinyxml2::XML_SUCCESS) - { - dtwarn << "[openXMLFile] Failed parsing XML: TinyXML2 returned error" - " code " << result << ".\n"; - throw std::runtime_error("Failed parsing XML."); - } -} - -bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) -{ - const char* const result = element->Attribute(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); -} - -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()); - } -} - -ElementEnumerator::ElementEnumerator(tinyxml2::XMLElement* _parent, - const std::string& _name) - : m_name(_name), - m_parent(_parent), - m_current(nullptr) -{ -} - -ElementEnumerator::~ElementEnumerator() -{ -} - -bool ElementEnumerator::valid() const -{ - return m_current != nullptr; -} - -bool ElementEnumerator::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 = nullptr; - - return valid(); -} - -bool ElementEnumerator::operator==(const ElementEnumerator& _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& ElementEnumerator::operator=(const ElementEnumerator& _rhs) -{ - this->m_name = _rhs.m_name; - this->m_parent = _rhs.m_parent; - this->m_current = _rhs.m_current; - return *this; -} - -} // namespace utils -} // namespace dart diff --git a/dart/utils/Parser.h b/dart/utils/Parser.h deleted file mode 100644 index 34a1d55b96b07..0000000000000 --- a/dart/utils/Parser.h +++ /dev/null @@ -1,151 +0,0 @@ -/* - * Copyright (c) 2013-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_UTILS_PARSER_H -#define DART_UTILS_PARSER_H - -#include -#include -// TinyXML-2 Library -// http://www.grinninglizard.com/tinyxml2/index.html -#include - -#include "dart/common/ResourceRetriever.h" -#include "dart/math/MathTypes.h" - -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); -// 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 (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); - -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); -std::string getAttribute(tinyxml2::XMLElement* element, const char* const name); -void getAttribute(tinyxml2::XMLElement* element, const char* const name, double* d); - -//------------------------------------------------------------------------------ -// -//------------------------------------------------------------------------------ -/// \brief -class ElementEnumerator -{ -public: - /// \brief - ElementEnumerator(tinyxml2::XMLElement* _parent, const std::string& _name); - - /// \brief - virtual ~ElementEnumerator(); - - /// \brief - bool valid() const; - - /// \brief - bool next(); - - /// \brief - tinyxml2::XMLElement* get() const { return m_current; } - - /// \brief - tinyxml2::XMLElement* operator->() const { return m_current; } - - /// \brief - tinyxml2::XMLElement& operator*() const { return *m_current; } - - /// \brief - bool operator==(const ElementEnumerator& _rhs) const; - - /// \brief - ElementEnumerator & operator=(const ElementEnumerator& _rhs); - -private: - /// \brief - std::string m_name; - - /// \brief - tinyxml2::XMLElement* m_parent; - - /// \brief - tinyxml2::XMLElement* m_current; -}; - -} // namespace utils -} // namespace dart - -#endif // #ifndef DART_UTILS_PARSER_H 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/SkelParser.cpp b/dart/utils/SkelParser.cpp index dc6a6b1a7eb53..19881df178d99 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/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/VskParser.cpp b/dart/utils/VskParser.cpp new file mode 100644 index 0000000000000..a9999751f18e4 --- /dev/null +++ b/dart/utils/VskParser.cpp @@ -0,0 +1,922 @@ +/* + * 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 + +// 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 VskParser { + +namespace { + +using BodyPropPtr = std::shared_ptr; +using JointPropPtr = std::shared_ptr; + +using ParameterMap = std::map; +using BodyNodeColorMap = std::map; + +struct VskData +{ + ParameterMap parameterMap; + BodyNodeColorMap bodyNodeColorMap; + VskParser::Options options; +}; + +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 VskData& vskData); + +bool readJointFree(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData); + +bool readJointBall(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData); + +bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData); + +bool readJointHinge(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData); + +bool readJointDummy(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + 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); + +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 + +//============================================================================== +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 readSkeleton( + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr, + const Options options) +{ + 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; + vskData.options = options; + + // 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) +{ + 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 = 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); + + // Joint + const tinyxml2::XMLElement* jointEle = nullptr; + JointPropPtr jointProperties; + std::string foundJointType; + + for (const auto& jointType : vskJointTypes) + { + jointEle = segment->FirstChildElement(jointType.c_str()); + if (jointEle) + { + 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; + } + } + + jointProperties->mName = "Joint-" + bodyNodeProperties.mName; + + // Create joint and body node + dynamics::BodyNode* bodyNode = nullptr; + + auto pair = createJointAndBodyNodePair( + foundJointType, skel, parentBodyNode, jointProperties.get(), + bodyNodeProperties); + bodyNode = pair.second; + assert(bodyNode != nullptr); + + vskData.bodyNodeColorMap[bodyNode] = rgb; + + // Read the subtree segments + 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 VskData& vskData) +{ + if (jointType == "JointFree") + { + return readJointFree(jointEle, jointProperties, tfFromParent, + vskData); + } + else if (jointType == "JointBall") + { + return readJointBall(jointEle, jointProperties, tfFromParent, + vskData); + } + else if (jointType == "JointHardySpicer") + { + return readJointHardySpicer(jointEle, jointProperties, tfFromParent, + vskData); + } + else if (jointType == "JointHinge") + { + return readJointHinge(jointEle, jointProperties, tfFromParent, + vskData); + } + else if (jointType == "JointDummy") + { + return readJointDummy(jointEle, jointProperties, tfFromParent, + vskData); + } + 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 VskData& /*vskData*/) +{ + 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 VskData& vskData) +{ + dynamics::BallJoint::Properties properties; + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + 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( + properties); + + return true; +} + +//============================================================================== +bool readJointHardySpicer(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData) +{ + 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", vskData.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; + 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( + properties); + + return true; +} + +//============================================================================== +bool readJointHinge(const tinyxml2::XMLElement* jointEle, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& vskData) +{ + dynamics::RevoluteJoint::Properties properties; + + // Attribute: AXIS + Eigen::Vector3d axis = Eigen::Vector3d::UnitX(); + if (hasAttribute(jointEle, "AXIS")) + axis = readAttributeVector<3>(jointEle, "AXIS", vskData.parameterMap); + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + properties.mAxis = axis; + 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( + properties); + + return true; +} + +//============================================================================== +bool readJointDummy(const tinyxml2::XMLElement* /*jointEle*/, + JointPropPtr& jointProperties, + const Eigen::Isometry3d& tfFromParent, + const VskData& /*vskData*/) +{ + dynamics::WeldJoint::Properties properties; + + properties.mT_ParentBodyToJoint = tfFromParent; + properties.mT_ChildBodyToJoint = Eigen::Isometry3d::Identity(); + + jointProperties + = Eigen::make_aligned_shared( + properties); + + 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, + 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::Vector4d rgba = Eigen::Vector4d::Ones(); + rgba.head<3>() = readColorAttribute(markerEle, vskData.parameterMap, + Eigen::Vector3d::Constant(0.5)); + + // 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, rgba, + 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 = readColorAttribute(segment, vskData.parameterMap, + // Eigen::Vector3d(0.5, 0.5, 0.5)); + + 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] = vskData.options.thicknessRatio * 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); + } + + // 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 + 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 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); + + // Now all the bodies should have at least one shape + assert(bodyNode->getNumVisualizationShapes() > 0); + + double totalMass = 0.0; + Eigen::Matrix3d totalMoi = Eigen::Matrix3d::Zero(); + const auto& shapes = bodyNode->getVisualizationShapes(); + + for (const dynamics::ShapePtr& shape : shapes) + { + 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); + } +} + +//============================================================================== +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 VskParser + +} // namespace utils +} // namespace dart + diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h new file mode 100644 index 0000000000000..ec2bbe5233c4b --- /dev/null +++ b/dart/utils/VskParser.h @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2011-2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Sehoon Ha + * 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_UTILS_VSKPARSER_H_ +#define DART_UTILS_VSKPARSER_H_ + +#include "dart/common/ResourceRetriever.h" +#include "dart/common/Uri.h" +#include "dart/dynamics/Skeleton.h" + +namespace dart { +namespace utils { + +namespace VskParser +{ + /// 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 + dynamics::SkeletonPtr readSkeleton( + const common::Uri& fileUri, + const common::ResourceRetrieverPtr& retrieverOrNullptr = nullptr, + const Options options = Options()); + +} // namespace VskParser + +} // namespace utils +} // namespace dart + +#endif // #ifndef DART_UTILS_VSKPARSER_H_ + diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp new file mode 100644 index 0000000000000..1182e28e34b1a --- /dev/null +++ b/dart/utils/XmlHelpers.cpp @@ -0,0 +1,866 @@ +/* + * Copyright (c) 2013-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/utils/XmlHelpers.h" + +#include +#include +#include +#include + +#include "dart/common/Console.h" +#include "dart/math/Geometry.h" +#include "dart/common/LocalResourceRetriever.h" + +namespace dart { +namespace utils { + +//============================================================================== +std::string toString(bool v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(int v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(unsigned int v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(float v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(double v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(char v) +{ + return boost::lexical_cast(v); +} + +//============================================================================== +std::string toString(const Eigen::Vector2d& v) +{ + return boost::lexical_cast(v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::Vector3d& v) +{ + return boost::lexical_cast(v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::Vector3i& v) +{ + return boost::lexical_cast(v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::Vector6d& v) +{ + return boost::lexical_cast(v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::VectorXd& v) +{ + return boost::lexical_cast(v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::Isometry3d& v) +{ + std::ostringstream ostr; + ostr.precision(6); + + Eigen::Vector3d xyz = math::matrixToEulerXYZ(v.linear()); + + ostr << v.translation()(0) << " " + << v.translation()(1) << " " + << v.translation()(2) << " "; + ostr << xyz[0] << " " << xyz[1] << " " << xyz[2]; + + return ostr.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; + } +} + +//============================================================================== +int toInt(const std::string& str) +{ + return boost::lexical_cast(str); +} + +//============================================================================== +unsigned int toUInt(const std::string& str) +{ + return boost::lexical_cast(str); +} + +//============================================================================== +float toFloat(const std::string& str) +{ + return boost::lexical_cast(str); +} + +//============================================================================== +double toDouble(const std::string& str) +{ + return boost::lexical_cast(str); +} +//============================================================================== +char toChar(const std::string& str) +{ + return boost::lexical_cast(str); +} + +//============================================================================== +Eigen::Vector2d toVector2d(const std::string& str) +{ + 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); + + for (size_t i = 0; i < pieces.size(); ++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; + } + } + } + + return ret; +} + +//============================================================================== +Eigen::Vector3d toVector3d(const std::string& str) +{ + 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); + + for (size_t i = 0; i < pieces.size(); ++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; + } + } + } + + return ret; +} + +//============================================================================== +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); + assert(pieces.size() == 3); + + for (size_t i = 0; i < pieces.size(); ++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 int for Eigen::Vector3i[" + << i + << "]" + << std::endl; + } + } + } + + return ret; +} + +//============================================================================== +Eigen::Vector6d toVector6d(const std::string& str) +{ + 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); + + for (size_t i = 0; i < pieces.size(); ++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; + } + } + } + + return ret; +} + +//============================================================================== +Eigen::VectorXd toVectorXd(const std::string& str) +{ + 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() > 0); + + Eigen::VectorXd ret(pieces.size()); + + for (size_t i = 0; i < pieces.size(); ++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::VectorXd[" + << i + << "]" + << std::endl; + } + } + } + + return ret; +} + +//============================================================================== +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); + + for (size_t i = 0; i < pieces.size(); ++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; + } + } + } + + T.linear() = math::eulerXYZToMatrix(elements.tail<3>()); + T.translation() = elements.head<3>(); + return T; +} + +//============================================================================== +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); + assert(pieces.size() == 6); + + for (size_t i = 0; i < pieces.size(); ++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; + } + } + } + + Eigen::Vector3d reverseEulerAngles( + elements.tail<3>()[2], + elements.tail<3>()[1], + elements.tail<3>()[0]); + + T.linear() = math::eulerZYXToMatrix(reverseEulerAngles); + T.translation() = elements.head<3>(); + return T; +} + +//============================================================================== +std::string getValueString(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return str; +} + +//============================================================================== +bool getValueBool(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + 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; + } +} + +//============================================================================== +int getValueInt(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toInt(str); +} + +//============================================================================== +unsigned int getValueUInt(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toUInt(str); +} + +//============================================================================== +float getValueFloat(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toFloat(str); +} + +//============================================================================== +double getValueDouble(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toDouble(str); +} + +//============================================================================== +char getValueChar(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toChar(str); +} + +//============================================================================== +Eigen::Vector2d getValueVector2d(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVector2d(str); +} + +//============================================================================== +Eigen::Vector3d getValueVector3d(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVector3d(str); +} + +//============================================================================== +Eigen::Vector3i getValueVector3i(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVector3i(str); +} + +//============================================================================== +Eigen::Vector6d getValueVector6d(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVector6d(str); +} + +//============================================================================== +Eigen::VectorXd getValueVectorXd(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVectorXd(str); +} + +//============================================================================== +Eigen::Vector3d getValueVec3(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toVector3d(str); +} + +//============================================================================== +Eigen::Isometry3d getValueIsometry3d(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toIsometry3d(str); +} + +//============================================================================== +Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation( + const tinyxml2::XMLElement* parentElement, const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + std::string str = parentElement->FirstChildElement(name.c_str())->GetText(); + + return toIsometry3dWithExtrinsicRotation(str); +} + +//============================================================================== +bool hasElement(const tinyxml2::XMLElement* parentElement, + const std::string& name) +{ + assert(parentElement != nullptr); + assert(!name.empty()); + + return parentElement->FirstChildElement(name.c_str()) + == 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) +{ + assert(!name.empty()); + + return parentElement->FirstChildElement(name.c_str()); +} + +//============================================================================== +void openXMLFile(tinyxml2::XMLDocument& doc, + const common::Uri& uri, + const common::ResourceRetrieverPtr& retrieverOrNullPtr) +{ + common::ResourceRetrieverPtr retriever; + if(retrieverOrNullPtr) + retriever = retrieverOrNullPtr; + else + retriever = std::make_shared(); + + const common::ResourcePtr resource = retriever->retrieve(uri); + if(!resource) + { + dtwarn << "[openXMLFile] Failed opening URI '" + << uri.toString() << "'.\n"; + throw std::runtime_error("Failed opening URI."); + } + + // C++11 guarantees that std::string has contiguous storage. + const size_t size = resource->getSize(); + std::string content; + content.resize(size); + if(resource->read(&content.front(), size, 1) != 1) + { + dtwarn << "[openXMLFile] Failed reading from URI '" + << uri.toString() << "'.\n"; + throw std::runtime_error("Failed reading from URI."); + } + + int const result = doc.Parse(&content.front()); + if(result != tinyxml2::XML_SUCCESS) + { + dtwarn << "[openXMLFile] Failed parsing XML: TinyXML2 returned error" + " code " << result << ".\n"; + throw std::runtime_error("Failed parsing XML."); + } +} + +//============================================================================== +bool hasAttribute(const tinyxml2::XMLElement* element, const char* const name) +{ + const char* const result = element->Attribute(name); + return result != 0; +} + +//============================================================================== +std::string getAttribute(tinyxml2::XMLElement * element, + const char* const name) +{ + return getAttributeString(element, name); +} + +//============================================================================== +void getAttribute(tinyxml2::XMLElement* element, const char* const name, + double* d) +{ + *d = getAttributeDouble(element, name); +} + +//============================================================================== +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); +} + +//============================================================================== +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; +} + +//============================================================================== +int getAttributeInt(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + 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; +} + +//============================================================================== +unsigned int getAttributeUInt(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + unsigned int val = 0u; + const int result = element->QueryUnsignedAttribute(attributeName.c_str(), + &val); + + 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); +} + +//============================================================================== +Eigen::Vector2d getAttributeVector2d(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + const std::string val = getAttributeString(element, attributeName); + + return toVector2d(val); +} + +//============================================================================== +Eigen::Vector3d getAttributeVector3d(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + const std::string val = getAttributeString(element, attributeName); + + return toVector3d(val); +} + +//============================================================================== +Eigen::Vector6d getAttributeVector6d(const tinyxml2::XMLElement* element, + const std::string& attributeName) +{ + 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 +} // namespace dart diff --git a/dart/utils/XmlHelpers.h b/dart/utils/XmlHelpers.h new file mode 100644 index 0000000000000..9213b59ed80e1 --- /dev/null +++ b/dart/utils/XmlHelpers.h @@ -0,0 +1,240 @@ +/* + * Copyright (c) 2013-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_UTILS_XMLHELPERS_H_ +#define DART_UTILS_XMLHELPERS_H_ + +#include +#include +// TinyXML-2 Library +// http://www.grinninglizard.com/tinyxml2/index.html +#include + +#include "dart/common/ResourceRetriever.h" +#include "dart/math/MathTypes.h" + +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); +// 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 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(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) +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); + +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 +{ +protected: + + using ElementPtr = ElementType*; + using ElementRef = ElementType&; + +public: + + /// 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; + } + +private: + + /// Returns true if the current element is valid (not a nullptr) + bool valid() const + { + return mCurrentElement != nullptr; + } + +private: + + /// Parent element + ElementPtr mParentElement; + + /// Child element name + std::string mChildElementName; + + /// Currently visiting child element + ElementPtr mCurrentElement; + +}; + +// ElementEnumerator is for iterating elements for +using ElementEnumerator + = TemplatedElementEnumerator; +using ConstElementEnumerator + = TemplatedElementEnumerator; + +} // namespace utils +} // namespace dart + +#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 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" 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 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/debian/changelog b/debian/changelog index d5fa5af1de538..7a50f63eaffbe 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,20 @@ +dart (6.0.0) unstable; urgency=medium + + * Added missing 'liburdfdom-dev' dependency in Ubuntu package + +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, 06 Nov 2015 00:00:00 -0500 + dart (5.1.0) unstable; urgency=medium * Fixed incorrect rotational motion of BallJoint and FreeJoint @@ -116,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 @@ -196,4 +213,3 @@ dart (3.0.0) unstable; urgency=low * Add "common" namespace -- Tobias Kunz Fri, 11 Oct 2013 22:00:00 -0400 - diff --git a/debian/control b/debian/control index 6c8efec230e7c..1ae442ca3a304 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, @@ -14,9 +13,11 @@ 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) + libboost-regex-dev (>= 1.54.0-4ubuntu3), + libopenthreads-dev, + libopenscenegraph-dev Standards-Version: 3.9.6 Section: libs Homepage: http://dartsim.github.io/ @@ -26,15 +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), - libassimp-dev (<< 3.0~dfsg-4), 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 @@ -62,14 +62,17 @@ 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, + 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 @@ -98,7 +101,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 @@ -128,7 +131,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 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/* diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 5f545909437af..118c0882ef169 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 @@ -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/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 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; 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 } 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/dart/renderer/Light.h b/unittests/testConcurrency.cpp similarity index 50% rename from dart/renderer/Light.h rename to unittests/testConcurrency.cpp index 60ad936356b4f..4cb0ac38e555d 100644 --- a/dart/renderer/Light.h +++ b/unittests/testConcurrency.cpp @@ -1,8 +1,8 @@ /* - * Copyright (c) 2011-2015, Georgia Tech Research Corporation + * Copyright (c) 2015, Georgia Tech Research Corporation * All rights reserved. * - * Author(s): Jie (Jay) Tan + * Author(s): Michael X. Grey * * Georgia Tech Graphics Lab and Humanoid Robotics Lab * @@ -34,44 +34,57 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_RENDERER_LIGHT_H -#define DART_RENDERER_LIGHT_H +#include +#include -#include +#include -namespace dart { -namespace renderer { +#include "dart/simulation/World.h" -enum LightType { - LT_PointLight, - LT_DirectionLight, -}; +#include "TestHelpers.h" -class Light { -public: - /* construction function */ - Light(); - Light(LightType type); - Light(const Eigen::Vector3d& _pos, const Eigen::Vector3d& _diffuse, const Eigen::Vector3d& _specular, LightType _type); +using namespace dart; +using namespace dynamics; - 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; -}; +//============================================================================== +void createAndDestroyFrames(int threadNum) +{ + for(size_t i=0; i < 100; ++i) + { + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); -} // namespace renderer -} // namespace dart + SimpleFrame someFrame(Frame::World(), + "Frame_"+std::to_string(threadNum)+std::to_string(i)); -#endif // #ifndef DART_RENDERER_LIGHT_H + EXPECT_EQ(Frame::World()->getNumChildEntities(), 0); + EXPECT_EQ(Frame::World()->getNumChildFrames(), 0); + } +} + +//============================================================================== +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, + &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); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 004f7c9d3e2ba..ef3c807cd5473 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) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index f0b4c6181ba29..774acb745c0a4 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -216,6 +216,36 @@ 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(0)); + + 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 ); + + EXPECT_TRUE( test_mgr.getObject("0") == int_another0 ); + EXPECT_TRUE( test_mgr.getObject("1") == int1 ); + EXPECT_TRUE( test_mgr.getObject("2") == int2 ); +} + //============================================================================== TEST(NameManagement, WorldSkeletons) { 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; diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index 5edbc4372840f..76c3b3e7535db 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -773,6 +773,23 @@ TEST(Skeleton, CloneNodeOrdering) } } +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(); @@ -987,6 +1004,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 @@ -999,6 +1029,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"); @@ -1007,6 +1038,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(); @@ -1022,6 +1054,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(); @@ -1041,23 +1074,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")); @@ -1066,6 +1104,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; @@ -1078,12 +1117,81 @@ 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); +} + +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]); } TEST(Skeleton, Configurations) diff --git a/unittests/testVskParser.cpp b/unittests/testVskParser.cpp new file mode 100644 index 0000000000000..a82c3c6c5b54c --- /dev/null +++ b/unittests/testVskParser.cpp @@ -0,0 +1,115 @@ +/* + * 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()); + EXPECT_TRUE(world != nullptr); + + SkeletonPtr skeleton + = VskParser::readSkeleton(DART_DATA_PATH"vsk/test/empty.vsk"); + EXPECT_TRUE(skeleton == nullptr); + + world->addSkeleton(skeleton); + 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); + 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); + 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(); +}