int main(int argc, char* argv[])
{
WorldPtr world = std::make_shared<World>();
world->addSkeleton(createGround());
world->addSkeleton(createWall());
MyWindow window(world, createBall(), createSoftBody(), createHybridBody(),
createRigidChain(), createRigidRing());
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'1': toss a rigid ball" << std::endl;
std::cout << "'2': toss a soft body" << std::endl;
std::cout << "'3': toss a hybrid soft/rigid body" << std::endl;
std::cout << "'4': toss a rigid chain" << std::endl;
std::cout << "'5': toss a ring of rigid bodies" << std::endl;
std::cout << "\n'd': delete the oldest object" << std::endl;
std::cout << "'r': toggle randomness" << std::endl;
std::cout << "\nWarning: Let objects settle before tossing a new one, or the simulation could explode." << std::endl;
std::cout << " If the simulation freezes, you may need to force quit the application.\n" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Collisions");
glutMainLoop();
}
//==============================================================================
TEST(SKEL_PARSER, JointActuatorType)
{
WorldPtr world = SkelParser::readWorld(
DART_DATA_PATH"/skel/test/joint_actuator_type_test.skel");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
EXPECT_TRUE(skel1 != nullptr);
// Test for no actuator type attribute being specified
Joint* joint0 = skel1->getJoint("joint0");
EXPECT_EQ(joint0->getActuatorType(), Joint::DefaultActuatorType);
EXPECT_EQ(joint0->getActuatorType(), Joint::FORCE);
// Test for when actuator type attribute are specified
Joint* joint1 = skel1->getJoint("joint1");
EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE);
// Test for only a dof name being changed
Joint* joint2 = skel1->getJoint("joint2");
EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE);
joint2->setActuatorType(Joint::FORCE);
EXPECT_EQ(joint2->getActuatorType(), Joint::FORCE);
// Test for when actuator type attribute are specified
Joint* joint3 = skel1->getJoint("joint3");
EXPECT_EQ(joint3->getActuatorType(), Joint::ACCELERATION);
// Test for when actuator type attribute are specified
Joint* joint4 = skel1->getJoint("joint4");
EXPECT_EQ(joint4->getActuatorType(), Joint::VELOCITY);}
//==============================================================================
TEST(SkelParser, JointDynamicsElements)
{
WorldPtr world
= SkelParser::readWorld(
DART_DATA_PATH"/skel/test/joint_dynamics_elements_test.skel");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
Joint* joint0 = skel1->getJoint("joint0");
EXPECT_EQ(joint0->getDampingCoefficient(0), 1.0);
EXPECT_EQ(joint0->getCoulombFriction (0), 5.0);
EXPECT_EQ(joint0->getRestPosition (0), 0.1);
EXPECT_EQ(joint0->getSpringStiffness (0), 3.0);
Joint* joint1 = skel1->getJoint("joint1");
EXPECT_EQ(joint1->getDampingCoefficient(0), 1.0);
EXPECT_EQ(joint1->getCoulombFriction (0), 5.0);
EXPECT_EQ(joint1->getRestPosition (0), 0.1);
EXPECT_EQ(joint1->getSpringStiffness (0), 3.0);
EXPECT_EQ(joint1->getDampingCoefficient(1), 2.0);
EXPECT_EQ(joint1->getCoulombFriction (1), 4.0);
EXPECT_EQ(joint1->getRestPosition (1), 0.2);
EXPECT_EQ(joint1->getSpringStiffness (1), 2.0);
EXPECT_EQ(joint1->getDampingCoefficient(2), 3.0);
EXPECT_EQ(joint1->getCoulombFriction (2), 3.0);
EXPECT_EQ(joint1->getRestPosition (2), 0.3);
EXPECT_EQ(joint1->getSpringStiffness (2), 1.0);
}
void AbstractGLDisplayHelper::initRenderingContext(WorldPtr world)
{
int height = world->getHeight();
int width = world->getWidth();
int depth = world->getDepth();
int pixelResolution = world->getPixelResolution();
_renderContext = std::make_shared<OpenGLRenderContext>(width, height, depth, pixelResolution);
}
collision_detection::WorldDiff::WorldDiff(WorldDiff &other)
{
WorldPtr world = other.world_.lock();
if (world)
{
changes_ = other.changes_;
boost::weak_ptr<World>(world).swap(world_);
observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
}
}
// Load a biped model and enable joint limits and self-collision
SkeletonPtr loadBiped()
{
// Lesson 1
// Create the world with a skeleton
WorldPtr world = SkelParser::readWorld("dart://sample/skel/biped.skel");
assert(world != nullptr);
SkeletonPtr biped = world->getSkeleton("biped");
return biped;
}
// Load a skateboard model and connect it to the biped model via an Euler joint
// (Lesson 5 Answer)
void modifyBipedWithSkateboard(SkeletonPtr biped)
{
// Load the Skeleton from a file
WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");
SkeletonPtr skateboard = world->getSkeleton(0);
EulerJoint::Properties properties = EulerJoint::Properties();
properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
skateboard->getRootBodyNode()->moveTo<EulerJoint>
(biped->getBodyNode("h_heel_left"), properties);
}
//==============================================================================
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();
}
int main(int argc, char* argv[])
{
SkeletonPtr floor = createFloor();
// Lesson 1
SkeletonPtr biped = loadBiped();
// Lesson 2
setInitialPose(biped);
// Lesson 5
modifyBipedWithSkateboard(biped);
// Lesson 6
setVelocityAccuators(biped);
// Lesson 7
Eigen::VectorXd balancedPose = solveIK(biped);
biped->setPositions(balancedPose);
WorldPtr world = std::make_shared<World>();
world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
#if HAVE_BULLET_COLLISION
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
#endif
world->addSkeleton(floor);
world->addSkeleton(biped);
// Create a window for rendering the world and handling user input
MyWindow window(world);
// Print instructions
std::cout << "'.': forward push" << std::endl;
std::cout << "',': backward push" << std::endl;
std::cout << "'s': increase skateboard forward speed" << std::endl;
std::cout << "'a': increase skateboard backward speed" << std::endl;
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': replay simulation" << std::endl;
std::cout << "'v': Turn contact force visualization on/off" << std::endl;
std::cout <<
"'[' and ']': replay one frame backward and forward" << std::endl;
// Initialize glut, initialize the window, and begin the glut event loop
glutInit(&argc, argv);
window.initWindow(640, 480, "Multi-Pendulum Tutorial");
glutMainLoop();
}
void collision_detection::WorldDiff::setWorld(const WorldPtr& world)
{
WorldPtr old_world = world_.lock();
if (old_world)
{
old_world->notifyObserverAllObjects(observer_handle_, World::DESTROY);
old_world->removeObserver(observer_handle_);
}
boost::weak_ptr<World>(world).swap(world_);
observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
world->notifyObserverAllObjects(observer_handle_, World::CREATE|World::ADD_SHAPE);
}
static WorldPtr createWorld()
{
WorldPtr world = boost::make_shared<World>(500,500);
world->addObstacle( boost::make_shared<SquareObstacle>( 0, 50, 400, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>( 0, 150, 200, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>(300, 150, 200, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>(100, 250, 400, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>( 0, 350, 250, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>(350, 350, 50, 50) );
world->addObstacle( boost::make_shared<SquareObstacle>(150, 450, 350, 50) );
return world;
}
// Load a biped model and enable joint limits and self-collision
// (Lesson 1 Answer)
SkeletonPtr loadBiped()
{
// Create the world with a skeleton
WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel");
assert(world != nullptr);
SkeletonPtr biped = world->getSkeleton("biped");
// Set joint limits
for(size_t i = 0; i < biped->getNumJoints(); ++i)
biped->getJoint(i)->setPositionLimitEnforced(true);
// Enable self collision check but ignore adjacent bodies
biped->enableSelfCollision();
return biped;
}
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;
}
//==============================================================================
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());
}
void GL2DDisplayHelper::paintSingleSolution(WorldPtr world, PartLocationListPtr parts)
{
for (tuple<PartOrientationPtr, Point>& partLocation : *parts)
{
PartOrientationPtr currentOrientation = std::get<0>(partLocation);
Point anchorPartPoint = std::get<1>(partLocation);
RGB_COLOR orientationColor = _colorManager->getColor(*currentOrientation);
float r = std::get<0>(orientationColor);
float g = std::get<1>(orientationColor);
float b = std::get<2>(orientationColor);
float a;
r /= 255.0;
g /= 255.0;
b /= 255.0;
a = 1.0f;
for (Point& relativePartCoordinate : *(currentOrientation->getPointList()))
{
Point point = anchorPartPoint + relativePartCoordinate;
// Paint a square for each unmasked pixel in the world
float v1x = point.getX();
float v1y = world->getHeight() - point.getY() - 1; // OpenGL Y axis is inverted
float v2x = v1x + SQUARE_SIZE;
float v2y = v1y;
float v3x = v1x;
float v3y = v1y + SQUARE_SIZE;
float v4x = v1x + SQUARE_SIZE;
float v4y = v1y + SQUARE_SIZE;
_renderContext->write2DTriangle(v1x, v1y, v2x, v2y, v3x, v3y, r, g, b, a);
_renderContext->write2DTriangle(v2x, v2y, v3x, v3y, v4x, v4y, r, g, b, a);
if (currentOrientation->isContainsPointAbovePoint(relativePartCoordinate))
_renderContext->writeLine(v1x, v1y, v2x, v2y);
else
_renderContext->writeBoldLine(v1x, v1y, v2x, v2y);
if (currentOrientation->isContainsPointRightOfPoint(relativePartCoordinate))
_renderContext->writeLine(v2x, v2y, v4x, v4y);
else
_renderContext->writeBoldLine(v2x, v2y, v4x, v4y);
if (currentOrientation->isContainsPointBelowPoint(relativePartCoordinate))
_renderContext->writeLine(v3x, v3y, v4x, v4y);
else
_renderContext->writeBoldLine(v3x, v3y, v4x, v4y);
if (currentOrientation->isContainsPointLeftPoint(relativePartCoordinate))
_renderContext->writeLine(v3x, v3y, v1x, v1y);
else
_renderContext->writeBoldLine(v3x, v3y, v1x, v1y);
}
}
}
SimulationWorld(WorldPtr world, const std::vector<Eigen::VectorXd>& trajectory)
: osgDart::WorldNode(world), mTrajectory(trajectory)
{
mHubo = world->getSkeleton("drchubo");
double height = mHubo->getPosition(5);
mHubo->setPositions(mTrajectory[0]);
mHubo->setPosition(5, height);
// index = 0;
}
/// Create a new World instance.
///
/// @param[in] pSceneDefinition The SceneDefinition from which to create the new World.
///
/// @return Newly created world instance.
Helium::World* WorldManager::CreateWorld( SceneDefinition* pSceneDefinition )
{
// Any scene that creates a world must define a world definition so that the world knows what components to init on itself
HELIUM_ASSERT(pSceneDefinition);
const WorldDefinition *pWorldDefinition = pSceneDefinition->GetWorldDefinition();
WorldPtr spWorld;
if (pWorldDefinition)
{
// Let the world definition provide the world
spWorld = pWorldDefinition->CreateWorld();
}
else
{
// Make a blank, default world.
// TODO: Consider having the concept of a "default" world definition. Generally, not loading a world from a definition
// is bad because pretty much any useful world should be initialized with world components
HELIUM_ASSERT( 0 );
spWorld = new World();
spWorld->Initialize();
}
HELIUM_ASSERT(spWorld.Get());
m_worlds.Push( spWorld );
if ( pSceneDefinition && spWorld )
{
Slice *pRootSlice = spWorld->GetRootSlice();
size_t entityDefinitionCount = pSceneDefinition->GetEntityDefinitionCount();
for ( size_t i = 0; i < entityDefinitionCount; ++i )
{
EntityDefinition *pEntityDefinition = pSceneDefinition->GetEntityDefinition( i );
HELIUM_ASSERT( pEntityDefinition );
Entity *pEntity = pRootSlice->CreateEntity(pEntityDefinition);
}
}
return spWorld;
}
void collision_detection::WorldDiff::reset(const WorldPtr& world)
{
clearChanges();
WorldPtr old_world = world_.lock();
if (old_world)
old_world->removeObserver(observer_handle_);
boost::weak_ptr<World>(world).swap(world_);
observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
}
//==============================================================================
TEST(SdfParser, SDFSingleBodyWithoutJoint)
{
// Regression test for #444
WorldPtr world
= SdfParser::readSdfFile(
DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel = world->getSkeleton(0);
EXPECT_TRUE(skel != nullptr);
EXPECT_EQ(skel->getNumBodyNodes(), 1u);
EXPECT_EQ(skel->getNumJoints(), 1u);
BodyNodePtr bodyNode = skel->getBodyNode(0);
EXPECT_TRUE(bodyNode != nullptr);
EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1u);
EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u);
JointPtr joint = skel->getJoint(0);
EXPECT_TRUE(joint != nullptr);
EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
}
//==============================================================================
TEST(SkelParser, RigidAndSoftBodies)
{
using namespace dart;
using namespace math;
using namespace dynamics;
using namespace simulation;
using namespace utils;
WorldPtr world = SkelParser::readWorld(
DART_DATA_PATH"skel/test/test_articulated_bodies.skel");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
EXPECT_TRUE(skel1 != nullptr);
EXPECT_EQ(static_cast<int>(skel1->getNumBodyNodes()), 2);
EXPECT_EQ(static_cast<int>(skel1->getNumRigidBodyNodes()), 1);
EXPECT_EQ(static_cast<int>(skel1->getNumSoftBodyNodes()), 1);
SoftBodyNode* sbn = skel1->getSoftBodyNode(0);
EXPECT_TRUE(static_cast<int>(sbn->getNumPointMasses()) > 0);
world->step();
}
//==============================================================================
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();
}
/// Constructor
MyWindow(WorldPtr world)
: mBallConstraint(nullptr),
mPositiveSign(true),
mBodyForce(false)
{
setWorld(world);
// Find the Skeleton named "pendulum" within the World
mPendulum = world->getSkeleton("pendulum");
// Make sure that the pendulum was found in the World
assert(mPendulum != nullptr);
mForceCountDown.resize(mPendulum->getNumDofs(), 0);
ArrowShape::Properties arrow_properties;
arrow_properties.mRadius = 0.05;
mArrow = std::shared_ptr<ArrowShape>(new ArrowShape(
Eigen::Vector3d(-default_height, 0.0, default_height / 2.0),
Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0),
arrow_properties, dart::Color::Orange(1.0)));
}
//==============================================================================
TEST(FileInfoWorld, Basic)
{
const size_t numFrames = 100;
const std::string fileName = "testWorld.txt";
double tol = 1e-6;
bool result = false;
FileInfoWorld worldFile;
WorldPtr world = SkelParser::readWorld(
DART_DATA_PATH"/skel/test/file_info_world_test.skel");
EXPECT_TRUE(world != nullptr);
Recording* recording1 = nullptr;
Recording* recording2 = nullptr;
// Do some simulation with recording
for (size_t i = 0; i < numFrames; ++i)
{
world->step();
world->bake();
}
recording1 = world->getRecording();
EXPECT_TRUE(recording1 != nullptr);
// Save the recording to a file
result = worldFile.saveFile(fileName.c_str(), world->getRecording());
EXPECT_TRUE(result);
// Load the file
result = worldFile.loadFile(fileName.c_str());
EXPECT_TRUE(result);
recording2 = worldFile.getRecording();
EXPECT_TRUE(recording2 != nullptr);
// Check number of frames
EXPECT_EQ(recording1->getNumFrames(), (int)numFrames);
EXPECT_EQ(recording2->getNumFrames(), (int)numFrames);
// Check number of skeletons
size_t numSkeletons = recording1->getNumSkeletons();
EXPECT_EQ(recording1->getNumSkeletons(), recording2->getNumSkeletons());
// Check number of dofs of the skeletons
for (size_t i = 0; i < numSkeletons; ++i)
EXPECT_EQ(recording1->getNumDofs(i), recording2->getNumDofs(i));
// Check generalized positions and contact info
for (size_t i = 0; i < numFrames; ++i)
{
// Check generalized positions
for (size_t j = 0; j < numSkeletons; ++j)
{
size_t dofs = recording1->getNumDofs(j);
for (size_t k = 0; k < dofs; ++k)
{
EXPECT_NEAR(recording1->getGenCoord(i, j, k),
recording2->getGenCoord(i, j, k), tol);
}
}
// Check contact info
tol = 1e-3;
size_t numContacts = recording1->getNumContacts(i);
EXPECT_EQ(recording1->getNumContacts(i), recording2->getNumContacts(i));
for (size_t j = 0; j < numContacts; ++j)
{
for (size_t k = 0; k < 3; ++k)
{
EXPECT_NEAR(recording1->getContactForce(i, j)[k],
recording2->getContactForce(i, j)[k], tol);
EXPECT_NEAR(recording1->getContactPoint(i, j)[k],
recording2->getContactPoint(i, j)[k], tol);
}
}
}
}
//==============================================================================
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(SkelParser, DofAttributes)
{
WorldPtr world = SkelParser::readWorld(
DART_DATA_PATH"/skel/test/dof_attribute_test.skel");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
// Test for no dof elements being specified
Joint* joint0 = skel1->getJoint("joint0");
EXPECT_EQ(joint0->getDof(0)->getPositionLowerLimit(), -DART_DBL_INF);
EXPECT_EQ(joint0->getDof(0)->getPositionUpperLimit(), DART_DBL_INF);
EXPECT_EQ(joint0->getDof(0)->getPosition(), 0);
EXPECT_EQ(joint0->getDof(0)->getVelocityLowerLimit(), -DART_DBL_INF);
EXPECT_EQ(joint0->getDof(0)->getVelocityUpperLimit(), DART_DBL_INF);
EXPECT_EQ(joint0->getDof(0)->getVelocity(), 0);
EXPECT_EQ(joint0->getDof(0)->getName(), joint0->getName());
// Test for only a dof name being changed
Joint* joint1 = skel1->getJoint("joint1");
EXPECT_EQ(joint1->getDof(0)->getPositionLowerLimit(), -1.57);
EXPECT_EQ(joint1->getDof(0)->getPositionUpperLimit(), 1.57);
EXPECT_EQ(joint1->getDof(0)->getName(), "customJoint1");
// Test for when dof attributes (but not a name) are specified
Joint* joint2 = skel1->getJoint("joint2");
EXPECT_EQ(joint2->getDof(0)->getName(), joint2->getName());
EXPECT_EQ(joint2->getDof(0)->getPositionLowerLimit(), -1);
EXPECT_EQ(joint2->getDof(0)->getPositionUpperLimit(), 1);
EXPECT_EQ(joint2->getDof(0)->getPosition(), 0);
EXPECT_EQ(joint2->getDof(0)->getVelocityLowerLimit(), -2);
EXPECT_EQ(joint2->getDof(0)->getVelocityUpperLimit(), 2);
EXPECT_EQ(joint2->getDof(0)->getVelocity(), 0);
EXPECT_EQ(joint2->getDof(0)->getAccelerationLowerLimit(), -3);
EXPECT_EQ(joint2->getDof(0)->getAccelerationUpperLimit(), 3);
EXPECT_EQ(joint2->getDof(0)->getAcceleration(), 0);
EXPECT_EQ(joint2->getDof(0)->getForceLowerLimit(), -4);
EXPECT_EQ(joint2->getDof(0)->getForceUpperLimit(), 4);
EXPECT_EQ(joint2->getDof(0)->getForce(), 0);
// Test for mixture of old method and new method
// Note: If there is a conflict, the data given in the dof element will win
Joint* joint3 = skel1->getJoint("joint3");
EXPECT_EQ(joint3->getDof(0)->getName(), joint3->getName() + "_1");
EXPECT_EQ(joint3->getDof(0)->getPositionLowerLimit(), -1);
EXPECT_EQ(joint3->getDof(0)->getPositionUpperLimit(), 1);
EXPECT_EQ(joint3->getDof(0)->getPosition(), 5);
EXPECT_EQ(joint3->getDof(1)->getName(), joint3->getName() + "_2");
EXPECT_EQ(joint3->getDof(1)->getPositionLowerLimit(), -2);
EXPECT_EQ(joint3->getDof(1)->getPositionUpperLimit(), 2);
EXPECT_EQ(joint3->getDof(1)->getPosition(), -5);
// Test for only some of the DOFs being specified
Joint* joint4 = skel1->getJoint("joint4");
EXPECT_EQ(joint4->getDof(0)->getName(), "joint4_1");
EXPECT_EQ(joint4->getDof(0)->getPositionLowerLimit(), -1);
EXPECT_EQ(joint4->getDof(0)->getPositionUpperLimit(), 1);
EXPECT_EQ(joint4->getDof(0)->getVelocityLowerLimit(), -10);
EXPECT_EQ(joint4->getDof(0)->getVelocityUpperLimit(), 10);
EXPECT_EQ(joint4->getDof(1)->getName(), joint4->getName() + "_y");
EXPECT_EQ(joint4->getDof(2)->getName(), "joint4_3");
EXPECT_EQ(joint4->getDof(2)->getPositionLowerLimit(), -2);
EXPECT_EQ(joint4->getDof(2)->getPositionUpperLimit(), 2);
EXPECT_EQ(joint4->getDof(2)->getVelocityLowerLimit(), -20);
EXPECT_EQ(joint4->getDof(2)->getVelocityUpperLimit(), 20);
}
void GL2DDisplayHelper::paintWorld(WorldPtr world)
{
// Paint the world pixels to the context
auto worldPainter = std::make_shared<World2DDisplayVisitor>(_renderContext);
world->accept(worldPainter);
}
//==============================================================================
TEST(SkelParser, PlanarJoint)
{
using namespace dart;
using namespace math;
using namespace dynamics;
using namespace simulation;
using namespace utils;
WorldPtr world = SkelParser::readWorld(
DART_DATA_PATH"skel/test/planar_joint.skel");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel1 = world->getSkeleton("skeleton1");
EXPECT_TRUE(skel1 != nullptr);
BodyNode* body1 = skel1->getBodyNode("link1");
BodyNode* body2 = skel1->getBodyNode("link2");
BodyNode* body3 = skel1->getBodyNode("link3");
BodyNode* body4 = skel1->getBodyNode("link4");
EXPECT_TRUE(body1 != nullptr);
EXPECT_TRUE(body2 != nullptr);
EXPECT_TRUE(body3 != nullptr);
EXPECT_TRUE(body4 != nullptr);
PlanarJoint* planarJoint1
= dynamic_cast<PlanarJoint*>(body1->getParentJoint());
PlanarJoint* planarJoint2
= dynamic_cast<PlanarJoint*>(body2->getParentJoint());
PlanarJoint* planarJoint3
= dynamic_cast<PlanarJoint*>(body3->getParentJoint());
PlanarJoint* planarJoint4
= dynamic_cast<PlanarJoint*>(body4->getParentJoint());
EXPECT_TRUE(planarJoint1 != nullptr);
EXPECT_TRUE(planarJoint2 != nullptr);
EXPECT_TRUE(planarJoint3 != nullptr);
EXPECT_TRUE(planarJoint4 != nullptr);
EXPECT_EQ(planarJoint1->getPlaneType(), PlanarJoint::PT_XY);
EXPECT_EQ(planarJoint2->getPlaneType(), PlanarJoint::PT_YZ);
EXPECT_EQ(planarJoint3->getPlaneType(), PlanarJoint::PT_ZX);
EXPECT_EQ(planarJoint4->getPlaneType(), PlanarJoint::PT_ARBITRARY);
EXPECT_EQ(planarJoint1->getTranslationalAxis1(), Eigen::Vector3d::UnitX());
EXPECT_EQ(planarJoint2->getTranslationalAxis1(), Eigen::Vector3d::UnitY());
EXPECT_EQ(planarJoint3->getTranslationalAxis1(), Eigen::Vector3d::UnitZ());
EXPECT_EQ(planarJoint4->getTranslationalAxis1(), Eigen::Vector3d::UnitX());
EXPECT_EQ(planarJoint1->getTranslationalAxis2(), Eigen::Vector3d::UnitY());
EXPECT_EQ(planarJoint2->getTranslationalAxis2(), Eigen::Vector3d::UnitZ());
EXPECT_EQ(planarJoint3->getTranslationalAxis2(), Eigen::Vector3d::UnitX());
EXPECT_EQ(planarJoint4->getTranslationalAxis2(), Eigen::Vector3d::UnitY());
EXPECT_EQ(planarJoint1->getRotationalAxis(), Eigen::Vector3d::UnitZ());
EXPECT_EQ(planarJoint2->getRotationalAxis(), Eigen::Vector3d::UnitX());
EXPECT_EQ(planarJoint3->getRotationalAxis(), Eigen::Vector3d::UnitY());
EXPECT_EQ(planarJoint4->getRotationalAxis(), Eigen::Vector3d::UnitZ());
EXPECT_EQ(planarJoint1->getPositions(), Eigen::Vector3d(1, 2, 3));
EXPECT_EQ(planarJoint2->getPositions(), Eigen::Vector3d(1, 2, 3));
EXPECT_EQ(planarJoint3->getPositions(), Eigen::Vector3d(1, 2, 3));
EXPECT_EQ(planarJoint4->getPositions(), Eigen::Vector3d(1, 2, 3));
EXPECT_EQ(planarJoint1->getVelocities(), Eigen::Vector3d(4, 5, 6));
EXPECT_EQ(planarJoint2->getVelocities(), Eigen::Vector3d(4, 5, 6));
EXPECT_EQ(planarJoint3->getVelocities(), Eigen::Vector3d(4, 5, 6));
EXPECT_EQ(planarJoint4->getVelocities(), Eigen::Vector3d(4, 5, 6));
EXPECT_EQ(planarJoint1->getDampingCoefficient(0), 1);
EXPECT_EQ(planarJoint2->getDampingCoefficient(0), 1);
EXPECT_EQ(planarJoint3->getDampingCoefficient(0), 1);
EXPECT_EQ(planarJoint4->getDampingCoefficient(0), 1);
EXPECT_EQ(planarJoint1->getDampingCoefficient(1), 2);
EXPECT_EQ(planarJoint2->getDampingCoefficient(1), 2);
EXPECT_EQ(planarJoint3->getDampingCoefficient(1), 2);
EXPECT_EQ(planarJoint4->getDampingCoefficient(1), 2);
EXPECT_EQ(planarJoint1->getDampingCoefficient(2), 3);
EXPECT_EQ(planarJoint2->getDampingCoefficient(2), 3);
EXPECT_EQ(planarJoint3->getDampingCoefficient(2), 3);
EXPECT_EQ(planarJoint4->getDampingCoefficient(2), 3);
EXPECT_EQ(planarJoint1->getPositionLowerLimit(0), -1.0);
EXPECT_EQ(planarJoint2->getPositionLowerLimit(0), -1.0);
EXPECT_EQ(planarJoint3->getPositionLowerLimit(0), -1.0);
EXPECT_EQ(planarJoint4->getPositionLowerLimit(0), -1.0);
EXPECT_EQ(planarJoint1->getPositionUpperLimit(0), +1.0);
EXPECT_EQ(planarJoint2->getPositionUpperLimit(0), +1.0);
EXPECT_EQ(planarJoint3->getPositionUpperLimit(0), +1.0);
EXPECT_EQ(planarJoint4->getPositionUpperLimit(0), +1.0);
EXPECT_EQ(planarJoint1->getPositionLowerLimit(1), -2.0);
EXPECT_EQ(planarJoint2->getPositionLowerLimit(1), -2.0);
EXPECT_EQ(planarJoint3->getPositionLowerLimit(1), -2.0);
EXPECT_EQ(planarJoint4->getPositionLowerLimit(1), -2.0);
EXPECT_EQ(planarJoint1->getPositionUpperLimit(1), +2.0);
EXPECT_EQ(planarJoint2->getPositionUpperLimit(1), +2.0);
EXPECT_EQ(planarJoint3->getPositionUpperLimit(1), +2.0);
EXPECT_EQ(planarJoint4->getPositionUpperLimit(1), +2.0);
EXPECT_EQ(planarJoint1->getPositionLowerLimit(2), -3.0);
EXPECT_EQ(planarJoint2->getPositionLowerLimit(2), -3.0);
EXPECT_EQ(planarJoint3->getPositionLowerLimit(2), -3.0);
EXPECT_EQ(planarJoint4->getPositionLowerLimit(2), -3.0);
EXPECT_EQ(planarJoint1->getPositionUpperLimit(2), +3.0);
EXPECT_EQ(planarJoint2->getPositionUpperLimit(2), +3.0);
EXPECT_EQ(planarJoint3->getPositionUpperLimit(2), +3.0);
EXPECT_EQ(planarJoint4->getPositionUpperLimit(2), +3.0);
world->step();
}
collision_detection::WorldDiff::WorldDiff(const WorldPtr& world) :
world_(world)
{
observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
}
//==============================================================================
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;
}
}
