diff --git a/cmake/DARTFindDependencies.cmake b/cmake/DARTFindDependencies.cmake index 8f53a6feb8384..73d9716ba6f8e 100644 --- a/cmake/DARTFindDependencies.cmake +++ b/cmake/DARTFindDependencies.cmake @@ -90,30 +90,40 @@ dart_find_package(Boost) # octomap dart_find_package(octomap) -if (octomap_FOUND AND NOT MSVC) - if (MSVC) - # Supporting Octomap on Windows is disabled for the following issue: - # https://github.com/OctoMap/octomap/pull/213 - message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap " - "is not supported on Windows until " - "'https://github.com/OctoMap/octomap/pull/213' " - "is resolved.") - set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) - elseif (NOT octomap_VERSION VERSION_LESS 1.9.0) - message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap 1.9.0 or " - "greater is not supported yet. Please see " - "'https://github.com/dartsim/dart/issues/1078' for the details") - set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) - else() - set(HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE) - if(DART_VERBOSE) - message(STATUS "Looking for octomap - version ${octomap_VERSION} found") +if(MSVC) + # Supporting Octomap on Windows is disabled for the following issue: + # https://github.com/OctoMap/octomap/pull/213 + message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap " + "is not supported on Windows until " + "'https://github.com/OctoMap/octomap/pull/213' " + "is resolved.") + set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) +else() + if(OCTOMAP_FOUND OR octomap_FOUND) + if(NOT DEFINED octomap_VERSION) + set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) + message(STATUS "Looking for octomap - octomap_VERSION is not defined, " + "please install octomap with version information" + ) + else() + if(NOT octomap_VERSION VERSION_LESS 1.9.0) + message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap 1.9.0 or " + "greater is not supported yet. Please see " + "'https://github.com/dartsim/dart/issues/1078' for the details") + set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) + else() + set(HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE) + if(DART_VERBOSE) + message(STATUS "Looking for octomap - version ${octomap_VERSION} found") + endif() + endif() endif() + else() + set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) + message(STATUS "Looking for octomap - NOT found, to use VoxelGridShape, " + "please install octomap" + ) endif() -else() - set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE) - message(STATUS "Looking for octomap - NOT found, to use VoxelGridShape, " - "please install octomap") endif() #-------------------- diff --git a/dart/dynamics/CMakeLists.txt b/dart/dynamics/CMakeLists.txt index 3c1465e48c02c..5bd60edf1ccc1 100644 --- a/dart/dynamics/CMakeLists.txt +++ b/dart/dynamics/CMakeLists.txt @@ -28,17 +28,14 @@ install( COMPONENT headers ) -dart_format_add( - TranslationalJoint2D.hpp - TranslationalJoint2D.cpp - detail/TranslationalJoint2DAspect.hpp - detail/TranslationalJoint2DAspect.cpp -) - dart_format_add( HeightmapShape.hpp VoxelGridShape.hpp VoxelGridShape.cpp SmartPointer.hpp + TranslationalJoint2D.hpp + TranslationalJoint2D.cpp detail/HeightmapShape-impl.hpp + detail/TranslationalJoint2DAspect.hpp + detail/TranslationalJoint2DAspect.cpp ) diff --git a/dart/dynamics/VoxelGridShape.cpp b/dart/dynamics/VoxelGridShape.cpp index 1fcff68bf6c3c..4f62ae045a2ec 100644 --- a/dart/dynamics/VoxelGridShape.cpp +++ b/dart/dynamics/VoxelGridShape.cpp @@ -43,23 +43,35 @@ namespace dynamics { namespace { //============================================================================== -octomap::point3d toPoint3d(const Eigen::Vector3d& point) +octomap::point3d toPoint3f(const Eigen::Vector3f& point) { return octomap::point3d(point.x(), point.y(), point.z()); } //============================================================================== -octomath::Quaternion toQuaternion(const Eigen::Matrix3d& rotation) +octomap::point3d toPoint3d(const Eigen::Vector3d& point) +{ + return toPoint3f(point.cast()); +} + +//============================================================================== +octomath::Quaternion toQuaternionf(const Eigen::Matrix3f& rotation) { - Eigen::Quaterniond quat(rotation); + Eigen::Quaternionf quat(rotation); return octomath::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()); } +//============================================================================== +octomath::Quaternion toQuaterniond(const Eigen::Matrix3d& rotation) +{ + return toQuaternionf(rotation.cast()); +} + //============================================================================== octomap::pose6d toPose6d(const Eigen::Isometry3d& frame) { return octomap::pose6d( - toPoint3d(frame.translation()), toQuaternion(frame.linear())); + toPoint3d(frame.translation()), toQuaterniond(frame.linear())); } } // namespace @@ -68,6 +80,8 @@ octomap::pose6d toPose6d(const Eigen::Isometry3d& frame) VoxelGridShape::VoxelGridShape(double resolution) : Shape() { setOctree(fcl_make_shared(resolution)); + + mVariance = DYNAMIC_ELEMENTS; } //============================================================================== @@ -82,6 +96,8 @@ VoxelGridShape::VoxelGridShape(fcl_shared_ptr octree) : Shape() } setOctree(std::move(octree)); + + mVariance = DYNAMIC_ELEMENTS; } //============================================================================== diff --git a/dart/gui/osg/ShapeFrameNode.cpp b/dart/gui/osg/ShapeFrameNode.cpp index b75177ccc7509..d27bdc7827ead 100644 --- a/dart/gui/osg/ShapeFrameNode.cpp +++ b/dart/gui/osg/ShapeFrameNode.cpp @@ -48,6 +48,7 @@ #include "dart/gui/osg/render/MeshShapeNode.hpp" #include "dart/gui/osg/render/SoftMeshShapeNode.hpp" #include "dart/gui/osg/render/LineSegmentShapeNode.hpp" +#include "dart/gui/osg/render/VoxelGridShapeNode.hpp" #include "dart/gui/osg/render/WarningShapeNode.hpp" #include "dart/dynamics/Frame.hpp" @@ -64,6 +65,7 @@ #include "dart/dynamics/MeshShape.hpp" #include "dart/dynamics/SoftMeshShape.hpp" #include "dart/dynamics/LineSegmentShape.hpp" +#include "dart/dynamics/VoxelGridShape.hpp" #include "dart/dynamics/SimpleFrame.hpp" namespace dart { @@ -285,6 +287,15 @@ void ShapeFrameNode::createShapeNode( else warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName()); } + else if(VoxelGridShape::getStaticType() == shapeType) + { + std::shared_ptr lss = + std::dynamic_pointer_cast(shape); + if(lss) + mRenderShapeNode = new render::VoxelGridShapeNode(lss, this); + else + warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName()); + } else { mRenderShapeNode = new render::WarningShapeNode(shape, this); diff --git a/dart/gui/osg/render/CMakeLists.txt b/dart/gui/osg/render/CMakeLists.txt index 2a824c047cd86..2c1de5d751a30 100644 --- a/dart/gui/osg/render/CMakeLists.txt +++ b/dart/gui/osg/render/CMakeLists.txt @@ -18,3 +18,8 @@ install( DESTINATION include/dart/gui/osg/render COMPONENT headers ) + +dart_format_add( + VoxelGridShapeNode.hpp + VoxelGridShapeNode.cpp +) diff --git a/dart/gui/osg/render/ShapeNode.cpp b/dart/gui/osg/render/ShapeNode.cpp index 35576aed778dd..86699335cd098 100644 --- a/dart/gui/osg/render/ShapeNode.cpp +++ b/dart/gui/osg/render/ShapeNode.cpp @@ -42,7 +42,7 @@ namespace render { ShapeNode::ShapeNode(std::shared_ptr shape, ShapeFrameNode* parentNode, ::osg::Node* node) - : mShape(shape), + : mShape(std::move(shape)), mParentShapeFrameNode(parentNode), mNode(node), mUtilized(true) diff --git a/dart/gui/osg/render/VoxelGridShapeNode.cpp b/dart/gui/osg/render/VoxelGridShapeNode.cpp new file mode 100644 index 0000000000000..60e3f59fd5c24 --- /dev/null +++ b/dart/gui/osg/render/VoxelGridShapeNode.cpp @@ -0,0 +1,272 @@ +/* + * Copyright (c) 2011-2019, 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 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/gui/osg/render/VoxelGridShapeNode.hpp" + +#include +#include +#include +#include + +#include "dart/dynamics/SimpleFrame.hpp" +#include "dart/dynamics/VoxelGridShape.hpp" +#include "dart/gui/osg/Utils.hpp" + +namespace dart { +namespace gui { +namespace osg { +namespace render { + +//============================================================================== +class VoxelGridShapeDrawable : public ::osg::ShapeDrawable +{ +public: + VoxelGridShapeDrawable( + dynamics::VoxelGridShape* shape, + dynamics::VisualAspect* visualAspect, + VoxelGridShapeGeode* parent); + + void refresh(bool firstTime); + +protected: + ~VoxelGridShapeDrawable() override = default; + + dynamics::VoxelGridShape* mVoxelGridShape; + dynamics::VisualAspect* mVisualAspect; + VoxelGridShapeGeode* mParent; + std::size_t mVoxelVersion; + +private: + void updateBoxes( + ::osg::CompositeShape* osgShape, + const octomap::OcTree* tree, + double threashold); + std::vector<::osg::ref_ptr<::osg::Box>> mBoxes; +}; + +//============================================================================== +class VoxelGridShapeGeode : public ShapeNode, public ::osg::Geode +{ +public: + VoxelGridShapeGeode( + dynamics::VoxelGridShape* shape, + ShapeFrameNode* parentShapeFrame, + VoxelGridShapeNode* parentNode); + + void refresh(); + void extractData(); + +protected: + virtual ~VoxelGridShapeGeode(); + + VoxelGridShapeNode* mParentNode; + dynamics::VoxelGridShape* mVoxelGridShape; + VoxelGridShapeDrawable* mDrawable; +}; + +//============================================================================== +VoxelGridShapeNode::VoxelGridShapeNode( + std::shared_ptr shape, ShapeFrameNode* parent) + : ShapeNode(shape, parent, this), mVoxelGridShape(shape), mGeode(nullptr) +{ + extractData(true); + setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u); +} + +//============================================================================== +void VoxelGridShapeNode::refresh() +{ + mUtilized = true; + + setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u); + + if (mShape->getDataVariance() == dynamics::Shape::STATIC) + return; + + extractData(false); +} + +//============================================================================== +void VoxelGridShapeNode::extractData(bool /*firstTime*/) +{ + if (nullptr == mGeode) + { + mGeode = new VoxelGridShapeGeode( + mVoxelGridShape.get(), mParentShapeFrameNode, this); + addChild(mGeode); + return; + } + + mGeode->refresh(); +} + +//============================================================================== +VoxelGridShapeNode::~VoxelGridShapeNode() +{ + // Do nothing +} + +//============================================================================== +VoxelGridShapeGeode::VoxelGridShapeGeode( + dynamics::VoxelGridShape* shape, + ShapeFrameNode* parentShapeFrame, + VoxelGridShapeNode* parentNode) + : ShapeNode(parentNode->getShape(), parentShapeFrame, this), + mParentNode(parentNode), + mVoxelGridShape(shape), + mDrawable(nullptr) +{ + getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON); + extractData(); +} + +//============================================================================== +void VoxelGridShapeGeode::refresh() +{ + mUtilized = true; + + extractData(); +} + +//============================================================================== +void VoxelGridShapeGeode::extractData() +{ + if (nullptr == mDrawable) + { + mDrawable + = new VoxelGridShapeDrawable(mVoxelGridShape, mVisualAspect, this); + addDrawable(mDrawable); + return; + } + + mDrawable->refresh(false); +} + +//============================================================================== +VoxelGridShapeGeode::~VoxelGridShapeGeode() +{ + // Do nothing +} + +//============================================================================== +VoxelGridShapeDrawable::VoxelGridShapeDrawable( + dynamics::VoxelGridShape* shape, + dynamics::VisualAspect* visualAspect, + VoxelGridShapeGeode* parent) + : mVoxelGridShape(shape), + mVisualAspect(visualAspect), + mParent(parent), + mVoxelVersion(dynamics::INVALID_INDEX) +{ + refresh(true); +} + +//============================================================================== +::osg::Vec3 toVec3(const octomap::point3d& point) +{ + return ::osg::Vec3(point.x(), point.y(), point.z()); +} + +//============================================================================== +void VoxelGridShapeDrawable::refresh(bool firstTime) +{ + if (mVoxelGridShape->getDataVariance() == dynamics::Shape::STATIC) + setDataVariance(::osg::Object::STATIC); + else + setDataVariance(::osg::Object::DYNAMIC); + + if (mVoxelGridShape->checkDataVariance(dynamics::Shape::DYNAMIC_ELEMENTS) + || firstTime) + { + if (mVoxelVersion != mVoxelGridShape->getVersion()) + { + auto osgShape = new ::osg::CompositeShape(); + auto octomap = mVoxelGridShape->getOctree(); + updateBoxes(osgShape, octomap.get(), 0.75); + + setShape(osgShape); + dirtyDisplayList(); + } + } + + if (mVoxelGridShape->checkDataVariance(dynamics::Shape::DYNAMIC_COLOR) + || firstTime) + { + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); + } + + mVoxelVersion = mVoxelGridShape->getVersion(); +} + +//============================================================================== +void VoxelGridShapeDrawable::updateBoxes( + ::osg::CompositeShape* osgShape, + const octomap::OcTree* tree, + double threashold) +{ + const auto size = static_cast(tree->getResolution()); + const auto newNumBoxes = tree->getNumLeafNodes(); + mBoxes.reserve(newNumBoxes); + + // TODO(JS): Use begin_leafs_bbx/end_leafs_bbx to only render voxels that + // are in the view sight. For this, camera view frustum would be required. + + std::size_t boxIndex = 0u; + for (auto it = tree->begin_leafs(), end = tree->end_leafs(); it != end; ++it) + { + threashold = tree->getOccupancyThres(); + + if (it->getOccupancy() < threashold) + continue; + + if (boxIndex < mBoxes.size()) + { + mBoxes[boxIndex]->setCenter(toVec3(it.getCoordinate())); + } + else + { + auto osgSphere = new ::osg::Box(toVec3(it.getCoordinate()), size); + mBoxes.emplace_back(osgSphere); + } + + osgShape->addChild(mBoxes[boxIndex]); + + boxIndex++; + } + + mBoxes.resize(boxIndex); +} + +} // namespace render +} // namespace osg +} // namespace gui +} // namespace dart diff --git a/dart/gui/osg/render/VoxelGridShapeNode.hpp b/dart/gui/osg/render/VoxelGridShapeNode.hpp new file mode 100644 index 0000000000000..ada324d481af2 --- /dev/null +++ b/dart/gui/osg/render/VoxelGridShapeNode.hpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2011-2019, 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 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_GUI_OSG_RENDER_VOXELGRIDSHAPENODE_HPP_ +#define DART_GUI_OSG_RENDER_VOXELGRIDSHAPENODE_HPP_ + +#include +#include + +#include "dart/gui/osg/render/ShapeNode.hpp" + +namespace dart { + +namespace dynamics { +class VoxelGridShape; +} // namespace dynamics + +namespace gui { +namespace osg { +namespace render { + +class VoxelGridShapeGeode; + +class VoxelGridShapeNode : public ShapeNode, public ::osg::MatrixTransform +{ +public: + VoxelGridShapeNode( + std::shared_ptr shape, ShapeFrameNode* parent); + + void refresh() override; + void extractData(bool firstTime); + +protected: + virtual ~VoxelGridShapeNode() override; + + std::shared_ptr mVoxelGridShape; + VoxelGridShapeGeode* mGeode; +}; + +} // namespace render +} // namespace osg +} // namespace gui +} // namespace dart + +#endif // DART_GUI_OSG_RENDER_VOXELGRIDSHAPENODE_HPP_ diff --git a/examples/osgExamples/CMakeLists.txt b/examples/osgExamples/CMakeLists.txt index aa29623ec54c8..2f3862a745e15 100644 --- a/examples/osgExamples/CMakeLists.txt +++ b/examples/osgExamples/CMakeLists.txt @@ -6,6 +6,9 @@ add_subdirectory(osgEmpty) add_subdirectory(osgHuboPuppet) add_subdirectory(osgImGui) add_subdirectory(osgOperationalSpaceControl) +if(HAVE_OCTOMAP) + add_subdirectory(osgPointCloud) +endif() add_subdirectory(osgSoftBodies) add_subdirectory(osgTinkertoy) add_subdirectory(osgWamIkFast) diff --git a/examples/osgExamples/osgPointCloud/CMakeLists.txt b/examples/osgExamples/osgPointCloud/CMakeLists.txt new file mode 100644 index 0000000000000..b7eb318021231 --- /dev/null +++ b/examples/osgExamples/osgPointCloud/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5.1) + +if(DART_IN_SOURCE_BUILD) + include(${CMAKE_CURRENT_SOURCE_DIR}/InSourceBuild.cmake) + return() +endif() + +project(osgOperationalSpaceControl) + +find_package(DART 6.6.0 REQUIRED COMPONENTS utils-urdf gui-osg CONFIG) + +file(GLOB srcs "*.cpp" "*.hpp") +add_executable(${PROJECT_NAME} ${srcs}) +target_link_libraries(${PROJECT_NAME} PUBLIC dart dart-utils-urdf dart-gui-osg) diff --git a/examples/osgExamples/osgPointCloud/InSourceBuild.cmake b/examples/osgExamples/osgPointCloud/InSourceBuild.cmake new file mode 100644 index 0000000000000..64aa62244bb84 --- /dev/null +++ b/examples/osgExamples/osgPointCloud/InSourceBuild.cmake @@ -0,0 +1,17 @@ +get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) + +if(NOT TARGET dart-utils-urdf OR NOT TARGET dart-gui-osg) + return() +endif() + +file(GLOB srcs "*.cpp" "*.hpp") + +add_executable(${example_name} ${srcs}) +target_link_libraries(${example_name} dart dart-utils-urdf dart-gui-osg) +set_target_properties(${example_name} + PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" +) + +dart_add_example(${example_name}) +dart_format_add(${srcs}) diff --git a/examples/osgExamples/osgPointCloud/README.md b/examples/osgExamples/osgPointCloud/README.md new file mode 100644 index 0000000000000..965b52cdf07b2 --- /dev/null +++ b/examples/osgExamples/osgPointCloud/README.md @@ -0,0 +1,20 @@ +This project is dependent on DART. Please make sure a proper version of DART is +installed before building this project. + +## Build Instructions + +From this directory: + + $ mkdir build + $ cd build + $ cmake .. + $ make + +## Execute Instructions + +Launch the executable from the build directory above: + + $ ./{generated_executable} + +Follow the instructions detailed in the console. + diff --git a/examples/osgExamples/osgPointCloud/main.cpp b/examples/osgExamples/osgPointCloud/main.cpp new file mode 100644 index 0000000000000..8bff3e4661ea2 --- /dev/null +++ b/examples/osgExamples/osgPointCloud/main.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (c) 2011-2019, 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 following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include +#include + +using namespace dart; +using namespace dart::common; +using namespace dart::dynamics; +using namespace dart::math; + +const std::string& robotName = "KR5"; + +class PointCloudWorld : public gui::osg::WorldNode +{ +public: + explicit PointCloudWorld( + simulation::WorldPtr world, dynamics::SkeletonPtr robot) + : gui::osg::WorldNode(std::move(world)), mRobot(std::move(robot)) + { + auto voxelFrame = mWorld->getSimpleFrame("voxel"); + + mVoxelShape = std::dynamic_pointer_cast( + voxelFrame->getShape()); + + assert(mVoxelShape); + } + + // Triggered at the beginning of each simulation step + void customPreStep() override + { + if (!mRobot) + return; + + // Set robot pose + Eigen::VectorXd pos = mRobot->getPositions(); + pos += 0.01 * Eigen::VectorXd::Random(pos.size()); + mRobot->setPositions(pos); + + // Generate point cloud from robot meshes + auto pointCloud = generatePointCloud(100); + + // Update sensor position + static double time = 0.0; + const double dt = 0.01; + const double radius = 1.0; + Eigen::Vector3d center = Eigen::Vector3d(0.0, 0.1, 0.0); + Eigen::Vector3d sensorPos = center; + sensorPos[0] = radius * std::sin(time); + sensorPos[1] = radius * std::cos(time); + sensorPos[2] = 0.5 + 0.25 * std::sin(time * 2.0); + time += dt; + auto sensorFrame = mWorld->getSimpleFrame("sensor"); + assert(sensorFrame); + sensorFrame->setTranslation(sensorPos); + + // Update voxel + mVoxelShape->updateOccupancy(pointCloud, sensorPos); + } + +protected: + octomap::Pointcloud generatePointCloud(std::size_t numPoints) + { + octomap::Pointcloud pointCloud; + + const auto numBodies = mRobot->getNumBodyNodes(); + assert(numBodies > 0); + while (true) + { + const auto bodyIndex + = math::Random::uniform(0, numBodies - 1); + auto body = mRobot->getBodyNode(bodyIndex); + auto shapeNodes = body->getShapeNodesWith(); + if (shapeNodes.empty()) + continue; + + const auto shapeIndex + = math::Random::uniform(0, shapeNodes.size() - 1); + auto shapeNode = shapeNodes[shapeIndex]; + auto shape = shapeNode->getShape(); + assert(shape); + + if (!shape->is()) + continue; + auto mesh = std::static_pointer_cast(shape); + + auto assimpScene = mesh->getMesh(); + assert(assimpScene); + + if (assimpScene->mNumMeshes < 1) + continue; + const auto meshIndex + = math::Random::uniform(0, assimpScene->mNumMeshes - 1); + + auto assimpMesh = assimpScene->mMeshes[meshIndex]; + auto numVertices = assimpMesh->mNumVertices; + + auto vertexIndex + = math::Random::uniform(0, numVertices - 1); + auto vertex = assimpMesh->mVertices[vertexIndex]; + + Eigen::Isometry3d tf = shapeNode->getWorldTransform(); + Eigen::Vector3d eigenVertex + = Eigen::Vector3d(vertex.x, vertex.y, vertex.z); + eigenVertex = tf * eigenVertex; + + pointCloud.push_back(eigenVertex.x(), eigenVertex.y(), eigenVertex.z()); + + if (pointCloud.size() == numPoints) + return pointCloud; + } + } + + SkeletonPtr mRobot; + + std::shared_ptr mVoxelShape; +}; + +dynamics::SkeletonPtr createRobot(const std::string& name) +{ + auto urdfParser = dart::utils::DartLoader(); + + // Load the robot + auto robot + = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf"); + + // Set the colors of the models to obey the shape's color specification + for (std::size_t i = 0; i < robot->getNumBodyNodes(); ++i) + { + BodyNode* bn = robot->getBodyNode(i); + auto shapeNodes = bn->getShapeNodesWith(); + for (auto shapeNode : shapeNodes) + { + std::shared_ptr mesh + = std::dynamic_pointer_cast(shapeNode->getShape()); + if (mesh) + mesh->setColorMode(MeshShape::SHAPE_COLOR); + } + } + + // Rotate the robot so that z is upwards (default transform is not Identity) + robot->getJoint(0)->setTransformFromParentBodyNode( + Eigen::Isometry3d::Identity()); + + robot->setName(name); + + return robot; +} + +dynamics::SkeletonPtr createGround() +{ + auto urdfParser = dart::utils::DartLoader(); + + auto ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf"); + + // Rotate and move the ground so that z is upwards + Eigen::Isometry3d ground_tf + = ground->getJoint(0)->getTransformFromParentBodyNode(); + ground_tf.pretranslate(Eigen::Vector3d(0, 0, 0.5)); + ground_tf.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(1, 0, 0))); + ground->getJoint(0)->setTransformFromParentBodyNode(ground_tf); + + return ground; +} + +dynamics::SimpleFramePtr createVoxelFrame(double resolution = 0.01) +{ + auto voxelShape + = ::std::make_shared(resolution); + auto voxelFrame = ::dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World()); + voxelFrame->setName("voxel"); + voxelFrame->setShape(voxelShape); + auto visualAspect = voxelFrame->createVisualAspect(); + visualAspect->setRGBA(Color::Orange(0.5)); + + return voxelFrame; +} + +dynamics::SimpleFramePtr createSensorFrame() +{ + auto sphereShape = ::std::make_shared(0.05); + auto sensorFrame = ::dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World()); + sensorFrame->setName("sensor"); + sensorFrame->setShape(sphereShape); + auto visualAspect = sensorFrame->createVisualAspect(); + visualAspect->setRGB(Color::Red()); + + return sensorFrame; +} + +int main() +{ + auto world = dart::simulation::World::create(); + world->setGravity(Eigen::Vector3d::Zero()); + + auto robot = createRobot(robotName); + world->addSkeleton(robot); + + auto ground = createGround(); + world->addSkeleton(ground); + + auto voxel = createVoxelFrame(0.05); + world->addSimpleFrame(voxel); + + auto sensor = createSensorFrame(); + world->addSimpleFrame(sensor); + + // Create an instance of our customized WorldNode + ::osg::ref_ptr node = new PointCloudWorld(world, robot); + node->setNumStepsPerCycle(16); + + // Create the Viewer instance + dart::gui::osg::Viewer viewer; + viewer.addWorldNode(node); + viewer.simulate(true); + + // Print out instructions + std::cout << viewer.getInstructions() << std::endl; + + // Set up the window to be 640x480 pixels + viewer.setUpViewInWindow(0, 0, 640, 480); + + viewer.getCameraManipulator()->setHomePosition( + ::osg::Vec3(2.57, 3.14, 1.64), + ::osg::Vec3(0.00, 0.00, 0.00), + ::osg::Vec3(-0.24, -0.25, 0.94)); + // We need to re-dirty the CameraManipulator by passing it into the viewer + // again, so that the viewer knows to update its HomePosition setting + viewer.setCameraManipulator(viewer.getCameraManipulator()); + + // Begin the application loop + viewer.run(); +}