Increment body node version after it has been moved to a new skeleton (…

…#1445)

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

dart/dynamics/Skeleton.cpp

@@ -2208,6 +2208,7 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode)
   }
 #endif // ------- Debug mode
 
+  _newBodyNode->incrementVersion();
   _newBodyNode->mStructuralChangeSignal.raise(_newBodyNode);
 }
 

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)

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 <TestHelpers.hpp>
+#include <gtest/gtest.h>
+
+#include <dart/dart.hpp>
+#include <dart/utils/sdf/sdf.hpp>
+
+// This test is adapted from @azeey's work here:
+// https://github.com/ignitionrobotics/ign-physics/pull/31
+TEST(Issue1445, Collision)
+{
+  std::string model1Str = R"(
+  <sdf version="1.6">
+    <model name="M1">
+      <pose>0 0 0.1 0 0 0</pose>
+      <link name="body">
+        <collision name="coll_box">
+          <geometry>
+            <box>
+              <size>0.2 0.2 0.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </sdf>)";
+
+  std::string model2Str = R"(
+  <sdf version="1.6">
+    <model name="M2">
+      <pose>1 0 0.1 0 0 0</pose>
+      <link name="chassis">
+        <collision name="coll_sphere">
+          <geometry>
+            <sphere>
+              <radius>0.1</radius>
+            </sphere>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </sdf>)";
+
+  auto world = std::make_shared<dart::simulation::World>();
+
+  {
+    auto ground = dart::dynamics::Skeleton::create("ground");
+    auto* bn =
+        ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>().second;
+    bn->createShapeNodeWith<
+        dart::dynamics::CollisionAspect,
+        dart::dynamics::DynamicsAspect>(
+          std::make_shared<dart::dynamics::PlaneShape>(
+            Eigen::Vector3d::UnitZ(), 0.0));
+
+    world->addSkeleton(ground);
+  }
+
+  auto model1 = dart::dynamics::Skeleton::create("M1");
+  {
+    auto pair = model1->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
+    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<dart::dynamics::BoxShape>(
+            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<dart::dynamics::FreeJoint>();
+    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<dart::dynamics::SphereShape>(
+            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<dart::dynamics::WeldJoint>(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<dart::dynamics::FreeJoint>(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);
+}
+
+
+
+