diff --git a/pixi.toml b/pixi.toml index 8c6fa4dac411a..07c625269c2cf 100644 --- a/pixi.toml +++ b/pixi.toml @@ -13,9 +13,18 @@ install-local = { cmd = "cmake --install build --prefix $CONDA_PREFIX", depends_ "configure_local", "build", ] } -example-atlas-puppet = { cmd = "cmake --build build --target atlas_puppet --parallel && ./build/bin/atlas_puppet", depends_on = ["configure"] } -example-atlas-simbicon = { cmd = "cmake --build build --target atlas_simbicon --parallel && ./build/bin/atlas_simbicon", depends_on = ["configure"] } -bm-empty = { cmd = "cmake --build build --target BM_INTEGRATION_empty --parallel && ./build/bin/BM_INTEGRATION_empty", depends_on = ["configure"] } +example-atlas-puppet = { cmd = "cmake --build build --target atlas_puppet --parallel && ./build/bin/atlas_puppet", depends_on = [ + "configure", +] } +example-atlas-simbicon = { cmd = "cmake --build build --target atlas_simbicon --parallel && ./build/bin/atlas_simbicon", depends_on = [ + "configure", +] } +bm-empty = { cmd = "cmake --build build --target BM_INTEGRATION_empty --parallel && ./build/bin/BM_INTEGRATION_empty", depends_on = [ + "configure", +] } +bm-kinematics = { cmd = "cmake --build build --target BM_INTEGRATION_kinematics --parallel && ./build/bin/BM_INTEGRATION_kinematics", depends_on = [ + "configure", +] } [dependencies] assimp = ">=5.3.1,<5.4" diff --git a/tests/benchmark/integration/CMakeLists.txt b/tests/benchmark/integration/CMakeLists.txt index d8172f203ef43..c1e0cb2198e24 100644 --- a/tests/benchmark/integration/CMakeLists.txt +++ b/tests/benchmark/integration/CMakeLists.txt @@ -30,5 +30,18 @@ dart_benchmarks( TYPE BM_INTEGRATION - SOURCES bm_empty.cpp -) \ No newline at end of file + SOURCES + bm_empty.cpp +) + +if(TARGET dart-utils) + +dart_benchmarks( + TYPE BM_INTEGRATION + SOURCES + bm_kinematics.cpp + LINK_LIBRARIES + dart-utils +) + +endif() diff --git a/tests/benchmark/integration/bm_kinematics.cpp b/tests/benchmark/integration/bm_kinematics.cpp new file mode 100644 index 0000000000000..3adc85251ec6f --- /dev/null +++ b/tests/benchmark/integration/bm_kinematics.cpp @@ -0,0 +1,188 @@ +/* + * 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 + +#include + +#include + +#include +#include + +using namespace dart; + +std::vector getSceneFiles() +{ + std::vector scenes; + scenes.push_back("dart://sample/skel/test/chainwhipa.skel"); + scenes.push_back("dart://sample/skel/test/single_pendulum.skel"); + scenes.push_back("dart://sample/skel/test/single_pendulum_euler_joint.skel"); + scenes.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel"); + scenes.push_back("dart://sample/skel/test/double_pendulum.skel"); + scenes.push_back("dart://sample/skel/test/double_pendulum_euler_joint.skel"); + scenes.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel"); + scenes.push_back("dart://sample/skel/test/serial_chain_revolute_joint.skel"); + scenes.push_back("dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"); + scenes.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel"); + scenes.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel"); + scenes.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel"); + scenes.push_back("dart://sample/skel/test/simple_tree_structure.skel"); + scenes.push_back( + "dart://sample/skel/test/simple_tree_structure_euler_joint.skel"); + scenes.push_back( + "dart://sample/skel/test/simple_tree_structure_ball_joint.skel"); + scenes.push_back("dart://sample/skel/test/tree_structure.skel"); + scenes.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel"); + scenes.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel"); + scenes.push_back("dart://sample/skel/fullbody1.skel"); + + return scenes; +} + +std::vector getWorlds() +{ + std::vector sceneFiles = getSceneFiles(); + std::vector worlds; + for (std::size_t i = 0; i < sceneFiles.size(); ++i) + worlds.push_back(dart::utils::SkelParser::readWorld(sceneFiles[i])); + + return worlds; +} + +void testForwardKinematicSpeed( + dart::dynamics::SkeletonPtr skel, + bool position, + bool velocity, + bool acceleration, + std::size_t numTests) +{ + if (nullptr == skel) + return; + + dart::dynamics::BodyNode* bn = skel->getBodyNode(0); + while (bn->getNumChildBodyNodes() > 0) + bn = bn->getChildBodyNode(0); + + for (std::size_t i = 0; i < numTests; ++i) { + for (std::size_t i = 0; i < skel->getNumDofs(); ++i) { + dart::dynamics::DegreeOfFreedom* dof = skel->getDof(i); + dof->setPosition(dart::math::Random::uniform( + std::max(dof->getPositionLowerLimit(), -1.0), + std::min(dof->getPositionUpperLimit(), 1.0))); + } + + for (std::size_t i = 0; i < skel->getNumBodyNodes(); ++i) { + if (position) + skel->getBodyNode(i)->getWorldTransform(); + if (velocity) { + skel->getBodyNode(i)->getSpatialVelocity(); + skel->getBodyNode(i)->getPartialAcceleration(); + } + if (acceleration) + skel->getBodyNode(i)->getSpatialAcceleration(); + } + } +} + +void runKinematicsTest( + const std::vector& worlds, + bool position, + bool velocity, + bool acceleration, + std::size_t numTests) +{ + // Test for updating the whole skeleton + for (std::size_t i = 0; i < worlds.size(); ++i) { + dart::simulation::WorldPtr world = worlds[i]; + testForwardKinematicSpeed( + world->getSkeleton(0), position, velocity, acceleration, numTests); + } +} + +static void BM_Kinematics(benchmark::State& state) +{ + std::vector worlds = getWorlds(); + + // Get the input value to be passed to the Kinematics function + int n = state.range(0); + + // Call the Kinematics function and measure the time it takes + for (auto _ : state) { + runKinematicsTest(worlds, true, true, true, n); + } +} + +BENCHMARK(BM_Kinematics)->Arg(1)->Arg(10)->Arg(100)->Arg(1000)->Arg(10000); + +void testDynamicsSpeed( + dart::simulation::WorldPtr world, std::size_t numIterations) +{ + if (nullptr == world) + return; + + world->eachSkeleton([](dart::dynamics::Skeleton* skel) { + skel->resetPositions(); + skel->resetVelocities(); + skel->resetAccelerations(); + }); + + for (std::size_t i = 0; i < numIterations; ++i) + world->step(); +} + +void runDynamicsTest( + const std::vector& worlds, + std::size_t numIterations) +{ + for (std::size_t i = 0; i < worlds.size(); ++i) + testDynamicsSpeed(worlds[i], numIterations); +} + +static void BM_Dynamics(benchmark::State& state) +{ + std::vector worlds = getWorlds(); + + // Get the input value to be passed to the Kinematics function + int n = state.range(0); + + // Call the Kinematics function and measure the time it takes + for (auto _ : state) { + runDynamicsTest(worlds, n); + } +} + +BENCHMARK(BM_Dynamics)->Arg(1)->Arg(10)->Arg(100)->Arg(1000)->Arg(10000);