//============================================================================== TEST(VskParser, SingleStepSimulations) { WorldPtr world = World::create(); EXPECT_NE(world, nullptr); SkeletonPtr nick = VskParser::readSkeleton("dart://sample/vsk/Nick01.vsk"); EXPECT_NE(nick, nullptr); EXPECT_EQ(nick->getNumMarkers(), 53u); SkeletonPtr sehoon = VskParser::readSkeleton("dart://sample/vsk/SehoonVSK3.vsk"); EXPECT_NE(sehoon, nullptr); EXPECT_EQ(nick->getNumMarkers(), 53u); SkeletonPtr yuting = VskParser::readSkeleton("dart://sample/vsk/Yuting.vsk"); EXPECT_NE(yuting, nullptr); EXPECT_EQ(nick->getNumMarkers(), 53u); world->removeAllSkeletons(); world->addSkeleton(nick); EXPECT_EQ(world->getNumSkeletons(), 1u); world->step(); world->removeAllSkeletons(); world->addSkeleton(sehoon); EXPECT_EQ(world->getNumSkeletons(), 1u); world->step(); world->removeAllSkeletons(); world->addSkeleton(yuting); EXPECT_EQ(world->getNumSkeletons(), 1u); world->step(); }
//============================================================================== TEST(VskParser, EmptySkeleton) { WorldPtr world = World::create(); EXPECT_TRUE(world != nullptr); SkeletonPtr skeleton = VskParser::readSkeleton("dart://sample/vsk/test/empty.vsk"); EXPECT_TRUE(skeleton == nullptr); world->addSkeleton(skeleton); EXPECT_EQ(world->getNumSkeletons(), 0u); world->step(); }
//============================================================================== TEST(SkelParser, EmptyWorld) { WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/test/empty.skel"); EXPECT_TRUE(world != nullptr); EXPECT_EQ(world->getTimeStep(), 0.001); EXPECT_EQ(world->getGravity()(0), 0); EXPECT_EQ(world->getGravity()(1), 0); EXPECT_EQ(world->getGravity()(2), -9.81); EXPECT_EQ((int)world->getNumSkeletons(), 0); EXPECT_EQ(world->getTime(), 0); world->step(); EXPECT_EQ(world->getTime(), world->getTimeStep()); }
//============================================================================== TEST(SkelParser, SerialChain) { WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"skel/test/serial_chain_ball_joint.skel"); EXPECT_TRUE(world != nullptr); EXPECT_EQ(world->getTimeStep(), 0.001); EXPECT_EQ(world->getGravity()(0), 0); EXPECT_EQ(world->getGravity()(1), -9.81); EXPECT_EQ(world->getGravity()(2), 0); EXPECT_EQ(static_cast<int>(world->getNumSkeletons()), 1); SkeletonPtr skel1 = world->getSkeleton("skeleton 1"); EXPECT_EQ(static_cast<int>(skel1->getNumBodyNodes()), 10); world->step(); }
//============================================================================== TEST(SkelParser, SinglePendulum) { WorldPtr world = SkelParser::readWorld( DART_DATA_PATH"skel/test/single_pendulum.skel"); EXPECT_TRUE(world != nullptr); EXPECT_EQ(world->getTimeStep(), 0.001); EXPECT_EQ(world->getGravity()(0), 0); EXPECT_EQ(world->getGravity()(1), -9.81); EXPECT_EQ(world->getGravity()(2), 0); EXPECT_EQ(static_cast<int>(world->getNumSkeletons()), 1); SkeletonPtr skel1 = world->getSkeleton("single_pendulum"); EXPECT_EQ(static_cast<int>(skel1->getNumBodyNodes()), 1); world->step(); }
std::vector<SkeletonPtr> getSkeletons() { std::vector<std::string> fileList = getFileList(); std::vector<WorldPtr> worlds; for(size_t i=0; i<fileList.size(); ++i) worlds.push_back(utils::SkelParser::readWorld(fileList[i])); std::vector<SkeletonPtr> skeletons; for(size_t i=0; i<worlds.size(); ++i) { WorldPtr world = worlds[i]; for(size_t j=0; j<world->getNumSkeletons(); ++j) skeletons.push_back(world->getSkeleton(j)); } return skeletons; }
//============================================================================== 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::io; //---------------------------------------------------------------------------- // 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; } }