diff --git a/CHANGELOG.md b/CHANGELOG.md index 3eb7e46775b0d..a643712cc1fb0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,11 @@ ## DART 6 +### [DART 6.9.3 XXXXX] + +* Dynamics + + * Update the Properties version of a BodyNode when moved to a new Skeleton: [#1445](https://github.com/dartsim/dart/pull/1445) + ### [DART 6.9.2 (2019-08-16)](https://github.com/dartsim/dart/milestone/60?closed=1) * Dynamics diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 055e02d5aafd8..b6abceb4f9ca2 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2208,6 +2208,7 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) } #endif // ------- Debug mode + _newBodyNode->incrementVersion(); _newBodyNode->mStructuralChangeSignal.raise(_newBodyNode); } diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index b2cb5f82013e7..5658512ea9424 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -15,6 +15,8 @@ if(TARGET dart-utils-urdf) dart_add_test("regression" test_Issue1231) + dart_add_test("regression" test_Issue1445) + endif() if(TARGET dart-collision-bullet) diff --git a/unittests/regression/test_Issue1445.cpp b/unittests/regression/test_Issue1445.cpp new file mode 100644 index 0000000000000..969cfea76d1f6 --- /dev/null +++ b/unittests/regression/test_Issue1445.cpp @@ -0,0 +1,171 @@ +/* + * Copyright (c) 2011-2020, 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 + +// This test is adapted from @azeey's work here: +// https://github.com/ignitionrobotics/ign-physics/pull/31 +TEST(Issue1445, Collision) +{ + std::string model1Str = R"( + + + 0 0 0.1 0 0 0 + + + + + 0.2 0.2 0.2 + + + + + + )"; + + std::string model2Str = R"( + + + 1 0 0.1 0 0 0 + + + + + 0.1 + + + + + + )"; + + auto world = std::make_shared(); + + { + auto ground = dart::dynamics::Skeleton::create("ground"); + auto* bn = + ground->createJointAndBodyNodePair().second; + bn->createShapeNodeWith< + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>( + std::make_shared( + Eigen::Vector3d::UnitZ(), 0.0)); + + world->addSkeleton(ground); + } + + auto model1 = dart::dynamics::Skeleton::create("M1"); + { + auto pair = model1->createJointAndBodyNodePair(); + auto* joint = pair.first; + auto* bn = pair.second; + joint->setName("joint1"); + bn->setName("body1"); + + bn->createShapeNodeWith< + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>( + std::make_shared( + Eigen::Vector3d(0.2, 0.2, 0.2))); + + auto tf = Eigen::Isometry3d::Identity(); + tf.translation()[2] = 0.1; + joint->setTransform(tf); + + world->addSkeleton(model1); + } + + world->step(); + + auto model2 = dart::dynamics::Skeleton::create("M2"); + { + auto pair = model2->createJointAndBodyNodePair(); + auto* joint = pair.first; + auto* bn = pair.second; + joint->setName("joint2"); + bn->setName("body2"); + + bn->createShapeNodeWith< + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>( + std::make_shared( + 0.1)); + + auto tf = Eigen::Isometry3d::Identity(); + tf.translation() = Eigen::Vector3d(1.0, 0.0, 0.1); + joint->setTransform(tf); + } + + auto* model1Body = model1->getRootBodyNode(); + auto* model2Body = model2->getRootBodyNode(); + + const auto poseParent = model1Body->getTransform(); + const auto poseChild = model2Body->getTransform(); + + // Commenting out the following `step` call makes this test fail + // world->Step(output, state, input); + auto fixedJoint = model2Body->moveTo(model1Body); + + // Pose of child relative to parent + auto poseParentChild = poseParent.inverse() * poseChild; + + // We let the joint be at the origin of the child link. + fixedJoint->setTransformFromParentBodyNode(poseParentChild); + + const std::size_t numSteps = 100; + + for (std::size_t i = 0; i < numSteps; ++i) + world->step(); + + // Expect both bodies to hit the ground and stop + EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3); + EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3); + + auto temp = dart::dynamics::Skeleton::create("temp"); + world->addSkeleton(temp); + model2Body->moveTo(temp, nullptr); + + for (std::size_t i = 0; i < numSteps; ++i) + world->step(); + + // Expect both bodies to remain in contact with the ground with zero velocity. + EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3); + EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3); +} + + + +