diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 59047a899e5d0..e7b24c818c68f 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -920,9 +920,9 @@ dynamics::SkeletonPtr ContactConstraint::getRootSkeleton() const assert(isActive()); if (mBodyNodeA->isReactive()) - return mBodyNodeA->getSkeleton()->mUnionRootSkeleton.lock(); + return ConstraintBase::getRootSkeleton(mBodyNodeA->getSkeleton()); else - return mBodyNodeB->getSkeleton()->mUnionRootSkeleton.lock(); + return ConstraintBase::getRootSkeleton(mBodyNodeB->getSkeleton()); } //============================================================================== diff --git a/data/sdf/test/issue1624_cubes.sdf b/data/sdf/test/issue1624_cubes.sdf new file mode 100644 index 0000000000000..02900fc9ddfd8 --- /dev/null +++ b/data/sdf/test/issue1624_cubes.sdf @@ -0,0 +1,5037 @@ + + + + 0 0 9 0 0 0 + 1 + + + + + 0 0 1 + 100 100 + + + + + 0 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 10 -0.445969 0.437062 0.232212 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 0 11.5 0.162706 0.237268 -0.04658 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 0 13 0.353318 0.359839 0.00596 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 0 14.5 0.279494 -0.284371 -0.051229 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 0 16 -0.03669 -0.197186 -0.095349 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 1.5 10 -0.444529 -0.146822 0.471578 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 1.5 11.5 0.313697 0.087827 0.266329 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 1.5 13 -0.097363 -0.144194 0.248824 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 1.5 14.5 -0.411716 -0.452408 -0.238945 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 1.5 16 -0.301174 0.051928 0.040391 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 3 10 0.325412 -0.150021 0.2704 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 3 11.5 -0.094698 0.330572 -0.26515 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 3 13 0.198001 -0.426438 0.054277 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 3 14.5 0.263427 -0.444841 0.21894 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 3 16 0.292098 -0.297187 -0.040628 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 4.5 10 -0.417755 0.391808 0.464083 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 4.5 11.5 0.300886 0.329793 -0.049279 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 4.5 13 0.46908 0.010174 -0.382881 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 4.5 14.5 -0.49055 0.081027 -0.24292 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 4.5 16 -0.314394 -0.475655 0.323114 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 6 10 -0.227574 0.107794 0.261805 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 6 11.5 0.170217 -0.314235 0.418764 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 6 13 0.400062 0.448993 -0.292275 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 6 14.5 -0.092668 -0.332408 0.475542 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 0 6 16 0.257004 -0.379118 -0.092636 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 0 10 0.354994 0.245428 0.266498 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 0 11.5 0.331195 0.322869 0.196046 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 0 13 0.068366 -0.022949 0.020605 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 0 14.5 -0.299301 -0.334679 -0.422738 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 0 16 0.187896 0.342837 -0.038506 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 1.5 10 -0.175396 0.200267 0.47763 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 1.5 11.5 -0.395518 -0.123414 0.050976 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 1.5 13 -0.183462 -0.18323 -0.351025 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 1.5 14.5 0.096896 0.428017 -0.173226 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 1.5 16 0.131744 0.494762 0.05059 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 3 10 -0.304631 -0.370278 -0.084566 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 3 11.5 -0.072463 -0.069379 -0.4 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 3 13 -0.402596 0.243529 -0.040504 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 3 14.5 0.31041 0.428389 -0.304969 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 3 16 -0.401715 0.014026 -0.178878 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 4.5 10 0.478232 -0.401519 -0.246683 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 4.5 11.5 0.205103 -0.295949 0.05506 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 4.5 13 -0.134601 -0.289928 0.411441 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 4.5 14.5 -0.423302 -0.205102 -0.28686 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 4.5 16 -0.425594 -0.138593 -0.41062 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 6 10 0.369977 -0.03656 0.142431 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 6 11.5 -0.218769 -0.102826 -0.212903 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 6 13 0.403949 0.377601 0.111663 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 6 14.5 0.121707 -0.125929 0.244155 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1.5 6 16 -0.101131 -0.223856 0.170884 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 0 10 -0.283591 0.406647 -0.062603 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 0 11.5 0.136025 0.355144 -0.394951 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 0 13 0.349044 0.33472 0.296917 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 0 14.5 -0.19994 -0.152393 0.420449 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 0 16 -0.194524 -0.262054 -0.389175 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 1.5 10 0.219018 0.092225 0.228936 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 1.5 11.5 0.412897 -0.224677 -0.049431 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 1.5 13 0.23988 -0.413764 -0.261685 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 1.5 14.5 -0.325931 0.091803 0.329326 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 1.5 16 0.035681 -0.321896 0.024914 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 3 10 0.484356 -0.24594 -0.151036 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 3 11.5 0.168977 -0.194917 0.46727 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 3 13 -0.031799 0.249432 0.090727 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 3 14.5 -0.198111 0.208371 0.246801 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 3 16 0.117927 -0.129518 0.290726 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 4.5 10 -0.025165 -0.414337 -0.152951 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 4.5 11.5 -0.265623 -0.089924 -0.241491 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 4.5 13 -0.090356 0.281294 0.422961 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 4.5 14.5 -0.149042 0.477807 -0.477402 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 4.5 16 -0.271486 -0.296293 -0.294881 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 6 10 0.209347 -0.454872 -0.161305 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 6 11.5 0.421568 0.2936 -0.053543 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 6 13 -0.140526 0.038979 -0.362839 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 6 14.5 -0.360287 -0.232538 -0.247077 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 3 6 16 0.352831 -0.359817 -0.105282 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 0 10 0.256834 -0.034594 0.128625 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 0 11.5 -0.368741 -0.178572 0.320848 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 0 13 0.134633 0.272501 -0.276824 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 0 14.5 0.326208 -0.136151 -0.48375 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 0 16 0.300434 0.129007 -0.460473 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 1.5 10 0.366286 -0.060726 0.37611 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 1.5 11.5 -0.365435 -0.353961 0.142329 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 1.5 13 0.286396 -0.404559 0.466458 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 1.5 14.5 0.13676 0.01454 0.038511 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 1.5 16 -0.182151 -0.385042 0.36934 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 3 10 0.348288 0.124407 0.003398 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 3 11.5 -0.196137 -0.394392 0.280725 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 3 13 0.119314 0.202347 0.386309 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 3 14.5 -0.358587 -0.423911 0.12514 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 3 16 0.392171 0.310731 -0.030338 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 4.5 10 -0.031809 0.411893 0.243838 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 4.5 11.5 -0.118548 0.415118 0.142056 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 4.5 13 -0.287036 0.421215 -0.124086 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 4.5 14.5 -0.126018 0.241975 0.079496 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 4.5 16 0.25064 0.309336 -0.132233 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 6 10 -0.369966 -0.384804 -0.313937 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 6 11.5 0.012061 0.353673 0.142476 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 6 13 0.287963 -0.308405 0.391526 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 6 14.5 0.167026 -0.382644 -0.360348 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 4.5 6 16 -0.399509 0.223433 -0.117832 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 0 10 0.275329 0.034203 0.459849 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 0 11.5 -0.479344 -0.384472 0.496045 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 0 13 -0.059376 -0.336423 -0.107769 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 0 14.5 -0.075613 -0.157664 -0.061889 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 0 16 0.002587 0.11586 0.348491 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 1.5 10 0.471153 0.248088 -0.14744 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 1.5 11.5 0.480398 0.035775 -0.01633 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 1.5 13 -0.136529 -0.444515 -0.456755 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 1.5 14.5 0.419368 -0.14412 -0.233303 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 1.5 16 0.15551 -0.198368 -0.217078 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 3 10 -0.285784 -0.480985 -0.285326 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 3 11.5 -0.107139 -0.175475 0.438333 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 3 13 -0.005922 -0.397777 0.453229 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 3 14.5 0.49633 -0.388825 0.132988 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 3 16 -0.221099 -0.167393 -0.490146 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 4.5 10 -0.248724 0.320639 0.450968 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 4.5 11.5 -0.343904 0.312495 -0.206604 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 4.5 13 -0.312696 0.004898 -0.210207 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 4.5 14.5 0.27778 0.454977 -0.313935 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 4.5 16 -0.195831 -0.468746 0.35285 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 6 10 -0.102246 -0.080965 -0.15701 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 6 11.5 -0.116101 -0.110291 0.439465 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 6 13 0.241486 0.446844 0.482211 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 6 14.5 -0.143891 -0.005632 -0.193793 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 6 6 16 -0.30886 0.046849 -0.239219 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 6 + + + + + 1 1 1 + + + + + 0 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + 0 0 -9.8 + + diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index bcbc62e121137..e0dd73c6a461d 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -42,6 +42,10 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode) dart-collision-ode dart-utils) + dart_add_test("regression" test_Issue1624) + target_link_libraries(test_Issue1624 + dart-collision-ode + dart-utils) endif() if(TARGET dart-utils) diff --git a/unittests/regression/test_Issue1624.cpp b/unittests/regression/test_Issue1624.cpp new file mode 100644 index 0000000000000..2a0d73e48c3cc --- /dev/null +++ b/unittests/regression/test_Issue1624.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2011-2021, 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/dart.hpp" +#include "dart/utils/sdf/SdfParser.hpp" + +#include "TestHelpers.hpp" + +using namespace dart::math; +using namespace dart::collision; +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::utils; + +//============================================================================== +TEST(Issue1624, ContactGrouping) +{ + // Load a world with a large number of contacts and run simulation to ensure + // constraint solver does not misbehave. + auto world + = SdfParser::readWorld("dart://sample/sdf/test/issue1624_cubes.sdf"); + ASSERT_TRUE(world != nullptr); + world->getConstraintSolver()->setCollisionDetector( + OdeCollisionDetector::create()); + const double dt = 0.001; + world->setTimeStep(dt); + std::size_t maxSteps = 1000; + for (std::size_t i = 0; i < maxSteps; i++) + { + world->step(); + } + + for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) + { + auto skel = world->getSkeleton(i); + Eigen::Vector3d velocity = skel->getCOMLinearVelocity(); + EXPECT_LE(velocity.norm(), 2.0); + } +}