From e934d08d504ca24949343be0094c94ce9b2d4436 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 27 Aug 2020 08:35:05 -0700 Subject: [PATCH] Merge release-6.9 into master (#1488) Co-authored-by: Grey --- .ci/travis/docs_versions.txt | 2 +- CHANGELOG.md | 7 + dart/collision/CollisionResult.cpp | 2 +- dart/dynamics/Skeleton.cpp | 1 + dart/dynamics/Skeleton.hpp | 27 +++- dart/dynamics/detail/GenericJoint.hpp | 11 +- doxygen/Doxyfile.in | 4 +- unittests/comprehensive/test_Dynamics.cpp | 93 ++++++++++++ unittests/regression/CMakeLists.txt | 2 + unittests/regression/test_Issue1445.cpp | 166 ++++++++++++++++++++++ 10 files changed, 306 insertions(+), 9 deletions(-) create mode 100644 unittests/regression/test_Issue1445.cpp diff --git a/.ci/travis/docs_versions.txt b/.ci/travis/docs_versions.txt index 1919d1944a5ab..b5bc5d09eeeff 100644 --- a/.ci/travis/docs_versions.txt +++ b/.ci/travis/docs_versions.txt @@ -1,5 +1,5 @@ DART 6 -v6.9.2 +v6.9.3 v6.8.5 v6.7.3 v6.6.2 diff --git a/CHANGELOG.md b/CHANGELOG.md index 3c60e06dde719..fc7c125e889c4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -83,6 +83,13 @@ * Updated tutorial documentation and code to reflect new APIs: [#1481](https://github.com/dartsim/dart/pull/1481) +### [DART 6.9.3 (2020-08-26)](https://github.com/dartsim/dart/milestone/61?closed=1) + +* Dynamics + + * Changed to update the Properties version of a BodyNode when moved to a new Skeleton: [#1445](https://github.com/dartsim/dart/pull/1445) + * Fixed incorrect implicit joint damping/spring force computation in inverse dynamics: [#1451](https://github.com/dartsim/dart/pull/1451) + ### [DART 6.9.2 (2019-08-16)](https://github.com/dartsim/dart/milestone/60?closed=1) * Dynamics diff --git a/dart/collision/CollisionResult.cpp b/dart/collision/CollisionResult.cpp index b725810e02c8c..d29168f7df196 100644 --- a/dart/collision/CollisionResult.cpp +++ b/dart/collision/CollisionResult.cpp @@ -129,7 +129,7 @@ void CollisionResult::addObject(CollisionObject* object) { dterr << "[CollisionResult::addObject] Attempting to add a collision with " << "a nullptr object to a CollisionResult instance. This is not " - << "allowed. Please report this as a bug!"; + << "allowed. Please report this as a bug!\n"; assert(false); return; } diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 48e9f52e9a58d..3208ca5ced212 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2265,6 +2265,7 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) } #endif // ------- Debug mode + _newBodyNode->incrementVersion(); _newBodyNode->mStructuralChangeSignal.raise(_newBodyNode); } diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index 3d97d3326a0c7..34052154ebe42 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -646,7 +646,32 @@ class Skeleton : public virtual common::VersionCounter, /// Compute forward dynamics void computeForwardDynamics(); - /// Compute inverse dynamics + /// Computes inverse dynamics. + /// + /// The inverse dynamics is computed according to the following equations of + /// motion: + /// + /// \f$ M(q) \ddot{q} + C(q, \dot{q}) + N(q) - \tau_{\text{ext}} - \tau_d - + /// \tau_s = \tau \f$ + /// + /// where \f$ \tau_{\text{ext}} \f$, \f$ \tau_d \f$, and \f$ \tau_s \f$ are + /// external forces, joint damping forces, and joint spring forces projected + /// on to the joint space, respectively. This function provides three flags + /// whether to take into account each forces in computing the joint forces, + /// \f$ \tau_d \f$. Not accounting each forces implies that the forces is + /// added to \f$ \tau \f$ in order to keep the equation equivalent. If there + /// forces are zero (by setting external forces, damping coeff, spring + /// stiffness zeros), these flags have no effect. + /// + /// Once this function is called, the joint forces, \f$ \tau \f$, can be + /// obtained by calling getForces(). + /// + /// \param[in] _withExternalForces Set \c true to take external forces into + /// account. + /// \param[in] _withDampingForces Set \c true to take damping forces into + /// account. + /// \param[in] _withSpringForces Set \c true to take spring forces into + /// account. void computeInverseDynamics( bool _withExternalForces = false, bool _withDampingForces = false, diff --git a/dart/dynamics/detail/GenericJoint.hpp b/dart/dynamics/detail/GenericJoint.hpp index 8128c2f4da82c..5afd0c2c9b5f1 100644 --- a/dart/dynamics/detail/GenericJoint.hpp +++ b/dart/dynamics/detail/GenericJoint.hpp @@ -2324,22 +2324,25 @@ void GenericJoint::updateForceID( this->mAspectState.mForces = getRelativeJacobianStatic().transpose() * bodyForce; - // Damping force + // Implicit damping force: + // tau_d = -Kd * dq - Kd * h * ddq if (withDampingForces) { const typename ConfigSpaceT::Vector dampingForces = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct( - getVelocitiesStatic()); + getVelocitiesStatic() + getAccelerationsStatic() * timeStep); this->mAspectState.mForces -= dampingForces; } - // Spring force + // Implicit spring force: + // tau_s = -Kp * (q - q0) - Kp * h * dq - Kp * h^2 * ddq if (withSpringForces) { const typename ConfigSpaceT::Vector springForces = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct( getPositionsStatic() - Base::mAspectProperties.mRestPositions - + getVelocitiesStatic() * timeStep); + + getVelocitiesStatic() * timeStep + + getAccelerationsStatic() * timeStep * timeStep); this->mAspectState.mForces -= springForces; } } diff --git a/doxygen/Doxyfile.in b/doxygen/Doxyfile.in index e786f258ce81d..fa9a7c299c006 100644 --- a/doxygen/Doxyfile.in +++ b/doxygen/Doxyfile.in @@ -1472,7 +1472,7 @@ FORMULA_TRANSPARENT = YES # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. -USE_MATHJAX = NO +USE_MATHJAX = YES # When MathJax is enabled you can set the default output format to be used for # the MathJax output. See the MathJax site (see: @@ -1502,7 +1502,7 @@ MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest # MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols # This tag requires that the tag USE_MATHJAX is set to YES. -MATHJAX_EXTENSIONS = +MATHJAX_EXTENSIONS = TeX/AMSmath # The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces # of code that will be used on startup of the MathJax code. See the MathJax site diff --git a/unittests/comprehensive/test_Dynamics.cpp b/unittests/comprehensive/test_Dynamics.cpp index d398ab21e91b0..2089d5789418b 100644 --- a/unittests/comprehensive/test_Dynamics.cpp +++ b/unittests/comprehensive/test_Dynamics.cpp @@ -91,6 +91,9 @@ class DynamicsTest : public ::testing::Test // transformations, spatial velocities, and spatial accelerations correctly. void testForwardKinematics(const common::Uri& uri); + // Test inverse dynamics with various joint forces. + void testInverseDynamics(const common::Uri& uri); + // Compare dynamics terms in equations of motion such as mass matrix, mass // inverse matrix, Coriolis force vector, gravity force vector, and external // force vector. @@ -1529,6 +1532,84 @@ void DynamicsTest::testForwardKinematics(const common::Uri& uri) } } +//============================================================================== +void DynamicsTest::testInverseDynamics(const common::Uri& uri) +{ + //---------------------------- Settings -------------------------------------- + // Number of random state tests for each skeleton +#ifndef NDEBUG // Debug mode + const std::size_t nRandomItr = 2; +#else + const std::size_t nRandomItr = 100; +#endif + + // Lower and upper bound of configuration for system + const double lb = -1.0 * math::constantsd::pi(); + const double ub = 1.0 * math::constantsd::pi(); + + // Lower and upper bound of joint damping and stiffness + const double lbD = 0.0; + const double ubD = 10.0; + const double lbK = 0.0; + const double ubK = 10.0; + + // Lower and upper bound of joint forces + const double lbF = -10.0; + const double ubF = 10.0; + + simulation::WorldPtr world = utils::SkelParser::readWorld(uri); + ASSERT_TRUE(world != nullptr); + + for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) + { + dynamics::SkeletonPtr skel = world->getSkeleton(i); + const std::size_t dofs = skel->getNumDofs(); + + for (std::size_t j = 0; j < nRandomItr; ++j) + { + // Random joint stiffness and damping coefficients + for (std::size_t l = 0; l < dofs; ++l) + { + dynamics::DegreeOfFreedom* dof = skel->getDof(l); + dof->setDampingCoefficient(math::Random::uniform(lbD, ubD)); + dof->setSpringStiffness(math::Random::uniform(lbK, ubK)); + + double lbRP = dof->getPositionLowerLimit(); + double ubRP = dof->getPositionUpperLimit(); + lbRP = std::max(lbRP, -math::constantsd::pi()); + ubRP = std::min(ubRP, +math::constantsd::pi()); + dof->setRestPosition(math::Random::uniform(lbRP, ubRP)); + } + + // Set random states and forces + const VectorXd q0 = math::Random::uniform(dofs, lb, ub); + const VectorXd dq0 = math::Random::uniform(dofs, lb, ub); + const VectorXd f0 = math::Random::uniform(dofs, lbF, ubF); + + // Forward dynamics to compute accelerations + skel->setPositions(q0); + skel->setVelocities(dq0); + skel->setForces(f0); + skel->computeForwardDynamics(); + const VectorXd ddq = skel->getAccelerations(); + + // Test inverse dynamics to ensure the result joint forces are identical + // to the forces used to compute the input accelerations + skel->setAccelerations(ddq); + skel->computeInverseDynamics(false, true, true); + const VectorXd f = skel->getForces(); + EXPECT_TRUE(equals(f, f0)); + + // Test forward dynamics to ensure the result joint accelerations are + // identical to the accelerations used to compute the input forces + skel->setForces(f); + skel->computeForwardDynamics(); + const VectorXd ddq2 = skel->getAccelerations(); + EXPECT_TRUE(equals(ddq, ddq2)); + } + } +} + //============================================================================== void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri) { @@ -2329,6 +2410,18 @@ TEST_F(DynamicsTest, testForwardKinematics) } } +//============================================================================== +TEST_F(DynamicsTest, testInverseDynamics) +{ + for (std::size_t i = 0; i < getList().size(); ++i) + { +#ifndef NDEBUG + dtdbg << getList()[i].toString() << std::endl; +#endif + testInverseDynamics(getList()[i]); + } +} + //============================================================================== TEST_F(DynamicsTest, compareEquationsOfMotion) { diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index 51bf3f7ec47f4..8b49e431b0f0e 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..b6072fd113a85 --- /dev/null +++ b/unittests/regression/test_Issue1445.cpp @@ -0,0 +1,166 @@ +/* + * 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); +}