int main(int argc, char* argv[])
{
// load a skeleton file
// create and initialize the world
dart::simulation::World* myWorld
= dart::utils::SkelParser::readWorld(
DART_DATA_PATH"skel/soft_open_chain.skel");
assert(myWorld != NULL);
int dof = myWorld->getSkeleton("skeleton 1")->getNumGenCoords();
Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof);
for (int i = 0; i < 3; i++)
initPose[i] = dart::math::random(-0.5, 0.5);
myWorld->getSkeleton("skeleton 1")->setConfigs(initPose, true, true, false);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Soft Open Chain");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// load a skeleton file
// create and initialize the world
dart::simulation::World* myWorld
= dart::utils::readSkelFile(DART_DATA_PATH"/skel/chain.skel");
assert(myWorld != NULL);
// create and initialize the world
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
myWorld->setTimeStep(1.0/2000);
int dof = myWorld->getSkeleton(0)->getDOF();
Eigen::VectorXd initPose(dof);
for (int i = 0; i < dof; i++)
initPose[i] = random(-0.5, 0.5);
myWorld->getSkeleton(0)->setPose(initPose);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[]) {
// create and initialize the world
dart::simulation::World *myWorld
= dart::utils::SkelParser::readWorld(
DART_DATA_PATH"/skel/bullet_collision.skel");
assert(myWorld != NULL);
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
std::cout << "'q': spawn a random cube" << std::endl;
std::cout << "'w': delete a spawned cube" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Bullet Collision");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// load a skeleton file
// create and initialize the world
dart::simulation::SoftWorld *myWorld
= dart::utils::SoftSkelParser::readSoftFile(
DART_DATA_PATH"skel/test/test_single_pendulum.skel");
assert(myWorld != NULL);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Soft Single Pendulum");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(DART_DATA_PATH"/skel/Chain.skel", SKEL);
// create and initialize the world
World *myWorld = new World();
Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
myWorld->setTimeStep(1.0/2000);
myWorld->addSkeleton((SkeletonDynamics*)model.getSkel());
int nDof = myWorld->getSkeleton(0)->getNumDofs();
VectorXd initPose(nDof);
for (int i = 0; i < nDof; i++)
initPose[i] = random(-0.5, 0.5);
myWorld->getSkeleton(0)->setPose(initPose);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
//==============================================================================
int main(int argc, char* argv[])
{
// create and initialize the world
auto world = std::make_shared<simulation::World>();
assert(world != nullptr);
// create and initialize the world
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
//world->setGravity(Eigen::Vector3d::Zero());
world->setGravity(gravity);
world->setTimeStep(1.0/1000);
const auto numLinks = 10;
const auto l = 1.0;
auto skel = createNLinkRobot(numLinks, Vector3d(0.3, 0.3, l), DOF_ROLL);
// auto skel = createNLinkRobot(numLinks, Vector3d(0.3, 0.3, l), BALL);
world->addSkeleton(skel);
const auto posLower = math::constantsd::pi() * -0.1;
const auto posUpper = math::constantsd::pi() * 0.1;
const auto velLower = math::constantsd::pi() * -0.1;
const auto velUpper = math::constantsd::pi() * 0.1;
for (auto i = 0u; i < numLinks; ++i)
{
auto joint = skel->getJoint(i);
const auto pos = math::random(posLower, posUpper);
const auto vel = math::random(velLower, velUpper);
for (auto j = 0u; j < joint->getNumDofs(); ++j)
{
joint->setPosition(j, pos);
joint->setVelocity(j, vel);
}
}
world->dm_initialize_RIQN_DRNEA();
// while (world->getTime() < 60.0)
// {
// std::cout << "time: " << world->getTime() << std::endl;
// world->dm_step();
// }
// std::cout << "E: " << world->getSkeleton(0)->getTotalEnergy() << std::endl;
// create a window and link it to the world
MyWindow window;
window.setWorld(world);
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// create and initialize the world
dart::simulation::WorldPtr myWorld
= dart::utils::SkelParser::readWorld("dart://sample/skel/fullbody1.skel");
assert(myWorld != nullptr);
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
dart::dynamics::SkeletonPtr skel = myWorld->getSkeleton(1);
std::vector<std::size_t> genCoordIds;
genCoordIds.push_back(1);
genCoordIds.push_back(6); // left hip
genCoordIds.push_back(9); // left knee
genCoordIds.push_back(10); // left ankle
genCoordIds.push_back(13); // right hip
genCoordIds.push_back(16); // right knee
genCoordIds.push_back(17); // right ankle
genCoordIds.push_back(21); // lower back
Eigen::VectorXd initConfig(8);
initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0;
skel->setPositions(genCoordIds, initConfig);
dart::dynamics::Joint* joint0 = skel->getJoint(0);
joint0->setActuatorType(dart::dynamics::Joint::PASSIVE);
for (std::size_t i = 1; i < skel->getNumBodyNodes(); ++i)
{
dart::dynamics::Joint* joint = skel->getJoint(i);
joint->setActuatorType(dart::dynamics::Joint::VELOCITY);
}
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
std::cout << "'h': harness on/off" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Hybrid Dynamics");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[]) {
// create and initialize the world
dart::simulation::World* myWorld
= dart::utils::SkelParser::readSkelFile(
DART_DATA_PATH"skel/fullbody1.skel");
assert(myWorld != NULL);
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
std::vector<int> genCoordIds;
genCoordIds.push_back(1);
genCoordIds.push_back(6); // left hip
genCoordIds.push_back(14); // left knee
genCoordIds.push_back(17); // left ankle
genCoordIds.push_back(9); // right hip
genCoordIds.push_back(15); // right knee
genCoordIds.push_back(19); // right ankle
genCoordIds.push_back(13); // lower back
Eigen::VectorXd initConfig(8);
initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1;
myWorld->getSkeleton(1)->setConfig(genCoordIds, initConfig);
// create controller
Controller* myController = new Controller(myWorld->getSkeleton(1),
myWorld->getConstraintHandler(),
myWorld->getTimeStep());
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
window.setController(myController);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Balance");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// Create a window for rendering and UI
MyWindow window;
// Create a world with 3 particles
MyWorld *world = new MyWorld(3);
// Link the world to the window and start running the glut event loop
window.setWorld(world);
std::cout << "space bar: simulation on/off" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Galileo's experiment");
int a = 0;
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// load a skeleton file
// create and initialize the world
dart::simulation::World* myWorld
= utils::SkelParser::readWorld(DART_DATA_PATH"/skel/chain.skel");
assert(myWorld != NULL);
// create and initialize the world
Eigen::Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
myWorld->setTimeStep(1.0/2000);
int dof = myWorld->getSkeleton(0)->getNumDofs();
Eigen::VectorXd initPose(dof);
initPose.setZero();
initPose[20] = 3.14159 * 0.4;
initPose[23] = 3.14159 * 0.4;
initPose[26] = 3.14159 * 0.4;
initPose[29] = 3.14159 * 0.4;
myWorld->getSkeleton(0)->setPositions(initPose);
myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false);
// create a ball joint constraint
BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6");
BodyNode *bd2 = myWorld->getSkeleton(0)->getBodyNode("link 10");
Eigen::Vector3d offset(0.0, 0.025, 0.0);
Eigen::Vector3d jointPos = bd1->getTransform() * offset;
BallJointConstraint *cl = new BallJointConstraint( bd1, bd2, jointPos);
//WeldJointConstraint *cl = new WeldJointConstraint(bd1, bd2);
myWorld->getConstraintSolver()->addConstraint(cl);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Closed Loop");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[]) {
// load a world
dart::simulation::World* myWorld =
dart::utils::SdfParser::readSdfFile(
DART_DATA_PATH"/sdf/double_pendulum_with_base.world");
assert(myWorld != NULL);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1'--'4': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "SDFormat Loader");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
// load a skeleton file
// create and initialize the world
dart::simulation::WorldPtr myWorld
= dart::utils::SkelParser::readWorld("dart://sample/skel/shapes.skel");
assert(myWorld != NULL);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'q': spawn a random cube" << std::endl;
std::cout << "'w': spawn a random ellipsoid" << std::endl;
std::cout << "'e': spawn a random cylinder" << std::endl;
std::cout << "'a': delete a spawned object at last" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Rigid Shapes");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
////////////////////////////////////////////////////////////////////////////
/// SETUP WORLD
////////////////////////////////////////////////////////////////////////////
// load a robot
DartLoader dart_loader;
string robot_file = VRC_DATA_PATH;
robot_file = argv[1];
World *mWorld = dart_loader.parseWorld(VRC_DATA_PATH ROBOT_URDF);
SkeletonDynamics *robot = mWorld->getSkeleton("atlas");
atlas_state_t state;
state.init(robot);
atlas_kinematics_t kin;
kin.init(robot);
robot->setPose(robot->getPose().setZero());
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(VRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
mWorld->addSkeleton(ground);
// Zero atlas's feet at ground
kinematics::BodyNode* foot = robot->getNode(ROBOT_LEFT_FOOT);
kinematics::Shape* shoe = foot->getCollisionShape();
double body_z = -shoe->getTransform().matrix()(2,3) + shoe->getDim()(2)/2;
body_z += -foot->getWorldTransform()(2,3);
VectorXd robot_dofs = robot->getPose();
cout << "body_z = " << body_z << endl;
state.dofs(2) = body_z;
robot->setPose(state.dart_pose());
cout << robot->getNode(ROBOT_BODY)->getWorldTransform() << endl;
// Zero ground
double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
ground->getNode("ground1_root")->getVisualizationShape()->setColor(Vector3d(0.5,0.5,0.5));
VectorXd ground_dofs = ground->getPose();
ground_dofs(1) = foot->getWorldTransform()(1,3);
ground_dofs(2) = -ground_z;
cout << "ground z = " << ground_z << endl;
ground->setPose(ground_dofs);
////////////////////////////////////////////////////////////////////////////
/// COM IK
////////////////////////////////////////////////////////////////////////////
Vector3d world_com;
Isometry3d end_effectors[NUM_MANIPULATORS];
IK_Mode mode[NUM_MANIPULATORS];
BodyNode* left_foot = state.robot()->getNode(ROBOT_LEFT_FOOT);
BodyNode* right_foot = state.robot()->getNode(ROBOT_RIGHT_FOOT);
end_effectors[MANIP_L_FOOT] = state.robot()->getNode(ROBOT_LEFT_FOOT)->getWorldTransform();
end_effectors[MANIP_R_FOOT] = state.robot()->getNode(ROBOT_RIGHT_FOOT)->getWorldTransform();
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
world_com = state.robot()->getWorldCOM();
world_com(2) -= 0.1;
Isometry3d body;
state.get_body(body);
body.rotate(AngleAxisd(M_PI/4, Vector3d::UnitY()));
state.set_body(body);
kin.com_ik(world_com, end_effectors, mode, state);
robot->setPose(state.dart_pose());
MyWindow window;
window.setWorld(mWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Robot Viz");
glutMainLoop();
}
int main(int argc, char* argv[]) {
using dart::dynamics::BodyNode;
using dart::dynamics::FreeJoint;
using dart::dynamics::MeshShape;
using dart::dynamics::Skeleton;
using dart::simulation::World;
using dart::utils::SkelParser;
// Create and initialize the world
World* myWorld = dart::utils::SkelParser::readSkelFile(
DART_DATA_PATH"/skel/mesh_collision.skel");
// Create a skeleton
Skeleton* MeshSkel = new Skeleton("Mesh Skeleton");
// Always set the root node ( 6DOF for rotation and translation )
FreeJoint* joint;
BodyNode* node;
// Set the initial Rootnode that controls the position and orientation of the
// whole robot
node = new BodyNode("rootBodyNode");
joint = new FreeJoint("rootJoint");
// Add joint to the body node
node->setParentJoint(joint);
// Load a Mesh3DTriangle to save in Shape
const aiScene* m3d = MeshShape::loadMesh(DART_DATA_PATH"/obj/foot.obj");
// Create Shape and assign it to node
MeshShape* Shape0 = new MeshShape(Eigen::Vector3d(1.0, 1.0, 1.0), m3d);
node->addVisualizationShape(Shape0);
node->addCollisionShape(Shape0);
node->setInertia(0.000416667, 0.000416667, 0.000416667);
node->setMass(1.0); // 1 Kg according to cube1.skel
// Add node to Skel
MeshSkel->addBodyNode(node);
// Add MeshSkel to the world
myWorld->addSkeleton(MeshSkel);
// Verify that our skeleton has something inside :)
std::printf("Our skeleton has %d nodes \n", MeshSkel->getNumBodyNodes());
// std::printf("Our skeleton has %d joints \n", MeshSkel->getNumJoints());
std::printf("Our skeleton has %d DOFs \n", MeshSkel->getNumGenCoords());
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'s': simulate one step" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1' and '2': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "meshCollision");
glutMainLoop();
aiReleaseImport(m3d);
return 0;
}
