/* * Copyright (c) 2011-2022, 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 "dart/collision/dart/DARTCollisionDetector.hpp" #include "dart/common/Console.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/math/Geometry.hpp" #include "dart/math/Helpers.hpp" #include "dart/math/Random.hpp" #include "dart/simulation/World.hpp" #include "dart/utils/SkelParser.hpp" #include "TestHelpers.hpp" //============================================================================== class ConstraintTest : public ::testing::Test { public: // Get Skel file list to test. const std::vector& getList(); // void SingleContactTest(const std::string& _fileName); protected: // Sets up the test fixture. void SetUp() override; // Skel file list. std::vector list; }; //============================================================================== void ConstraintTest::SetUp() { list.push_back("dart://sample/skel/test/chainwhipa.skel"); list.push_back("dart://sample/skel/test/single_pendulum.skel"); list.push_back("dart://sample/skel/test/single_pendulum_euler_joint.skel"); list.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel"); list.push_back("dart://sample/skel/test/double_pendulum.skel"); list.push_back("dart://sample/skel/test/double_pendulum_euler_joint.skel"); list.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel"); list.push_back("dart://sample/skel/test/serial_chain_revolute_joint.skel"); list.push_back("dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"); list.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel"); list.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel"); list.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel"); list.push_back("dart://sample/skel/test/simple_tree_structure.skel"); list.push_back( "dart://sample/skel/test/simple_tree_structure_euler_joint.skel"); list.push_back( "dart://sample/skel/test/simple_tree_structure_ball_joint.skel"); list.push_back("dart://sample/skel/test/tree_structure.skel"); list.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel"); list.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel"); list.push_back("dart://sample/skel/fullbody1.skel"); } //============================================================================== const std::vector& ConstraintTest::getList() { return list; } //============================================================================== void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) { using namespace std; using namespace Eigen; using namespace dart::math; using namespace dart::collision; using namespace dart::constraint; using namespace dart::dynamics; using namespace dart::simulation; using namespace dart::utils; //---------------------------------------------------------------------------- // Settings //---------------------------------------------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode // std::size_t testCount = 1; #else // std::size_t testCount = 1; #endif WorldPtr world = World::create(); EXPECT_TRUE(world != nullptr); world->setGravity(Vector3d(0.0, -10.00, 0.0)); world->setTimeStep(0.001); world->getConstraintSolver()->setCollisionDetector( DARTCollisionDetector::create()); SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0)); BodyNode* sphere = sphereSkel->getBodyNode(0); Joint* sphereJoint = sphere->getParentJoint(); sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis world->addSkeleton(sphereSkel); EXPECT_EQ(sphereSkel->getGravity(), world->getGravity()); assert(sphere); SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0), Vector3d(0.0, 1.0, 0.0)); BodyNode* box = boxSkel->getBodyNode(0); Joint* boxJoint = box->getParentJoint(); boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis // world->addSkeleton(boxSkel); // EXPECT_EQ(boxSkel->getGravity(), world->getGravity()); // assert(box); SkeletonPtr groundSkel = createGround( Vector3d(10000.0, 0.1, 10000.0), Vector3d(0.0, -0.05, 0.0)); groundSkel->setMobile(false); // BodyNode* ground = groundSkel->getBodyNode(0); world->addSkeleton(groundSkel); EXPECT_EQ(groundSkel->getGravity(), world->getGravity()); // assert(ground); EXPECT_EQ((int)world->getNumSkeletons(), 2); // Lower and upper bound of configuration for system // double lb = -1.5 * constantsd::pi(); // double ub = 1.5 * constantsd::pi(); int maxSteps = 500; for (int i = 0; i < maxSteps; ++i) { // Vector3d pos1 = sphere->getWorldTransform().translation(); // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1); // std::cout << "pos1:" << pos1.transpose() << std::endl; // std::cout << "vel1:" << vel1.transpose() << std::endl; if (!world->checkCollision()) { world->step(); continue; } // for (std::size_t j = 0; j < cd->getNumContacts(); ++j) // { // Contact contact = cd->getContact(j); // Vector3d pos1 = sphere->getTransform().inverse() * contact.point; // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1); // std::cout << "pos1:" << pos1.transpose() << std::endl; // std::cout << "vel1:" << vel1.transpose() << std::endl; // } world->step(); const auto& result = world->getConstraintSolver()->getLastCollisionResult(); for (const auto& contact : result.getContacts()) { Vector3d pos1 = sphere->getTransform().inverse() * contact.point; Vector3d vel1 = sphere->getLinearVelocity(pos1); // std::cout << "pos1:" << pos1.transpose() << std::endl; // std::cout << "pos1[1]: " << pos1[1] << std::endl; // std::cout << "pos1:" << pos1.transpose() << std::endl; std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl; // EXPECT_NEAR(pos1[0], 0.0, 1e-9); // EXPECT_NEAR(pos1[1], -0.05, 1e-2); // EXPECT_NEAR(pos1[2], 0.0, 1e-9); // EXPECT_NEAR(vel1[0], 0.0, 1e-9); // EXPECT_NEAR(vel1[1], 0.0, 1e-9); // EXPECT_NEAR(vel1[2], 0.0, 1e-9); // if (!equals(vel1, Vector3d(0.0, 0.0, 0.0))) // std::cout << "vel1:" << vel1.transpose() << std::endl; // EXPECT_EQ(vel1, Vector3d::Zero()); } // std::cout << std::endl; break; } } //============================================================================== TEST_F(ConstraintTest, SingleContactTest) { // for (int i = 0; i < getList().size(); ++i) // { //#ifndef NDEBUG // dtdbg << getList()[i] << std::endl; //#endif // SingleContactTest(getList()[i]); // } SingleContactTest(getList()[0]); }