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;
}
