diff --git a/.gitignore b/.gitignore index 036331c1b224e..6a193d4d631e9 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ TAGS .DS_Store .cproject .project +.vscode *.orig *.idb *.pdb diff --git a/.travis.yml b/.travis.yml index 78a3a127880e7..df03f644b75a2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,9 +22,11 @@ matrix: compiler: clang env: BUILD_TYPE=Release CODECOV=OFF COMPILER=CLANG - os: osx + osx_image: xcode9.3beta compiler: clang env: BUILD_TYPE=Debug CODECOV=OFF COMPILER=CLANG - os: osx + osx_image: xcode9.3beta compiler: clang env: BUILD_TYPE=Release CODECOV=OFF COMPILER=CLANG @@ -35,7 +37,7 @@ script: - 'ci/script.sh' after_success: - - bash <(curl -s https://codecov.io/bash) || echo "Codecov did not collect coverage reports" + - if [ $CODECOV = ON ]; then bash <(curl -s https://codecov.io/bash) || echo "Codecov did not collect coverage reports"; fi after_failure: - cat build/Testing/Temporary/LastTest.log diff --git a/Brewfile b/Brewfile deleted file mode 100644 index 768359ee5327e..0000000000000 --- a/Brewfile +++ /dev/null @@ -1,16 +0,0 @@ -brew 'git' -brew 'cmake' -brew 'assimp' -brew 'fcl' -brew 'bullet', args: ['--with-double-precision'] -brew 'ode', args: ['--with-libccd', '--with-double-precision'] -brew 'flann' -brew 'boost' -brew 'eigen' -brew 'tinyxml' -brew 'tinyxml2' -brew 'libccd' -brew 'nlopt' -#brew 'ipopt' -brew 'urdfdom' -brew 'open-scene-graph' diff --git a/CHANGELOG.md b/CHANGELOG.md index 633f3db7cc7a7..3e0b1d1fa3b51 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ * Kinematics/Dynamics + * Added vectorized joint limit functions: [#996](https://github.com/dartsim/dart/pull/996) * Added lazy evaluation for shape's volume and bounding-box computation: [#959](https://github.com/dartsim/dart/pull/959) * Added IkFast support as analytic IK solver: [#887](https://github.com/dartsim/dart/pull/887) * Fixed NaN values caused by zero-length normals in ContactConstraint: [#881](https://github.com/dartsim/dart/pull/881) @@ -36,6 +37,7 @@ * GUI * Added support of rendering texture images: [#973](https://github.com/dartsim/dart/pull/973) + * Added OSG shadows: [#978](https://github.com/dartsim/dart/pull/978) * License @@ -47,6 +49,7 @@ * Suppressed -Winjected-class-name warnings from Clang 5.0.0: [#964](https://github.com/dartsim/dart/pull/964) * Suppressed -Wdangling-else warnings from GCC 7.2.0: [#937](https://github.com/dartsim/dart/pull/937) * Fixed various build issues with Visual Studio: [#956](https://github.com/dartsim/dart/pull/956) + * Removed TinyXML dependency: [#993](https://github.com/dartsim/dart/pull/993) ### [DART 6.3.0 (2017-10-04)](https://github.com/dartsim/dart/milestone/36?closed=1) diff --git a/ci/appveyor_install.ps1 b/ci/appveyor_install.ps1 deleted file mode 100755 index 3a10c7517cd79..0000000000000 --- a/ci/appveyor_install.ps1 +++ /dev/null @@ -1,35 +0,0 @@ -function ExecuteCommand($command, $command_args) -{ - Write-Host $command $command_args - Start-Process -FilePath $command -ArgumentList $command_args -Wait -Passthru -} - -function InstallPrerequisites($work_dir, $install_dir) -{ - $old_dir = $pwd - cd $work_dir - - $msi = "dart-dependencies.msi" - $uri = "https://github.com/dartsim/dart-prerequisites-windows-installers/raw/master/02/DART-dependencies-msvc14-md-32bit.msi" - Invoke-WebRequest $uri -OutFile $msi - - $install_command = "msiexec.exe" - $install_args = "/qn /log log.txt /i $msi TARGETDIR=$install_dir" - ExecuteCommand $install_command $install_args - - cd $old_dir -} - -function main() -{ - try - { - InstallPrerequisites "C:/projects" "C:/Golems" - } - catch - { - throw - } -} - -main diff --git a/ci/install_linux.sh b/ci/install_linux.sh index 6558bd4320c15..a57e24f023d22 100755 --- a/ci/install_linux.sh +++ b/ci/install_linux.sh @@ -17,7 +17,6 @@ libbullet-dev libflann-dev libnlopt-dev coinor-libipopt-dev -libtinyxml-dev libtinyxml2-dev liburdfdom-dev liburdfdom-headers-dev diff --git a/ci/install_osx.sh b/ci/install_osx.sh index e03eef0a3b7bc..598dfd2370838 100755 --- a/ci/install_osx.sh +++ b/ci/install_osx.sh @@ -1,2 +1,20 @@ +brew tap dartsim/dart # for ipopt + brew update > /dev/null -brew bundle + +brew install git +brew install cmake +brew install assimp +brew install fcl +brew install bullet --with-double-precision +brew install ode --with-libccd --with-double-precision +brew install flann +brew install boost +brew install eigen +brew install tinyxml +brew install tinyxml2 +brew install libccd +brew install nlopt +brew install ipopt +brew install urdfdom +brew install open-scene-graph diff --git a/cmake/FindTINYXML.cmake b/cmake/FindTINYXML.cmake deleted file mode 100644 index de22293be6ab8..0000000000000 --- a/cmake/FindTINYXML.cmake +++ /dev/null @@ -1,46 +0,0 @@ -# Copyright (c) 2011-2017, The DART development contributors -# All rights reserved. -# -# The list of contributors can be found at: -# https://github.com/dartsim/dart/blob/master/LICENSE -# -# This file is provided under the "BSD-style" License - -# Find TINYXML -# -# This sets the following variables: -# TINYXML_FOUND -# TINYXML_INCLUDE_DIRS -# TINYXML_LIBRARIES -# TINYXML_VERSION - -find_package(PkgConfig QUIET) - -# Check to see if pkgconfig is installed. -pkg_check_modules(PC_TINYXML tinyxml QUIET) - -# Include directories -find_path(TINYXML_INCLUDE_DIRS - NAMES tinyxml.h - HINTS ${PC_TINYXML_INCLUDEDIR} - PATHS "${CMAKE_INSTALL_PREFIX}/include") - -# Libraries -if(MSVC) - set(TINYXML_LIBRARIES optimized tinyxml debug tinyxmld) -else() - find_library(TINYXML_LIBRARIES - NAMES tinyxml - HINTS ${PC_TINYXML_LIBDIR}) -endif() - -# Version -set(TINYXML_VERSION ${PC_TINYXML_VERSION}) - -# Set (NAME)_FOUND if all the variables and the version are satisfied. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(TINYXML - FAIL_MESSAGE DEFAULT_MSG - REQUIRED_VARS TINYXML_INCLUDE_DIRS TINYXML_LIBRARIES - VERSION_VAR TINYXML_VERSION) - diff --git a/dart/CMakeLists.txt b/dart/CMakeLists.txt index 9dd9bb08fbfec..1cb53c4d73ffe 100644 --- a/dart/CMakeLists.txt +++ b/dart/CMakeLists.txt @@ -43,7 +43,7 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib") # dart-optimizer-ipopt - {dart}, optimizer/ipopt, [ipopt] # dart-optimizer-nlopt - {dart}, optimizer/nlopt, [nlopt] # dart-collision-bullet - {dart}, collision/bullet, [bullet] -# dart-io - {dart}, io, io/sdf, [tinyxml, tinyxml2] +# dart-io - {dart}, io, io/sdf, [tinyxml2] # dart-io-urdf - {dart-io}, io/urdf, [urdfdom] # dart-gui - {dart}, gui, [opengl, glut] # dart-gui-osg - {dart-gui}, gui/osg, gui/osg/render, [openscenegraph] @@ -81,7 +81,7 @@ add_subdirectory(dynamics) add_subdirectory(collision) add_subdirectory(constraint) add_subdirectory(simulation) -add_subdirectory(io) # tinyxml, tinyxml2, bullet +add_subdirectory(io) # tinyxml2, bullet add_subdirectory(gui) # opengl, glut, bullet set(DART_CONFIG_HPP_IN ${CMAKE_SOURCE_DIR}/dart/config.hpp.in) diff --git a/dart/dynamics/GenericJoint.hpp b/dart/dynamics/GenericJoint.hpp index db273491b0e21..2f2ee67dfa488 100644 --- a/dart/dynamics/GenericJoint.hpp +++ b/dart/dynamics/GenericJoint.hpp @@ -175,12 +175,24 @@ class GenericJoint : // Documentation inherited double getPositionLowerLimit(std::size_t index) const override; + // Documentation inherited + void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getPositionLowerLimits() const override; + // Documentation inherited void setPositionUpperLimit(std::size_t index, double position) override; // Documentation inherited double getPositionUpperLimit(std::size_t index) const override; + // Documentation inherited + void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getPositionUpperLimits() const override; + // Documentation inherited bool hasPositionLimit(std::size_t index) const override; @@ -256,12 +268,24 @@ class GenericJoint : // Documentation inherited double getVelocityLowerLimit(std::size_t index) const override; + // Documentation inherited + void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getVelocityLowerLimits() const override; + // Documentation inherited void setVelocityUpperLimit(std::size_t index, double velocity) override; // Documentation inherited double getVelocityUpperLimit(std::size_t index) const override; + // Documentation inherited + void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getVelocityUpperLimits() const override; + // Documentation inherited void resetVelocity(std::size_t index) override; @@ -304,12 +328,24 @@ class GenericJoint : // Documentation inherited double getAccelerationLowerLimit(std::size_t index) const override; + // Documentation inherited + void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getAccelerationLowerLimits() const override; + // Documentation inherited void setAccelerationUpperLimit(std::size_t index, double acceleration) override; // Documentation inherited double getAccelerationUpperLimit(std::size_t index) const override; + // Documentation inherited + void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getAccelerationUpperLimits() const override; + // Documentation inherited void resetAccelerations() override; @@ -337,12 +373,24 @@ class GenericJoint : // Documentation inherited double getForceLowerLimit(std::size_t index) const override; + // Documentation inherited + void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getForceLowerLimits() const override; + // Documentation inherited void setForceUpperLimit(size_t index, double force) override; // Documentation inherited double getForceUpperLimit(size_t index) const override; + // Documentation inherited + void setForceUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getForceUpperLimits() const override; + // Documentation inherited void resetForces() override; diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index 0ce1501d07953..0bc0705a39f9b 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -276,12 +276,24 @@ class Joint : public virtual common::Subject, /// Get lower limit for position virtual double getPositionLowerLimit(std::size_t _index) const = 0; + /// Set the position lower limits of all the generalized coordinates. + virtual void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) = 0; + + /// Get the position lower limits of all the generalized coordinates. + virtual Eigen::VectorXd getPositionLowerLimits() const = 0; + /// Set upper limit for position virtual void setPositionUpperLimit(std::size_t _index, double _position) = 0; /// Get upper limit for position virtual double getPositionUpperLimit(std::size_t _index) const = 0; + /// Set the position upper limits of all the generalized coordinates. + virtual void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) = 0; + + /// Get the position upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getPositionUpperLimits() const = 0; + /// Get whether a generalized coordinate is cyclic. Return true if and only /// if this generalized coordinate has an infinite number of positions that /// produce the same relative transform. Note that, for a multi-DOF joint, @@ -337,12 +349,24 @@ class Joint : public virtual common::Subject, /// Get lower limit for velocity virtual double getVelocityLowerLimit(std::size_t _index) const = 0; + /// Set the velocity lower limits of all the generalized coordinates. + virtual void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) = 0; + + /// Get the velocity lower limits of all the generalized coordinates. + virtual Eigen::VectorXd getVelocityLowerLimits() const = 0; + /// Set upper limit for velocity virtual void setVelocityUpperLimit(std::size_t _index, double _velocity) = 0; /// Get upper limit for velocity virtual double getVelocityUpperLimit(std::size_t _index) const = 0; + /// Set the velocity upper limits of all the generalized coordinates. + virtual void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) = 0; + + /// Get the velocity upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getVelocityUpperLimits() const = 0; + /// Set the velocity of a generalized coordinate in this Joint to its initial /// velocity virtual void resetVelocity(std::size_t _index) = 0; @@ -392,12 +416,24 @@ class Joint : public virtual common::Subject, /// Get lower limit for acceleration virtual double getAccelerationLowerLimit(std::size_t _index) const = 0; + /// Set the acceleration upper limits of all the generalized coordinates. + virtual void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) = 0; + + /// Get the acceleration upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getAccelerationLowerLimits() const = 0; + /// Set upper limit for acceleration virtual void setAccelerationUpperLimit(std::size_t _index, double _acceleration) = 0; /// Get upper limit for acceleration virtual double getAccelerationUpperLimit(std::size_t _index) const = 0; + /// Set the acceleration upper limits of all the generalized coordinates. + virtual void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) = 0; + + /// Get the acceleration upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getAccelerationUpperLimits() const = 0; + /// \} //---------------------------------------------------------------------------- @@ -425,12 +461,24 @@ class Joint : public virtual common::Subject, /// Get lower limit for force virtual double getForceLowerLimit(std::size_t _index) const = 0; + /// Set the force upper limits of all the generalized coordinates. + virtual void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) = 0; + + /// Get the force upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getForceLowerLimits() const = 0; + /// Set upper limit for force virtual void setForceUpperLimit(std::size_t _index, double _force) = 0; /// Get upper limit for force virtual double getForceUpperLimit(std::size_t _index) const = 0; + /// Set the force upper limits of all the generalized coordinates. + virtual void setForceUpperLimits(const Eigen::VectorXd& upperLimits) = 0; + + /// Get the force upper limits of all the generalized coordinates. + virtual Eigen::VectorXd getForceUpperLimits() const = 0; + /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index a9d14ada7c15a..c7c355994746d 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -39,9 +39,11 @@ namespace detail { //============================================================================== VisualAspectProperties::VisualAspectProperties(const Eigen::Vector4d& color, - const bool hidden) + const bool hidden, + const bool shadowed) : mRGBA(color), - mHidden(hidden) + mHidden(hidden), + mShadowed(shadowed) { // Do nothing } diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index d81b0ab3d3c26..30f8d64639f9b 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -67,12 +67,15 @@ class VisualAspect final : void setRGBA(const Eigen::Vector4d& color); DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector4d, RGBA ) - // void setRGBA(const Eigen::Vector4d& value); // const Eigen::Vector4d& getRGBA() const; DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Hidden ) - // void setHidden(const Eigen::Vector4d& value); - // const Eigen::Vector4d& getHidden() const; + // void setHidden(const bool& value); + // const bool& getHidden() const; + + DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Shadowed ) + // void setShadowed(const bool& value); + // const bool& getShadowed() const; /// Identical to setRGB(const Eigen::Vector3d&) void setColor(const Eigen::Vector3d& color); diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index 5099e836ca358..a67a0f6bc180e 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -200,6 +200,19 @@ double ZeroDofJoint::getPositionLowerLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setPositionLowerLimits( + const Eigen::VectorXd& /*lowerLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getPositionLowerLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setPositionUpperLimit(std::size_t /*_index*/, double /*_position*/) @@ -213,6 +226,19 @@ double ZeroDofJoint::getPositionUpperLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setPositionUpperLimits( + const Eigen::VectorXd& /*upperLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getPositionUpperLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== bool ZeroDofJoint::hasPositionLimit(std::size_t /*_index*/) const { @@ -292,6 +318,18 @@ double ZeroDofJoint::getVelocityLowerLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setVelocityLowerLimits(const Eigen::VectorXd& /*lowerLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getVelocityLowerLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setVelocityUpperLimit(std::size_t /*_index*/, double /*_velocity*/) @@ -305,6 +343,18 @@ double ZeroDofJoint::getVelocityUpperLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setVelocityUpperLimits(const Eigen::VectorXd& /*upperLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getVelocityUpperLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::resetVelocity(std::size_t /*_index*/) { @@ -384,6 +434,18 @@ double ZeroDofJoint::getAccelerationLowerLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setAccelerationLowerLimits(const Eigen::VectorXd& /*lowerLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getAccelerationLowerLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setAccelerationUpperLimit(std::size_t /*_index*/, double /*_acceleration*/) @@ -397,6 +459,18 @@ double ZeroDofJoint::getAccelerationUpperLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setAccelerationUpperLimits(const Eigen::VectorXd& /*upperLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getAccelerationUpperLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setForce(std::size_t /*_index*/, double /*_force*/) { @@ -439,6 +513,18 @@ double ZeroDofJoint::getForceLowerLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setForceLowerLimits(const Eigen::VectorXd& /*lowerLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getForceLowerLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setForceUpperLimit(std::size_t /*_index*/, double /*_force*/) { @@ -451,6 +537,18 @@ double ZeroDofJoint::getForceUpperLimit(std::size_t /*_index*/) const return 0.0; } +//============================================================================== +void ZeroDofJoint::setForceUpperLimits(const Eigen::VectorXd& /*upperLimits*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getForceUpperLimits() const +{ + return Eigen::VectorXd::Zero(0); +} + //============================================================================== void ZeroDofJoint::setVelocityChange(std::size_t /*_index*/, double /*_velocityChange*/) diff --git a/dart/dynamics/ZeroDofJoint.hpp b/dart/dynamics/ZeroDofJoint.hpp index 28f2328ce3ace..80887442e1c5d 100644 --- a/dart/dynamics/ZeroDofJoint.hpp +++ b/dart/dynamics/ZeroDofJoint.hpp @@ -134,10 +134,22 @@ class ZeroDofJoint : public Joint double getPositionLowerLimit(std::size_t _index) const override; // Documentation inherited - void setPositionUpperLimit(std::size_t _index, double _position) override; + void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override; // Documentation inherited - double getPositionUpperLimit(std::size_t _index) const override; + Eigen::VectorXd getPositionLowerLimits() const override; + + // Documentation inherited + void setPositionUpperLimit(std::size_t index, double position) override; + + // Documentation inherited + double getPositionUpperLimit(std::size_t index) const override; + + // Documentation inherited + void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getPositionUpperLimits() const override; // Documentation inherited bool hasPositionLimit(std::size_t _index) const override; @@ -182,12 +194,24 @@ class ZeroDofJoint : public Joint // Documentation inherited double getVelocityLowerLimit(std::size_t _index) const override; + // Documentation inherited + void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getVelocityLowerLimits() const override; + // Documentation inherited void setVelocityUpperLimit(std::size_t _index, double _velocity) override; // Documentation inherited double getVelocityUpperLimit(std::size_t _index) const override; + // Documentation inherited + void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getVelocityUpperLimits() const override; + // Documentation inherited void resetVelocity(std::size_t _index) override; @@ -231,12 +255,24 @@ class ZeroDofJoint : public Joint // Documentation inherited double getAccelerationLowerLimit(std::size_t _index) const override; + // Documentation inherited + void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getAccelerationLowerLimits() const override; + // Documentation inherited void setAccelerationUpperLimit(std::size_t _index, double _acceleration) override; // Documentation inherited double getAccelerationUpperLimit(std::size_t _index) const override; + // Documentation inherited + void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getAccelerationUpperLimits() const override; + //---------------------------------------------------------------------------- // Force //---------------------------------------------------------------------------- @@ -262,12 +298,24 @@ class ZeroDofJoint : public Joint // Documentation inherited double getForceLowerLimit(std::size_t _index) const override; + // Documentation inherited + void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) override; + + // Documentation inherited + Eigen::VectorXd getForceLowerLimits() const override; + // Documentation inherited void setForceUpperLimit(std::size_t _index, double _force) override; // Documentation inherited double getForceUpperLimit(std::size_t _index) const override; + // Documentation inherited + void setForceUpperLimits(const Eigen::VectorXd& upperLimits) override; + + // Documentation inherited + Eigen::VectorXd getForceUpperLimits() const override; + //---------------------------------------------------------------------------- // Velocity change //---------------------------------------------------------------------------- diff --git a/dart/dynamics/detail/GenericJoint.hpp b/dart/dynamics/detail/GenericJoint.hpp index 924ee55dee408..32e9c13d7d2c8 100644 --- a/dart/dynamics/detail/GenericJoint.hpp +++ b/dart/dynamics/detail/GenericJoint.hpp @@ -542,6 +542,27 @@ double GenericJoint::getPositionLowerLimit(size_t index) const return Base::mAspectProperties.mPositionLowerLimits[index]; } +//============================================================================== +template +void GenericJoint::setPositionLowerLimits( + const Eigen::VectorXd& lowerLimits) +{ + if (static_cast(lowerLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setPositionLowerLimits, lowerLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mPositionLowerLimits, lowerLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getPositionLowerLimits() const +{ + return Base::mAspectProperties.mPositionLowerLimits; +} + //============================================================================== template void GenericJoint::setPositionUpperLimit(size_t index, @@ -569,6 +590,27 @@ double GenericJoint::getPositionUpperLimit(size_t index) const return Base::mAspectProperties.mPositionUpperLimits[index]; } +//============================================================================== +template +void GenericJoint::setPositionUpperLimits( + const Eigen::VectorXd& upperLimits) +{ + if (static_cast(upperLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setPositionUpperLimits, upperLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mPositionUpperLimits, upperLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getPositionUpperLimits() const +{ + return Base::mAspectProperties.mPositionUpperLimits; +} + //============================================================================== template bool GenericJoint::hasPositionLimit(size_t index) const @@ -794,6 +836,27 @@ double GenericJoint::getVelocityLowerLimit(size_t index) const return Base::mAspectProperties.mVelocityLowerLimits[index]; } +//============================================================================== +template +void GenericJoint::setVelocityLowerLimits( + const Eigen::VectorXd& lowerLimits) +{ + if (static_cast(lowerLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setVelocityLowerLimits, lowerLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mVelocityLowerLimits, lowerLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getVelocityLowerLimits() const +{ + return Base::mAspectProperties.mVelocityLowerLimits; +} + //============================================================================== template void GenericJoint::setVelocityUpperLimit(size_t index, @@ -822,6 +885,27 @@ double GenericJoint::getVelocityUpperLimit( return Base::mAspectProperties.mVelocityUpperLimits[index]; } +//============================================================================== +template +void GenericJoint::setVelocityUpperLimits( + const Eigen::VectorXd& upperLimits) +{ + if (static_cast(upperLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setVelocityUpperLimits, upperLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mVelocityUpperLimits, upperLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getVelocityUpperLimits() const +{ + return Base::mAspectProperties.mVelocityUpperLimits; +} + //============================================================================== template void GenericJoint::resetVelocity(size_t index) @@ -978,6 +1062,27 @@ double GenericJoint::getAccelerationLowerLimit( return Base::mAspectProperties.mAccelerationLowerLimits[index]; } +//============================================================================== +template +void GenericJoint::setAccelerationLowerLimits( + const Eigen::VectorXd& lowerLimits) +{ + if (static_cast(lowerLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setAccelerationLowerLimits, lowerLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mAccelerationLowerLimits, lowerLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getAccelerationLowerLimits() const +{ + return Base::mAspectProperties.mAccelerationLowerLimits; +} + //============================================================================== template void GenericJoint::setAccelerationUpperLimit( @@ -1007,6 +1112,27 @@ double GenericJoint::getAccelerationUpperLimit( return Base::mAspectProperties.mAccelerationUpperLimits[index]; } +//============================================================================== +template +void GenericJoint::setAccelerationUpperLimits( + const Eigen::VectorXd& upperLimits) +{ + if (static_cast(upperLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setAccelerationUpperLimits, upperLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mAccelerationUpperLimits, upperLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getAccelerationUpperLimits() const +{ + return Base::mAspectProperties.mAccelerationUpperLimits; +} + //============================================================================== template void GenericJoint::resetAccelerations() @@ -1093,6 +1219,27 @@ double GenericJoint::getForceLowerLimit( return Base::mAspectProperties.mForceLowerLimits[index]; } +//============================================================================== +template +void GenericJoint::setForceLowerLimits( + const Eigen::VectorXd& lowerLimits) +{ + if (static_cast(lowerLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setForceLowerLimits, lowerLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mForceLowerLimits, lowerLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getForceLowerLimits() const +{ + return Base::mAspectProperties.mForceLowerLimits; +} + //============================================================================== template void GenericJoint::setForceUpperLimit(size_t index, double force) @@ -1120,6 +1267,27 @@ double GenericJoint::getForceUpperLimit( return Base::mAspectProperties.mForceUpperLimits[index]; } +//============================================================================== +template +void GenericJoint::setForceUpperLimits( + const Eigen::VectorXd& upperLimits) +{ + if (static_cast(upperLimits.size()) != getNumDofs()) + { + GenericJoint_REPORT_DIM_MISMATCH(setForceUpperLimits, upperLimits); + return; + } + + GenericJoint_SET_IF_DIFFERENT(mForceUpperLimits, upperLimits); +} + +//============================================================================== +template +Eigen::VectorXd GenericJoint::getForceUpperLimits() const +{ + return Base::mAspectProperties.mForceUpperLimits; +} + //============================================================================== template void GenericJoint::resetForces() diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 97caa2abbb5d3..44a1331bebc9a 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -58,10 +58,14 @@ struct VisualAspectProperties /// True if this shape node should be kept from rendering bool mHidden; + /// True if this shape node should be shadowed + bool mShadowed; + /// Constructor VisualAspectProperties( const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0), - const bool hidden = false); + const bool hidden = false, + const bool shadowed = true); /// Destructor virtual ~VisualAspectProperties() = default; diff --git a/dart/gui/osg/CMakeLists.txt b/dart/gui/osg/CMakeLists.txt index 9d943f0d81772..2787bd1eeeaa0 100644 --- a/dart/gui/osg/CMakeLists.txt +++ b/dart/gui/osg/CMakeLists.txt @@ -7,7 +7,7 @@ endif() if(DART_BUILD_GUI_OSG) find_package(OpenSceneGraph 3.0 QUIET - COMPONENTS osg osgViewer osgManipulator osgGA osgDB) + COMPONENTS osg osgViewer osgManipulator osgGA osgDB osgShadow) # It seems that OPENSCENEGRAPH_FOUND will inadvertently get set to true when # OpenThreads is found, even if OpenSceneGraph is not installed. This is quite diff --git a/dart/gui/osg/InteractiveFrame.cpp b/dart/gui/osg/InteractiveFrame.cpp index 69cf6f85fac62..4b67da6ca5984 100644 --- a/dart/gui/osg/InteractiveFrame.cpp +++ b/dart/gui/osg/InteractiveFrame.cpp @@ -116,6 +116,8 @@ dart::dynamics::SimpleFrame* InteractiveTool::addShapeFrame( auto shapeFrame = mSimpleFrames.back().get(); shapeFrame->setShape(shape); shapeFrame->createVisualAspect(); + // Disable shadowing for InteractiveTool + shapeFrame->getVisualAspect(true)->setShadowed(false); return shapeFrame; } @@ -230,6 +232,8 @@ dart::dynamics::SimpleFrame* InteractiveFrame::addShapeFrame( auto shapeFrame = mSimpleFrames.back().get(); shapeFrame->setShape(shape); shapeFrame->createVisualAspect(); + // Disable shadowing for InteractiveFrame + shapeFrame->getVisualAspect(true)->setShadowed(false); return shapeFrame; } diff --git a/dart/gui/osg/Viewer.cpp b/dart/gui/osg/Viewer.cpp index 29b261e55fd26..c816ac8f8a1d5 100644 --- a/dart/gui/osg/Viewer.cpp +++ b/dart/gui/osg/Viewer.cpp @@ -380,6 +380,9 @@ void Viewer::addWorldNode(WorldNode* _newWorldNode, bool _active) _newWorldNode->simulate(mSimulating); _newWorldNode->mViewer = this; _newWorldNode->setupViewer(); + // set again the shadow technique to produce warning for ImGuiViewer + if(_newWorldNode->isShadowed()) + _newWorldNode->setShadowTechnique(_newWorldNode->getShadowTechnique()); } //============================================================================== @@ -468,6 +471,15 @@ const ::osg::Group* Viewer::getLightGroup() const return mLightGroup; } +//============================================================================== +const ::osg::ref_ptr<::osg::LightSource>& Viewer::getLightSource(std::size_t index) const +{ + assert(index < 2); + if(index == 0) + return mLightSource1; + return mLightSource2; +} + //============================================================================== void Viewer::setupDefaultLights() { diff --git a/dart/gui/osg/Viewer.hpp b/dart/gui/osg/Viewer.hpp index 30f31eaf1b085..360e77386107e 100644 --- a/dart/gui/osg/Viewer.hpp +++ b/dart/gui/osg/Viewer.hpp @@ -38,6 +38,7 @@ #include #include +#include #include @@ -114,7 +115,6 @@ class ViewerAttachment : public virtual ::osg::Group class Viewer : public osgViewer::Viewer, public dart::common::Subject { public: - /// Constructor for dart::gui::osg::Viewer. This will automatically create the /// default event handler. Viewer(const ::osg::Vec4& clearColor = ::osg::Vec4(0.9,0.9,0.9,1.0)); @@ -197,6 +197,11 @@ class Viewer : public osgViewer::Viewer, public dart::common::Subject /// Get the Group node that contains the LightSources for this Viewer const ::osg::Group* getLightGroup() const; + /// Get one of the LightSources of this Viewer + /// index either 0 or 1 + /// Useful for shadowing techniques + const ::osg::ref_ptr<::osg::LightSource>& getLightSource(std::size_t index = 0) const; + /// Set up the default lighting scheme void setupDefaultLights(); diff --git a/dart/gui/osg/WorldNode.cpp b/dart/gui/osg/WorldNode.cpp index 7b5508438623b..ee7cc5650dc66 100644 --- a/dart/gui/osg/WorldNode.cpp +++ b/dart/gui/osg/WorldNode.cpp @@ -34,8 +34,12 @@ #include +#include +#include + #include "dart/gui/osg/WorldNode.hpp" #include "dart/gui/osg/ShapeFrameNode.hpp" +#include "dart/gui/osg/ImGuiViewer.hpp" #include "dart/simulation/World.hpp" #include "dart/dynamics/Skeleton.hpp" @@ -61,19 +65,40 @@ class WorldNodeCallback : public ::osg::NodeCallback }; //============================================================================== -WorldNode::WorldNode(std::shared_ptr _world) - : mWorld(_world), +WorldNode::WorldNode(std::shared_ptr world, ::osg::ref_ptr shadowTechnique) + : mWorld(world), mSimulating(false), mNumStepsPerCycle(1), - mViewer(nullptr) + mViewer(nullptr), + mNormalGroup(new ::osg::Group) { + // Flags for shadowing; maybe this needs to be global? + constexpr int ReceivesShadowTraversalMask = 0x2; + constexpr int CastsShadowTraversalMask = 0x1; + + // Setup shadows + // Create a ShadowedScene + ::osg::ref_ptr shadowedScene = new osgShadow::ShadowedScene; + shadowedScene->setReceivesShadowTraversalMask(ReceivesShadowTraversalMask); + shadowedScene->setCastsShadowTraversalMask(CastsShadowTraversalMask); + + // set the shadowed group + mShadowedGroup = shadowedScene.get(); + mShadowedGroup->getOrCreateStateSet(); + + // Add normal and shadowed groups + addChild(mNormalGroup); + addChild(mShadowedGroup); + + setShadowTechnique(shadowTechnique); + setUpdateCallback(new WorldNodeCallback); } //============================================================================== -void WorldNode::setWorld(std::shared_ptr _newWorld) +void WorldNode::setWorld(std::shared_ptr newWorld) { - mWorld = _newWorld; + mWorld = newWorld; } //============================================================================== @@ -138,15 +163,15 @@ bool WorldNode::isSimulating() const } //============================================================================== -void WorldNode::simulate(bool _on) +void WorldNode::simulate(bool on) { - mSimulating = _on; + mSimulating = on; } //============================================================================== -void WorldNode::setNumStepsPerCycle(std::size_t _steps) +void WorldNode::setNumStepsPerCycle(std::size_t steps) { - mNumStepsPerCycle = _steps; + mNumStepsPerCycle = steps; } //============================================================================== @@ -193,7 +218,11 @@ void WorldNode::clearUnusedNodes() { NodeMap::iterator it = mFrameToNode.find(frame); ShapeFrameNode* node = it->second; - removeChild(node); + if(!node->getShapeFrame() || !node->getShapeFrame()->hasVisualAspect() || !node->getShapeFrame()->getVisualAspect(true)->getShadowed()) { + mNormalGroup->removeChild(node); + } + else + mShadowedGroup->removeChild(node); mFrameToNode.erase(it); } } @@ -260,6 +289,16 @@ void WorldNode::refreshShapeFrameNode(dart::dynamics::Frame* frame) if(!node) return; + // update the group that ShapeFrameNode should be + if((!node->getShapeFrame()->hasVisualAspect() || !node->getShapeFrame()->getVisualAspect(true)->getShadowed()) && node->getParent(0) != mNormalGroup) { + mShadowedGroup->removeChild(node); + mNormalGroup->addChild(node); + } + else if(node->getShapeFrame()->hasVisualAspect() && node->getShapeFrame()->getVisualAspect(true)->getShadowed() && node->getParent(0) != mShadowedGroup) { + mNormalGroup->removeChild(node); + mShadowedGroup->addChild(node); + } + node->refresh(true); return; } @@ -276,7 +315,52 @@ void WorldNode::refreshShapeFrameNode(dart::dynamics::Frame* frame) ::osg::ref_ptr node = new ShapeFrameNode(frame->asShapeFrame(), this); it->second = node; - addChild(node); + if(!node->getShapeFrame()->hasVisualAspect() || !node->getShapeFrame()->getVisualAspect(true)->getShadowed()) { + mNormalGroup->addChild(node); + } + else + mShadowedGroup->addChild(node); +} + +//============================================================================== +bool WorldNode::isShadowed() const +{ + return mShadowed; +} + +//============================================================================== +void WorldNode::setShadowTechnique(::osg::ref_ptr shadowTechnique) { + if(!shadowTechnique) { + mShadowed = false; + static_cast(mShadowedGroup.get())->setShadowTechnique(nullptr); + } + else { + ImGuiViewer* viewer = mViewer ? dynamic_cast(mViewer) : nullptr; + if(viewer) + dtwarn << "[WorldNode] You are enabling shadows inside an ImGuiViewer. " + << "The ImGui windows may not render properly.\n"; + mShadowed = true; + static_cast(mShadowedGroup.get())->setShadowTechnique(shadowTechnique.get()); + } +} + +//============================================================================== +::osg::ref_ptr WorldNode::getShadowTechnique() const { + if(!mShadowed) + return nullptr; + return static_cast(mShadowedGroup.get())->getShadowTechnique(); +} + +//============================================================================== +::osg::ref_ptr WorldNode::createDefaultShadowTechnique(const Viewer* viewer) { + ::osg::ref_ptr sm = new osgShadow::ShadowMap; + // increase the resolution of default shadow texture for higher quality + int mapres = std::pow(2, 13); + sm->setTextureSize(::osg::Vec2s(mapres,mapres)); + // we are using Light1 because this is the highest one (on up direction) + sm->setLight(viewer->getLightSource(0)); + + return sm; } } // namespace osg diff --git a/dart/gui/osg/WorldNode.hpp b/dart/gui/osg/WorldNode.hpp index 4a272e777c5d5..ca8f39b3abe36 100644 --- a/dart/gui/osg/WorldNode.hpp +++ b/dart/gui/osg/WorldNode.hpp @@ -34,6 +34,7 @@ #define DART_GUI_OSG_WORLDNODE_HPP_ #include +#include #include #include @@ -67,10 +68,11 @@ class WorldNode : public ::osg::Group friend class Viewer; /// Default constructor - explicit WorldNode(std::shared_ptr _world = nullptr); + /// Shadows are disabled by default + explicit WorldNode(std::shared_ptr world = nullptr, ::osg::ref_ptr shadowTechnique = nullptr); /// Set the World that this WorldNode is associated with - void setWorld(std::shared_ptr _newWorld); + void setWorld(std::shared_ptr newWorld); /// Get the World that this WorldNode is associated with std::shared_ptr getWorld() const; @@ -119,16 +121,31 @@ class WorldNode : public ::osg::Group /// Pass in true to take steps between render cycles; pass in false to turn /// off steps between render cycles. - void simulate(bool _on); + void simulate(bool on); /// Set the number of steps to take between each render cycle (only if the /// simulation is not paused) - void setNumStepsPerCycle(std::size_t _steps); + void setNumStepsPerCycle(std::size_t steps); /// Get the number of steps that will be taken between each render cycle (only /// if the simulation is not paused) std::size_t getNumStepsPerCycle() const; + /// Get whether the WorldNode is casting shadows + bool isShadowed() const; + + /// Set the ShadowTechnique + /// If you wish to disable shadows, pass a nullptr + void setShadowTechnique(::osg::ref_ptr shadowTechnique = nullptr); + + /// Get the current ShadowTechnique + /// nullptr is there are no shadows + ::osg::ref_ptr getShadowTechnique() const; + + /// Helper function to create a default ShadowTechnique given a Viewer + /// the default ShadowTechnique is ShadowMap + static ::osg::ref_ptr createDefaultShadowTechnique(const Viewer* viewer); + protected: /// Destructor @@ -173,6 +190,15 @@ class WorldNode : public ::osg::Group /// Viewer that this WorldNode is inside of Viewer* mViewer; + /// OSG group for non-shadowed objects + ::osg::ref_ptr<::osg::Group> mNormalGroup; + + /// OSG group for shadowed objects + ::osg::ref_ptr<::osg::Group> mShadowedGroup; + + /// Whether the shadows are enabled + bool mShadowed; + }; } // namespace osg diff --git a/dart/io/CMakeLists.txt b/dart/io/CMakeLists.txt index 63e0de1caf135..c2f3621d14815 100644 --- a/dart/io/CMakeLists.txt +++ b/dart/io/CMakeLists.txt @@ -1,7 +1,4 @@ # Dependency checks -find_package(TINYXML 2.6.2 QUIET) -dart_check_optional_package(TINYXML "dart-io" "tinyxml" "2.6.2") - find_package(TINYXML2 QUIET) dart_check_optional_package(TINYXML2 "dart-io" "tinyxml2" "1.0.1") @@ -32,13 +29,11 @@ dart_add_library(${target_name} ${hdrs} ${srcs} ${dart_io_headers} ${dart_io_sou target_include_directories( ${target_name} SYSTEM PUBLIC - ${TINYXML_INCLUDE_DIRS} ${TINYXML2_INCLUDE_DIRS} ) target_link_libraries( ${target_name} dart - ${TINYXML_LIBRARIES} ${TINYXML2_LIBRARIES} ) diff --git a/dart/io/XmlHelpers.hpp b/dart/io/XmlHelpers.hpp index 408b8148971da..7958fd16a3272 100644 --- a/dart/io/XmlHelpers.hpp +++ b/dart/io/XmlHelpers.hpp @@ -35,8 +35,6 @@ #include #include -// TinyXML-2 Library -// http://www.grinninglizard.com/tinyxml2/index.html #include #include "dart/common/Deprecated.hpp" diff --git a/dart/io/urdf/urdf_world_parser.cpp b/dart/io/urdf/urdf_world_parser.cpp index c25021a9a0cd6..37b65a7930960 100644 --- a/dart/io/urdf/urdf_world_parser.cpp +++ b/dart/io/urdf/urdf_world_parser.cpp @@ -41,8 +41,7 @@ #include #include -#include - +#include #include #include #include @@ -56,6 +55,46 @@ namespace dart { namespace io { namespace urdf_parsing { +namespace { + +bool parsePose(urdf::Pose &pose, tinyxml2::XMLElement* xml) +{ + pose.clear(); + if (xml) + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str) + { + try + { + pose.position.init(xyz_str); + } + catch (urdf::ParseError& e) + { + dterr << e.what() << "\n"; + return false; + } + } + + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str) + { + try + { + pose.rotation.init(rpy_str); + } + catch (urdf::ParseError& e) + { + dterr << e.what() << "\n"; + return false; + } + } + } + return true; +} + +} // (anonymous) namespace + Entity::Entity(const urdf::Entity& urdfEntity) : model(urdfEntity.model), origin(urdfEntity.origin), @@ -72,9 +111,16 @@ std::shared_ptr parseWorldURDF( const dart::common::Uri& _baseUri, const common::ResourceRetrieverPtr& retriever) { - TiXmlDocument xml_doc; - xml_doc.Parse( _xml_string.c_str() ); - TiXmlElement *world_xml = xml_doc.FirstChildElement("world"); + tinyxml2::XMLDocument xml_doc; + const auto result = xml_doc.Parse(&_xml_string.front()); + if (result != tinyxml2::XML_SUCCESS) + { + dtwarn << "[parseWorldURDF] Failed parsing XML: TinyXML2 returned error" + " code " << result << ".\n"; + return nullptr; + } + + auto* world_xml = xml_doc.FirstChildElement("world"); if( !world_xml ) { dtwarn << "[parseWorldURDF] ERROR: Could not find a element in XML, exiting and not loading! \n"; @@ -89,7 +135,7 @@ std::shared_ptr parseWorldURDF( return nullptr; } - std::shared_ptr world(new World); + std::shared_ptr world = std::make_shared(); world->name = std::string(name); if(debug) std::cout<< "World name: "<< world->name << std::endl; @@ -98,9 +144,9 @@ std::shared_ptr parseWorldURDF( int count = 0; std::map includedFiles; - for( TiXmlElement* include_xml = world_xml->FirstChildElement("include"); + for( auto* include_xml = world_xml->FirstChildElement("include"); include_xml != nullptr; - include_xml = include_xml->NextSiblingElement("include") ) + include_xml = include_xml->NextSiblingElement("include")) { ++count; const char *filename = include_xml->Attribute("filename"); @@ -114,7 +160,7 @@ std::shared_ptr parseWorldURDF( // Get all entities count = 0; - for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity"); + for( auto* entity_xml = world_xml->FirstChildElement("entity"); entity_xml != nullptr; entity_xml = entity_xml->NextSiblingElement("entity") ) { @@ -162,10 +208,10 @@ std::shared_ptr parseWorldURDF( else { // Parse location - TiXmlElement* origin = entity_xml->FirstChildElement("origin"); + auto* origin = entity_xml->FirstChildElement("origin"); if( origin ) { - if( !urdf::parsePose( entity.origin, origin ) ) + if( !parsePose( entity.origin, origin ) ) { dtwarn << "[ERROR] Missing origin tag for '" << entity.model->getName() << "'\n"; return world; @@ -187,7 +233,7 @@ std::shared_ptr parseWorldURDF( } - catch( urdf::ParseError& e ) + catch( urdf::ParseError& /*e*/) { if(debug) std::cout << "Entity xml not initialized correctly \n"; //entity->reset(); diff --git a/data/urdf/test/testWorld.urdf b/data/urdf/test/testWorld.urdf index d617db4bd9a29..9461ae15872ac 100644 --- a/data/urdf/test/testWorld.urdf +++ b/data/urdf/test/testWorld.urdf @@ -2,8 +2,8 @@ - - + + diff --git a/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp b/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp index 338e310db106e..75e0cdb7babd4 100644 --- a/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp +++ b/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp @@ -248,6 +248,41 @@ class ConstraintEventHandler : public ::osgGA::GUIEventHandler dart::sub_ptr mDnD; }; +class ShadowEventHandler : public osgGA::GUIEventHandler +{ +public: + + ShadowEventHandler(OperationalSpaceControlWorld* node, dart::gui::osg::Viewer* viewer) : mNode(node), mViewer(viewer) {} + + bool handle(const osgGA::GUIEventAdapter& ea, + osgGA::GUIActionAdapter&) override + { + if(ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN) + { + if(ea.getKey() == 's' || ea.getKey() == 'S') + { + if(mNode->isShadowed()) + mNode->setShadowTechnique(nullptr); + else + mNode->setShadowTechnique(dart::gui::osg::WorldNode::createDefaultShadowTechnique(mViewer)); + return true; + } + } + + // The return value should be 'true' if the input has been fully handled + // and should not be visible to any remaining event handlers. It should be + // false if the input has not been fully handled and should be viewed by + // any remaining event handlers. + return false; + } + +protected: + + OperationalSpaceControlWorld* mNode; + dart::gui::osg::Viewer* mViewer; + +}; + int main() { dart::simulation::WorldPtr world(new dart::simulation::World); @@ -299,6 +334,7 @@ int main() // Add our custom event handler to the Viewer viewer.addEventHandler(new ConstraintEventHandler(node->dnd)); + viewer.addEventHandler(new ShadowEventHandler(node.get(), &viewer)); // Print out instructions std::cout << viewer.getInstructions() << std::endl; diff --git a/package.xml b/package.xml index 2ac6558a31c53..e6a2c470a026f 100644 --- a/package.xml +++ b/package.xml @@ -31,7 +31,6 @@ liburdfdom-dev libxi-dev libxmu-dev - tinyxml tinyxml2 catkin diff --git a/unittests/comprehensive/test_Joints.cpp b/unittests/comprehensive/test_Joints.cpp index 3c7be02a1444e..7a9dda3cda145 100644 --- a/unittests/comprehensive/test_Joints.cpp +++ b/unittests/comprehensive/test_Joints.cpp @@ -497,6 +497,75 @@ TEST_F(JOINTS, POSITION_LIMIT) } } +//============================================================================== +TEST_F(JOINTS, JOINT_LIMITS) +{ + simulation::WorldPtr myWorld + = utils::SkelParser::readWorld( + "dart://sample/skel/test/joint_limit_test.skel"); + EXPECT_TRUE(myWorld != nullptr); + + myWorld->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); + + dynamics::SkeletonPtr pendulum = myWorld->getSkeleton("double_pendulum"); + EXPECT_TRUE(pendulum != nullptr); + + dynamics::Joint* joint0 = pendulum->getJoint("joint0"); + + EXPECT_TRUE(joint0 != nullptr); + + double limit = constantsd::pi() / 6.0; + Eigen::VectorXd limits = Eigen::VectorXd::Constant(1, constantsd::pi() / 2.0); + + joint0->setPositionLowerLimit(0, -limit); + joint0->setPositionUpperLimit(0, limit); + EXPECT_EQ( + joint0->getPositionLowerLimits(), Eigen::VectorXd::Constant(1, -limit)); + EXPECT_EQ( + joint0->getPositionUpperLimits(), Eigen::VectorXd::Constant(1, limit)); + + joint0->setPositionLowerLimits(-limits); + joint0->setPositionUpperLimits(limits); + EXPECT_EQ(joint0->getPositionLowerLimits(), -limits); + EXPECT_EQ(joint0->getPositionUpperLimits(), limits); + + joint0->setVelocityLowerLimit(0, -limit); + joint0->setVelocityUpperLimit(0, limit); + EXPECT_EQ( + joint0->getVelocityLowerLimits(), Eigen::VectorXd::Constant(1, -limit)); + EXPECT_EQ( + joint0->getVelocityUpperLimits(), Eigen::VectorXd::Constant(1, limit)); + + joint0->setVelocityLowerLimits(-limits); + joint0->setVelocityUpperLimits(limits); + EXPECT_EQ(joint0->getVelocityLowerLimits(), -limits); + EXPECT_EQ(joint0->getVelocityUpperLimits(), limits); + + joint0->setAccelerationLowerLimit(0, -limit); + joint0->setAccelerationUpperLimit(0, limit); + EXPECT_EQ( + joint0->getAccelerationLowerLimits(), Eigen::VectorXd::Constant(1, -limit)); + EXPECT_EQ( + joint0->getAccelerationUpperLimits(), Eigen::VectorXd::Constant(1, limit)); + + joint0->setAccelerationLowerLimits(-limits); + joint0->setAccelerationUpperLimits(limits); + EXPECT_EQ(joint0->getAccelerationLowerLimits(), -limits); + EXPECT_EQ(joint0->getAccelerationUpperLimits(), limits); + + joint0->setForceLowerLimit(0, -limit); + joint0->setForceUpperLimit(0, limit); + EXPECT_EQ( + joint0->getForceLowerLimits(), Eigen::VectorXd::Constant(1, -limit)); + EXPECT_EQ( + joint0->getForceUpperLimits(), Eigen::VectorXd::Constant(1, limit)); + + joint0->setForceLowerLimits(-limits); + joint0->setForceUpperLimits(limits); + EXPECT_EQ(joint0->getForceLowerLimits(), -limits); + EXPECT_EQ(joint0->getForceUpperLimits(), limits); +} + //============================================================================== void testJointCoulombFrictionForce(double _timeStep) {