|
| 1 | +/* |
| 2 | + * Copyright (c) 2011-2025, The DART development contributors |
| 3 | + * All rights reserved. |
| 4 | + * |
| 5 | + * The list of contributors can be found at: |
| 6 | + * https://github.com/dartsim/dart/blob/main/LICENSE |
| 7 | + * |
| 8 | + * This file is provided under the following "BSD-style" License: |
| 9 | + * Redistribution and use in source and binary forms, with or |
| 10 | + * without modification, are permitted provided that the following |
| 11 | + * conditions are met: |
| 12 | + * * Redistributions of source code must retain the above copyright |
| 13 | + * notice, this list of conditions and the following disclaimer. |
| 14 | + * * Redistributions in binary form must reproduce the above |
| 15 | + * copyright notice, this list of conditions and the following |
| 16 | + * disclaimer in the documentation and/or other materials provided |
| 17 | + * with the distribution. |
| 18 | + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND |
| 19 | + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, |
| 20 | + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| 21 | + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 22 | + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR |
| 23 | + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
| 24 | + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
| 25 | + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF |
| 26 | + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
| 27 | + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 28 | + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 29 | + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 30 | + * POSSIBILITY OF SUCH DAMAGE. |
| 31 | + */ |
| 32 | + |
| 33 | +#include "dart/dynamics/dynamics.hpp" |
| 34 | +#include "dart/math/math.hpp" |
| 35 | +#include "dart/simulation/simulation.hpp" |
| 36 | +#include "dart/utils/utils.hpp" |
| 37 | + |
| 38 | +#include <gtest/gtest.h> |
| 39 | + |
| 40 | +using namespace dart; |
| 41 | +using namespace dynamics; |
| 42 | +using namespace math; |
| 43 | +using namespace simulation; |
| 44 | +using namespace utils; |
| 45 | + |
| 46 | +// Regression test for https://github.com/dartsim/dart/issues/1899. |
| 47 | +// |
| 48 | +// This reproduces the scenario provided in the attachment |
| 49 | +// "Test_code_and_Reproduction_File.tar.gz" where a non-zero command is |
| 50 | +// applied to a PASSIVE joint while stepping the world without resetting joint |
| 51 | +// commands. Prior to the fix, ODE would assert when solving the constraints. |
| 52 | +TEST(Issue1899, CollisionOfPrescribedJointsReproduction) |
| 53 | +{ |
| 54 | + const double tol = 1e-9; |
| 55 | + const double timeStep = 1e-3; |
| 56 | + const std::size_t numFrames = 5; |
| 57 | + |
| 58 | + auto world = SkelParser::readWorld( |
| 59 | + "dart://sample/skel/test/collision_of_prescribed_joints_test.skel"); |
| 60 | + ASSERT_NE(world, nullptr); |
| 61 | + world->setTimeStep(timeStep); |
| 62 | + ASSERT_NEAR(world->getTimeStep(), timeStep, tol); |
| 63 | + |
| 64 | + auto skel1 = world->getSkeleton("skeleton 1"); |
| 65 | + auto skel2 = world->getSkeleton("skeleton 2"); |
| 66 | + auto skel3 = world->getSkeleton("skeleton 3"); |
| 67 | + auto skel4 = world->getSkeleton("skeleton 4"); |
| 68 | + auto skel5 = world->getSkeleton("skeleton 5"); |
| 69 | + auto skel6 = world->getSkeleton("skeleton 6"); |
| 70 | + ASSERT_NE(skel1, nullptr); |
| 71 | + ASSERT_NE(skel2, nullptr); |
| 72 | + ASSERT_NE(skel3, nullptr); |
| 73 | + ASSERT_NE(skel4, nullptr); |
| 74 | + ASSERT_NE(skel5, nullptr); |
| 75 | + ASSERT_NE(skel6, nullptr); |
| 76 | + |
| 77 | + Joint* joint1 = skel1->getJoint(0); |
| 78 | + Joint* joint2 = skel2->getJoint(0); |
| 79 | + Joint* joint3 = skel3->getJoint(0); |
| 80 | + Joint* joint4 = skel4->getJoint(0); |
| 81 | + Joint* joint5 = skel5->getJoint(0); |
| 82 | + Joint* joint6 = skel6->getJoint(0); |
| 83 | + ASSERT_NE(joint1, nullptr); |
| 84 | + ASSERT_NE(joint2, nullptr); |
| 85 | + ASSERT_NE(joint3, nullptr); |
| 86 | + ASSERT_NE(joint4, nullptr); |
| 87 | + ASSERT_NE(joint5, nullptr); |
| 88 | + ASSERT_NE(joint6, nullptr); |
| 89 | + ASSERT_EQ(joint1->getActuatorType(), Joint::FORCE); |
| 90 | + ASSERT_EQ(joint2->getActuatorType(), Joint::PASSIVE); |
| 91 | + ASSERT_EQ(joint3->getActuatorType(), Joint::SERVO); |
| 92 | + ASSERT_EQ(joint4->getActuatorType(), Joint::ACCELERATION); |
| 93 | + ASSERT_EQ(joint5->getActuatorType(), Joint::VELOCITY); |
| 94 | + ASSERT_EQ(joint6->getActuatorType(), Joint::LOCKED); |
| 95 | + |
| 96 | + for (std::size_t i = 0; i < numFrames; ++i) { |
| 97 | + const double time = world->getTime(); |
| 98 | + |
| 99 | + joint1->setCommand(0, -0.5 * constantsd::pi() * std::cos(time)); |
| 100 | + joint2->setCommand(0, -0.5 * constantsd::pi() * std::cos(time)); |
| 101 | + joint3->setCommand(0, -0.5 * constantsd::pi() * std::cos(time)); |
| 102 | + joint4->setCommand(0, -0.5 * constantsd::pi() * std::cos(time)); |
| 103 | + joint5->setCommand(0, -0.5 * constantsd::pi() * std::sin(time)); |
| 104 | + joint6->setCommand(0, -0.5 * constantsd::pi() * std::sin(time)); |
| 105 | + |
| 106 | + // Do not reset joint commands after stepping. |
| 107 | + world->step(false); |
| 108 | + |
| 109 | + EXPECT_TRUE(joint1->isDynamic()); |
| 110 | + EXPECT_TRUE(joint2->isDynamic()); |
| 111 | + EXPECT_TRUE(joint3->isDynamic()); |
| 112 | + |
| 113 | + EXPECT_TRUE(joint4->isKinematic()); |
| 114 | + EXPECT_NEAR(joint4->getAcceleration(0), joint4->getCommand(0), tol); |
| 115 | + EXPECT_TRUE(joint5->isKinematic()); |
| 116 | + EXPECT_NEAR(joint5->getVelocity(0), joint5->getCommand(0), tol); |
| 117 | + |
| 118 | + // The PASSIVE joint's command should have been cleared. |
| 119 | + EXPECT_EQ(joint2->getCommand(0), 0.0); |
| 120 | + |
| 121 | + EXPECT_TRUE(joint6->isKinematic()); |
| 122 | + EXPECT_NEAR(joint6->getVelocity(0), 0.0, tol); |
| 123 | + EXPECT_NEAR(joint6->getAcceleration(0), 0.0, tol); |
| 124 | + } |
| 125 | +} |
0 commit comments