From 8dd13e2e5c3f7181bcd9e204b3d4edfcc9a1a9df Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Mar 2024 22:15:21 -0700 Subject: [PATCH] [example] Add boxes example (cherry picked from commit ad4ea97e6d8ae72a3bcd84053caef9005efa686f) --- examples/CMakeLists.txt | 1 + examples/boxes/CMakeLists.txt | 19 +++++ examples/boxes/README.md | 20 +++++ examples/boxes/main.cpp | 149 ++++++++++++++++++++++++++++++++++ 4 files changed, 189 insertions(+) create mode 100644 examples/boxes/CMakeLists.txt create mode 100644 examples/boxes/README.md create mode 100644 examples/boxes/main.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index fec298b7637e3..5e1ee816aff98 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -11,6 +11,7 @@ add_subdirectory(atlas_puppet) add_subdirectory(atlas_simbicon) add_subdirectory(biped_stand) add_subdirectory(box_stacking) +add_subdirectory(boxes) add_subdirectory(drag_and_drop) add_subdirectory(empty) add_subdirectory(heightmap) diff --git a/examples/boxes/CMakeLists.txt b/examples/boxes/CMakeLists.txt new file mode 100644 index 0000000000000..444b2dfbfee04 --- /dev/null +++ b/examples/boxes/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.22.1) + +get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) + +project(${example_name}) + +set(required_components utils-urdf gui-osg) +set(required_libraries dart dart-utils-urdf dart-gui-osg) + +if(DART_IN_SOURCE_BUILD) + dart_build_example_in_source(${example_name} LINK_LIBRARIES ${required_libraries}) + return() +endif() + +find_package(DART 6.14.0 REQUIRED COMPONENTS ${required_components} CONFIG) + +file(GLOB srcs "*.cpp" "*.hpp") +add_executable(${example_name} ${srcs}) +target_link_libraries(${example_name} PUBLIC ${required_libraries}) diff --git a/examples/boxes/README.md b/examples/boxes/README.md new file mode 100644 index 0000000000000..965b52cdf07b2 --- /dev/null +++ b/examples/boxes/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/boxes/main.cpp b/examples/boxes/main.cpp new file mode 100644 index 0000000000000..7acfa1dd2cd62 --- /dev/null +++ b/examples/boxes/main.cpp @@ -0,0 +1,149 @@ +/* + * Copyright (c) 2011-2024, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/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 + +using namespace dart; + +[[nodiscard]] dynamics::SkeletonPtr createBox( + const Eigen::Vector3d& position, + const Eigen::Vector3d& size = Eigen::Vector3d(1, 1, 1), + const Eigen::Vector3d& color + = dart::math::Random::uniform(0.0, 1.0)) +{ + static size_t index = 0; + dynamics::SkeletonPtr boxSkel + = dynamics::Skeleton::create("box" + std::to_string(index++)); + + // Give the floor a body + dynamics::BodyNodePtr boxBody + = boxSkel->createJointAndBodyNodePair(nullptr) + .second; + + // Give the body a shape + auto boxShape = std::make_shared(size); + dynamics::ShapeNode* shapeNode = boxBody->createShapeNodeWith< + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>(boxShape); + shapeNode->getVisualAspect()->setColor(color); + + // Put the body into position + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = position; + boxBody->getParentJoint()->setTransformFromParentBodyNode(tf); + + return boxSkel; +} + +int main() +{ + // Create an empty world + auto world = simulation::World::create(); + + // Create dim x dim x dim boxes + auto dim = 5; + for (auto i = 0; i < dim; ++i) { + for (auto j = 0; j < dim; ++j) { + for (auto k = 0; k < dim; ++k) { + auto x = i - dim / 2; + auto y = j - dim / 2; + auto z = k + 1; + auto position = Eigen::Vector3d(x, y, z); + auto size = Eigen::Vector3d(0.9, 0.9, 0.9); + auto color = Eigen::Vector3d( + static_cast(i) / dim, + static_cast(j) / dim, + static_cast(k) / dim); + auto box = createBox(position, size, color); + world->addSkeleton(box); + } + } + } + + // Create ground + auto ground = dynamics::Skeleton::create("ground"); + auto groundBody + = ground->createJointAndBodyNodePair().second; + auto groundShapeNode = groundBody->createShapeNodeWith< + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>( + std::make_shared(Eigen::Vector3d(25.0, 25.0, 0.1))); + groundShapeNode->getVisualAspect()->setColor(dart::Color::LightGray()); + world->addSkeleton(ground); + + // Wrap a WorldNode around it + ::osg::ref_ptr node + = new gui::osg::RealTimeWorldNode(world); + + // Create a Viewer and set it up with the WorldNode + auto viewer = gui::osg::Viewer(); + viewer.addWorldNode(node); + + // Enable shadow + auto shadow + = dart::gui::osg::WorldNode::createDefaultShadowTechnique(&viewer); + if (auto sm = dynamic_cast<::osgShadow::ShadowMap*>(shadow.get())) { + auto mapResolution = static_cast(std::pow(2, 12)); + sm->setTextureSize(::osg::Vec2s(mapResolution, mapResolution)); + } + node->setShadowTechnique(shadow); + + viewer.addInstructionText( + "[Experimental] Please note: This example is in an experimental phase " + "and may not be fully functional at this time.\n"); + viewer.addInstructionText("Press space to start free falling the box.\n"); + std::cout << viewer.getInstructions() << std::endl; + + // Set up the window to be 1360x768 (HD) + viewer.setUpViewInWindow(0, 0, 1360, 768); + + // Adjust the viewpoint of the Viewer + viewer.getCameraManipulator()->setHomePosition( + ::osg::Vec3(15.0f, 15.0f, 10.0f), + ::osg::Vec3(0.00f, 0.00f, 2.50f), + ::osg::Vec3(0, 0, 1.0f)); + + // 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 running the application loop + viewer.run(); + + return 0; +}