int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
int i;
contactgroup.create ();
world.setGravity (0,0,-0.5);
dWorldSetCFM (world.id(),1e-5);
dPlane plane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i].create (world);
dReal k = i*SIDE;
body[i].setPosition (k,k,k+0.4);
dMass m;
m.setBox (1,SIDE,SIDE,SIDE);
m.adjust (MASS);
body[i].setMass (&m);
body[i].setData ((void*)(size_t)i);
box[i].create (space,SIDE,SIDE,SIDE);
box[i].setBody (body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i].create (world);
joint[i].attach (body[i],body[i+1]);
dReal k = (i+0.5)*SIDE;
joint[i].setAnchor (k,k,k+0.4);
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dCloseODE();
return 0;
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
// @@@ it's still more convenient to use the C interface here.
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
dJointAttach (c,b1,b2);
}
}
static void simLoop (int pause)
{
if (!pause) {
static double angle = 0;
angle += 0.05;
body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
space.collide (0,&nearCallback);
world.step (0.05);
// remove all contact joints
contactgroup.empty();
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<NUM; i++)
dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if (argc >= 2 ) {
for (int i=1; i < argc; ++i) {
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
type = dJointTypePR;
if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
world.setERP (0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
// Create the body attached to the World
body[W].create (world);
// Main axis of cylinder is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[W], body[W]);
dGeomSetCategoryBits (geom[W], catBits[W]);
dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
body[W].setMass(m);
// Create the dandling body
body[D].create(world);
// Main axis of capsule is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[D], body[D]);
dGeomSetCategoryBits (geom[D], catBits[D]);
dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
body[D].setMass(&m);
// Create the external part of the slider joint
geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
dGeomSetCollideBits (geom[EXT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
// Create the internal part of the slider joint
geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
INT_EXT_RATIO*extDim[Y],
INT_EXT_RATIO*extDim[Z]);
dGeomSetCategoryBits (geom[INT], catBits[INT]);
dGeomSetCollideBits (geom[INT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
dMatrix3 R;
dGeomID id;
// Create the first axis of the universal joi9nt
geom[AXIS1] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
dGeomSetRotation (geom[AXIS1], R);
dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
dGeomSetCollideBits (geom[AXIS1],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS1];
dGeomTransformSetGeom (geom[AXIS1], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the second axis of the universal joint
geom[AXIS2] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
dGeomSetRotation (geom[AXIS2], R);
dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
dGeomSetCollideBits (geom[AXIS2],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS2];
dGeomTransformSetGeom (geom[AXIS2], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the anchor
geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
dGeomSetCollideBits (geom[ANCHOR],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
if (body[W]) {
body[W].setPosition(0, 0, 5);
}
if (geom[EXT]) {
dGeomSetPosition (geom[EXT], 0,0,3.8);
}
if (geom[INT]) {
dGeomSetPosition (geom[INT], 0,0,2.6);
}
if (geom[AXIS1]) {
dGeomSetPosition (geom[AXIS1], 0,0,2.5);
}
if (geom[AXIS2]) {
dGeomSetPosition (geom[AXIS2], 0,0,2.5);
}
if (geom[ANCHOR]) {
dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
}
if (body[D]) {
body[D].setPosition(0,0,1.5);
}
// Attache the upper box to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[W]);
dJointSetFixed (fixed );
if (type == dJointTypePR) {
dPRJoint *pr = new dPRJoint (world, 0);
pr->attach (body[W], body[D]);
pr->setAxis1 (0, 0, -1);
pr->setAxis2 (1, 0, 0);
joint = pr;
dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
}
else {
dPUJoint *pu = new dPUJoint (world, 0);
pu->attach (body[W], body[D]);
pu->setAxis1 (1, 0, 0);
pu->setAxis2 (0, 1, 0);
pu->setAxisP (0, 0, -1);
joint = pu;
dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
}
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'h' : case 'H' : case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' : case 'Q' :
body[D].addForce(40,0,0);
break;
case 'w' : case 'W' :
body[D].addForce(-40,0,0);
break;
case 'a' : case 'A' :
body[D].addForce(0,40,0);
break;
case 's' : case 'S' :
body[D].addForce(0,-40,0);
break;
case 'z' : case 'Z' :
body[D].addForce(0,0,40);
break;
case 'x' : case 'X' :
body[D].addForce(0,0,-40);
break;
// Torque
case 'e': case 'E':
body[D].addTorque(0.1,0,0);
break;
case 'r': case 'R':
body[D].addTorque(-0.1,0,0);
break;
case 'd': case 'D':
body[D].addTorque(0, 0.1,0);
break;
case 'f': case 'F':
body[D].addTorque(0,-0.1,0);
break;
case 'c': case 'C':
body[D].addTorque(0,0,0.1);
break;
case 'v': case 'V':
body[D].addTorque(0,0,0.1);
break;
// Velocity of joint
case ',': case '<' : {
dReal vel = joint->getParam (dParamVel3) - VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.': case '>' : {
dReal vel = joint->getParam (dParamVel3) + VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'l': case 'L' : {
dReal aLimit, lLimit, fmax;
if ( joint->getParam (dParamFMax) ) {
aLimit = dInfinity;
lLimit = dInfinity;
fmax = 0;
}
else {
aLimit = 0.25*PI;
lLimit = 0.5*axDim[LENGTH];
fmax = 0.02;
}
joint->setParam (dParamFMax1, fmax);
joint->setParam (dParamFMax2, fmax);
joint->setParam (dParamFMax3, fmax);
switch (joint->getType() ) {
case dJointTypePR : {
dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
pr->setParam (dParamLoStop, -lLimit);
pr->setParam (dParamHiStop, -lLimit);
pr->setParam (dParamLoStop2, aLimit);
pr->setParam (dParamHiStop2, -aLimit);
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
pu->setParam (dParamLoStop1, -aLimit);
pu->setParam (dParamHiStop1, aLimit);
pu->setParam (dParamLoStop2, -aLimit);
pu->setParam (dParamHiStop2, aLimit);
pu->setParam (dParamLoStop3, -lLimit);
pu->setParam (dParamHiStop3, lLimit);
}
break;
default: {} // keep the compiler happy
}
}
break;
case 'g': case 'G' : {
dVector3 g;
world.getGravity(g);
if ( g[2]< -0.1 )
world.setGravity(0, 0, 0);
else
world.setGravity(0, 0, -0.5);
}
case 'p' :case 'P' : {
switch (joint->getType() ) {
case dJointTypeSlider : {
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
std::cout<<"Position ="<<pu->getPosition() <<"\n";
std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
}
break;
default: {} // keep the compiler happy
}
}
break;
}
}
//===========================================================================================
//! \brief シミュレーションオブジェクトを作成
void create_world( void )
//===========================================================================================
{
// acrobot の回転軸（以下，支柱）は ここでは 直方体(Box)にしています．
const dReal param_h0 = 0.05; // 支柱（直方体）の高さ[m]
const dReal param_wx0 = 0.05; // 同幅(x)
const dReal param_wy0 = 0.80; // 同幅(y)
const dReal param_z0 = 1.20; // 支柱の垂直位置[m]
const dReal param_l1 = 0.50; // 第1リンク（支柱に近いリンク）の長さ[m]
const dReal param_d1 = 0.15; // 同直径[m]
const dReal param_l2 = 0.50; // 第2リンク（支柱に近いリンク）の長さ[m]
const dReal param_d2 = 0.15; // 同直径[m]
const dReal density = 1000.0; // 各リンクの密度[kg/m^3]. 参考(?)`人体の密度' は 900~1100 kg/m^3 (wikipedia)
int i;
contactgroup.create (0);
world.setGravity (0,0,-9.8); // 重力 [m/s^2]
dWorldSetCFM (world.id(),1e-5);
plane.create (space,0,0,1,0); // 地面（平面）．
i=0; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0); // 支柱の中心座標
dReal xx=param_wx0, yy=param_wy0, zz=param_h0;
dMass m;
m.setBox (density,xx,yy,zz);
body[i].setMass (&m);
LinkBase.create (space,xx,yy,zz);
LinkBase.setBody (body[i]);
}
i=1; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0-0.5*param_l1); // リンク1の中心座標
dReal rad=0.5*param_d1, len=param_l1-2.0*rad;
dMass m;
m.setCappedCylinder (density,3,rad,len); // direction(3): z-axis
body[i].setMass (&m);
Link1.create (space,rad,len);
Link1.setBody (body[i]);
}
i=2; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0-param_l1-0.5*param_l2); // リンク2の中心座標
dReal rad=0.5*param_d2, len=param_l2-2.0*rad;
dMass m;
m.setCappedCylinder (density,3,rad,len); // direction(3): z-axis
body[i].setMass (&m);
Link2.create (space,rad,len);
Link2.setBody (body[i]);
}
i=0; {
const dReal *pos = body[0].getPosition();
joint[i].create (world);
joint[i].attach (body[0],body[1]);
joint[i].setAnchor (pos[0],pos[1],pos[2]); // 回転中心=支柱の中心(=原点)
joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
// joint[i].setParam (dParamHiStop, +0.5*M_PI); // 関節の可動範囲を制約するときに使う
// joint[i].setParam (dParamLoStop, -0.5*M_PI); // acrobot の場合は省略
}
i=1; {
const dReal *pos = body[1].getPosition();
joint[i].create (world);
joint[i].attach (body[1],body[2]);
joint[i].setAnchor (pos[0],pos[1],pos[2]-0.5*param_l1); // 回転中心=リンク1とリンク2の間
joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
}
base_joint.create(world);
base_joint.attach(body[0].id(),0); // 支柱(body[0]) と 平面(0)の間の固定リンク．支柱が固定される．
base_joint.set();
}
