diff --git a/CHANGELOG.md b/CHANGELOG.md index eb445a625eb0e..091363e39df9f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,11 @@ ## DART 6 +### [DART 6.11.1 (TBD)](https://github.com/dartsim/dart/milestone/67?closed=1) + +* Dynamics + + * Fixed incorrect LCP construction in JointConstraint for multi-DOFs joints : [#1597](https://github.com/dartsim/dart/pull/1597) + ### [DART 6.11.0 (2021-07-15)](https://github.com/dartsim/dart/milestone/64?closed=1) * Math diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index f8e8260407c45..9551d8a4a186b 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -400,14 +400,33 @@ void JointConstraint::getInformation(ConstraintInfo* lcp) const int dof = static_cast(mJoint->getNumDofs()); for (int i = 0; i < dof; ++i) { - assert(lcp->w[index] == 0.0); + if (!mActive[i]) + continue; + +#ifndef NDEBUG // debug + if (std::abs(lcp->w[index]) > 1e-6) + { + dterr << "Invalid " << index + << "-th slack variable. Expected: 0.0. Actual: " + << lcp->w[index] << ".\n"; + assert(false); + } +#endif lcp->b[index] = mDesiredVelocityChange[i]; lcp->lo[index] = mImpulseLowerBound[i]; lcp->hi[index] = mImpulseUpperBound[i]; - assert(lcp->findex[index] == -1); +#ifndef NDEBUG // debug + if (lcp->findex[index] != -1) + { + dterr << "Invalid " << index + << "-th friction index. Expected: -1. Actual: " + << lcp->findex[index] << ".\n"; + assert(false); + } +#endif if (mLifeTime[i]) lcp->x[index] = mOldX[i]; diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 76dfe863d4a52..83c592356491b 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -626,7 +626,7 @@ std::pair createJointAndNodePair( parent, static_cast(*joint.properties), static_cast(*node.properties)); - else if (std::string("revolute2") == type) + else if (std::string("revolute2") == type || std::string("universal") == type) return skeleton->createJointAndBodyNodePair( parent, static_cast( @@ -1028,10 +1028,18 @@ void readVisualizationShapeNode( const common::Uri& baseUri, const common::ResourceRetrieverPtr& retriever) { + std::string visualName = "visual shape"; + if (hasAttribute(vizShapeNodeEle, "name")) { + visualName = getAttributeString(vizShapeNodeEle, "name"); + } else { + dtwarn << "Missing required attribute [name] in element of " + << ".\n"; + } + dynamics::ShapeNode* newShapeNode = readShapeNode( bodyNode, vizShapeNodeEle, - bodyNode->getName() + " - visual shape", + bodyNode->getName() + " - " + visualName, baseUri, retriever); @@ -1052,10 +1060,18 @@ void readCollisionShapeNode( const common::Uri& baseUri, const common::ResourceRetrieverPtr& retriever) { + std::string collName = "collision shape"; + if (hasAttribute(collShapeNodeEle, "name")) { + collName = getAttributeString(collShapeNodeEle, "name"); + } else { + dtwarn << "Missing required attribute [name] in element of " + << ".\n"; + } + dynamics::ShapeNode* newShapeNode = readShapeNode( bodyNode, collShapeNodeEle, - bodyNode->getName() + " - collision shape", + bodyNode->getName() + " - " + collName, baseUri, retriever); @@ -1224,23 +1240,42 @@ SDFJoint readJoint( = (childWorld * childToJoint).inverse() * _skeletonFrame; if (type == std::string("fixed")) + { newJoint.properties = dynamics::WeldJoint::Properties::createShared( readWeldJoint(_jointElement, parentModelFrame, name)); - if (type == std::string("prismatic")) + } + else if (type == std::string("prismatic")) + { newJoint.properties = dynamics::PrismaticJoint::Properties::createShared( readPrismaticJoint(_jointElement, parentModelFrame, name)); - if (type == std::string("revolute")) + } + else if (type == std::string("revolute")) + { newJoint.properties = dynamics::RevoluteJoint::Properties::createShared( readRevoluteJoint(_jointElement, parentModelFrame, name)); - if (type == std::string("screw")) + } + else if (type == std::string("screw")) + { newJoint.properties = dynamics::ScrewJoint::Properties::createShared( readScrewJoint(_jointElement, parentModelFrame, name)); - if (type == std::string("revolute2")) + } + else if (type == std::string("revolute2") || type == std::string("universal")) + { newJoint.properties = dynamics::UniversalJoint::Properties::createShared( readUniversalJoint(_jointElement, parentModelFrame, name)); - if (type == std::string("ball")) + } + else if (type == std::string("ball")) + { newJoint.properties = dynamics::BallJoint::Properties::createShared( readBallJoint(_jointElement, parentModelFrame, name)); + } + else + { + dterr << "Unsupported joint type [" << type + << "]. Using [fixed] joint type instead.\n"; + newJoint.properties = dynamics::WeldJoint::Properties::createShared( + readWeldJoint(_jointElement, parentModelFrame, name)); + } newJoint.type = type; diff --git a/data/sdf/test/test_issue1596.model b/data/sdf/test/test_issue1596.model new file mode 100644 index 0000000000000..ee7342057be34 --- /dev/null +++ b/data/sdf/test/test_issue1596.model @@ -0,0 +1,182 @@ + + + + true + 0 0 2 0 0 0 + + 0.1 + 0 0 -0.5 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 -0.5 0 0 0 + + + 0.10 0.20 1.0 + + + + + 0 0 -0.5 0 0 0 + + + 0.10 0.20 1.0 + + + + + + + + 0 0.125 -0.5 0 0 0 + + + 0.05 + + + + + + + + + true + 0 0 1.0 0 0 0 + + 0.1 + 0 0 -0.5 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 -0.5 0 0 0 + + + 0.10 0.20 1.0 + + + + + + 0.005 + + + + + 1.0 + 1.0 + + + + + + 0 0 -0.5 0 0 0 + + + 0.10 0.20 1.0 + + + + + + + + 0 0.125 -0.5 0 0 0 + + + 0.05 + + + + + + + + + + world + link_00 + + 1 0 0 + + -1.2 + 1.2 + 1e6 + + + 0.0001 + + + + 0 1 0 + + -1.2 + 1.2 + 1e6 + + + 0.0001 + + + + + 1 + + + + + link_01 + link_00 + + 1 0 0 + + -1.2 + 1.2 + + + 0.1 + + + + 0 1 0 + + -1.2 + 1.2 + + + 0.1 + + + + + 1 + + + + + diff --git a/package.xml b/package.xml index 05959c591e334..488c8e96c2af8 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.11.0 + 6.11.1 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index 9e4f21c58f05a..ace85bafc91c3 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -5,6 +5,9 @@ dart_add_test("regression" test_Issue1243 test_Issue1243.cpp) if(TARGET dart-utils) dart_add_test("regression" test_Issue1583) target_link_libraries(test_Issue1583 dart-utils) + + dart_add_test("regression" test_Issue1596) + target_link_libraries(test_Issue1596 dart-utils) endif() if(TARGET dart-utils-urdf) diff --git a/unittests/regression/test_Issue1596.cpp b/unittests/regression/test_Issue1596.cpp new file mode 100644 index 0000000000000..44654e809d470 --- /dev/null +++ b/unittests/regression/test_Issue1596.cpp @@ -0,0 +1,91 @@ +/* + * 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 +#include + +//======================================================================================== +TEST(Issue1596, ServoJointWithPositionLimits) +{ +#if NDEBUG // release + const auto num_steps = 50000; +#else + const auto num_steps = 1000; +#endif + auto skel = dart::utils::SdfParser::readSkeleton( + "dart://sample/sdf/test/test_issue1596.model"); + ASSERT_NE(skel, nullptr); + + auto world = dart::simulation::World::create(); + world->setGravity(Eigen::Vector3d(9.81, 9.81, 0)); + world->addSkeleton(skel); + ASSERT_EQ(world->getNumSkeletons(), 1); + + auto* joint0 = skel->getJoint("joint_00"); + auto* joint1 = skel->getJoint("joint_01"); + std::vector joints = {joint0, joint1}; + + for (auto joint : joints) + { + ASSERT_NE(joint, nullptr); + ASSERT_EQ( + joint->getType(), dart::dynamics::UniversalJoint::getStaticType()); + EXPECT_EQ(joint->getNumDofs(), 2); + joint->setLimitEnforcement(true); + + EXPECT_DOUBLE_EQ(0, joint->getPosition(0)); + EXPECT_DOUBLE_EQ(0, joint->getPosition(1)); + EXPECT_DOUBLE_EQ(0, joint->getVelocity(0)); + EXPECT_DOUBLE_EQ(0, joint->getVelocity(1)); + EXPECT_DOUBLE_EQ(0, joint->getAcceleration(0)); + EXPECT_DOUBLE_EQ(0, joint->getAcceleration(1)); + } + + for (std::size_t i = 0; i < num_steps; ++i) + { + world->step(); + + for (const auto joint : joints) + { + const double pos0 = joint->getPosition(0); + const double pos1 = joint->getPosition(1); + + EXPECT_LE(pos0, joint->getPositionUpperLimit(0) + 1e-6); + EXPECT_LE(pos1, joint->getPositionUpperLimit(1) + 1e-6); + EXPECT_GE(pos0, joint->getPositionLowerLimit(0) - 1e-6); + EXPECT_GE(pos1, joint->getPositionLowerLimit(1) - 1e-6); + } + } +} diff --git a/unittests/unit/test_Signal.cpp b/unittests/unit/test_Signal.cpp index 0cc1603a18cbd..6be0c39f3244c 100644 --- a/unittests/unit/test_Signal.cpp +++ b/unittests/unit/test_Signal.cpp @@ -139,8 +139,8 @@ TEST(Signal, NonStaticMemberFunction) signal2.connect(&Viewer::onSignal2Static); // Connect non-static member function - using placeholders::_1; - using placeholders::_2; + using std::placeholders::_1; + using std::placeholders::_2; signal1.connect(bind(&Viewer::onSignal1, &viewer, _1)); signal2.connect(bind(&Viewer::onSignal2, &viewer, _1, _2));