From 701ef61d83d66aca9911bb0d86a35939c1bc1549 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 19 Feb 2019 15:13:44 +0800 Subject: [PATCH 1/4] Patch for issue #1243 and add a regression test --- dart/common/detail/Cloneable.hpp | 2 + dart/common/detail/ProxyAspect.hpp | 8 +- unittests/regression/CMakeLists.txt | 2 + unittests/regression/test_Issue1243.cpp | 117 ++++++++++++++++++++++++ 4 files changed, 125 insertions(+), 4 deletions(-) create mode 100644 unittests/regression/test_Issue1243.cpp diff --git a/dart/common/detail/Cloneable.hpp b/dart/common/detail/Cloneable.hpp index 12d06b96a7531..5e9faf1c80ae5 100644 --- a/dart/common/detail/Cloneable.hpp +++ b/dart/common/detail/Cloneable.hpp @@ -235,6 +235,7 @@ template ::operator =( const ProxyCloneable& other) -> ProxyCloneable& { + mOwner = other.mOwner; set(other); return *this; } @@ -246,6 +247,7 @@ template ::operator =( ProxyCloneable&& other) -> ProxyCloneable& { + mOwner = other.mOwner; set(other); return *this; } diff --git a/dart/common/detail/ProxyAspect.hpp b/dart/common/detail/ProxyAspect.hpp index 7e6f776236c8d..230de07d15c0e 100644 --- a/dart/common/detail/ProxyAspect.hpp +++ b/dart/common/detail/ProxyAspect.hpp @@ -92,7 +92,7 @@ class ProxyStateAspect : public BaseT if(owner && mProxyState.getOwner() != owner) { // Link the ProxyState to its new owner - mProxyState = State(owner); + mProxyState = State(owner, mProxyState.get()); } } @@ -153,13 +153,13 @@ class ProxyPropertiesAspect : public BaseT void setComposite(Composite* newComposite) override { Base::setComposite(newComposite); - typename Properties::Owner* comp = + typename Properties::Owner* owner = dynamic_cast(newComposite); - if(comp && mProxyProperties.getOwner() != comp) + if(owner && mProxyProperties.getOwner() != owner) { // Link the ProxyProperties to its new owner - mProxyProperties = Properties(comp); + mProxyProperties = Properties(owner, mProxyProperties.get()); } } diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index 366a47fb46b63..96a499454add8 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -1,5 +1,7 @@ dart_add_test("regression" test_Issue000Template test_Issue000Template.cpp) +dart_add_test("regression" test_Issue1243 test_Issue1243.cpp) + if(TARGET dart-utils-urdf) dart_add_test("regression" test_Issue838) diff --git a/unittests/regression/test_Issue1243.cpp b/unittests/regression/test_Issue1243.cpp new file mode 100644 index 0000000000000..7962ee5e9bcce --- /dev/null +++ b/unittests/regression/test_Issue1243.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (c) 2011-2019, 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 + +dart::dynamics::SkeletonPtr create_box(const Eigen::Vector3d& dims, double mass, const Eigen::Vector4d& color, const std::string& box_name) +{ + dart::dynamics::SkeletonPtr box_skel = dart::dynamics::Skeleton::create(box_name); + + // Give the box a body + dart::dynamics::BodyNodePtr body; + body = box_skel->createJointAndBodyNodePair(nullptr).second; + body->setName(box_name); + + // Give the body a shape + auto box = std::make_shared(dims); + auto box_node = body->createShapeNodeWith(box); + box_node->getVisualAspect()->setColor(color); + // Set up inertia + dart::dynamics::Inertia inertia; + inertia.setMass(mass); + inertia.setMoment(box->computeInertia(mass)); + body->setInertia(inertia); + + return box_skel; +} + +dart::dynamics::SkeletonPtr create_floor() +{ + double floor_width = 10.; + double floor_height = 0.1; + + dart::dynamics::SkeletonPtr floor_skel = dart::dynamics::Skeleton::create("floor"); + + // Give the floor a body + dart::dynamics::BodyNodePtr body = floor_skel->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + auto box = std::make_shared(Eigen::Vector3d(floor_width, floor_width, floor_height)); + auto box_node = body->createShapeNodeWith(box); + box_node->getVisualAspect()->setColor(dart::Color::Gray()); + + // Put the body into position + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation()[2] -= floor_height / 2.0; + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor_skel; +} + +//============================================================================== +TEST(Issue1243, State) +{ + auto world = std::make_shared(); + + auto box_skel = create_box({0.1, 0.1, 0.1}, 1., {1., 0., 0., 1.}, "box"); + auto floor_skel = create_floor(); + + box_skel->setPosition(5, 0.2); + + world->addSkeleton(box_skel); + world->addSkeleton(floor_skel); + + dart::dynamics::Skeleton::State bookmark_state; + Eigen::Isometry3d bookmark_tf; + for (size_t i = 0; i < 20; i++) + { + if(i==10) + { + bookmark_state = box_skel->getState(); + bookmark_tf = box_skel->getRootBodyNode()->getTransform(); + } + world->step(); + } + + const Eigen::Isometry3d final_tf = + box_skel->getRootBodyNode()->getTransform(); + + box_skel->setState(bookmark_state); + const Eigen::Isometry3d rewind_tf = + box_skel->getRootBodyNode()->getTransform(); + + EXPECT_FALSE(equals(bookmark_tf, final_tf)); + EXPECT_TRUE(equals(bookmark_tf, rewind_tf)); +} From 1c59ec710aa0c861acd0b9323069ad35bfa71bb1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 19 Feb 2019 15:18:40 +0800 Subject: [PATCH 2/4] Update CHANGELOG --- CHANGELOG.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5fb14b54bee53..fe91d34be8c55 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,13 @@ ## DART 6 +### [DART 6.7.3 (2019-XX-XX)] + +### Changes + +* Dynamics + + * Fixed Skeleton::setState(): [#1245](https://github.com/dartsim/dart/pull/1245) + ### [DART 6.7.2 (2019-01-17)](https://github.com/dartsim/dart/milestone/50?closed=1) ### Changes From 063f8ba526ea5243fd381fb9bfe7660618fa3910 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 19 Feb 2019 20:13:03 -0800 Subject: [PATCH 3/4] Bump version to 6.7.3 --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d3754c2de0d02..8ed2ad762a7b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,7 +57,7 @@ endif() # If you change the version, please update the tag in package.xml. set(DART_MAJOR_VERSION "6") set(DART_MINOR_VERSION "7") -set(DART_PATCH_VERSION "2") +set(DART_PATCH_VERSION "3") set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}") set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.") set(DART_PKG_EXTERNAL_DEPS "eigen, ccd, fcl, assimp, boost") diff --git a/package.xml b/package.xml index 03a7dcfe65c10..938b3ce28edb0 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ a Catkin workspace. Catkin is not required to build DART. For more information, see: http://ros.org/reps/rep-0136.html --> dartsim - 6.7.2 + 6.7.3 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics From c3a8e618ecd06d741fd50b0e784e51871ef9abfe Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 19 Feb 2019 20:13:15 -0800 Subject: [PATCH 4/4] Update changelog --- CHANGELOG.md | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fe91d34be8c55..28a1eac56b2cc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,6 @@ ## DART 6 -### [DART 6.7.3 (2019-XX-XX)] +### [DART 6.7.3 (2019-02-19)](https://github.com/dartsim/dart/milestone/51?closed=1) ### Changes @@ -8,6 +8,20 @@ * Fixed Skeleton::setState(): [#1245](https://github.com/dartsim/dart/pull/1245) +#### Compilers Tested + +* Linux + + * GCC (C++11): 5.4.0, 7.3.0, 8.2.0 + +* Linux (32-bit) + + * GCC (C++11): 5.4.0 + +* macOS + + * AppleClang (C++11): 9.1.0 + ### [DART 6.7.2 (2019-01-17)](https://github.com/dartsim/dart/milestone/50?closed=1) ### Changes