void PhysicsBody::setCapsuleMassTotal( Real32 totalMass, Int32 direction, Real32 radius, Real32 length ) { dMass mass; dMassSetCapsuleTotal(&mass, totalMass, direction, radius, length); setMassStruct(mass); }
void createPart(const dWorldID & _world , dPart & part, dReal * pos3){ dMass mass; part.body = dBodyCreate(_world); dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position dMassSetZero(&mass); // Set mass parameter to zero dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length); // Calculate mass parameter dBodySetMass(part.body, &mass); // Set mass }
void capsule::set_mass(double mass) { if(body_id) { dMass dmass; dMassSetZero(&dmass); dMassSetSphereTotal(&dmass,mass,radius); dMassSetCapsuleTotal(&dmass,mass,3,radius,length); dBodySetMass (body_id, &dmass); material.mass = mass; } }
void cPhysicsObject::CreateCapsule(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot) { geom = dCreateCapsule(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(), fRadius, fLength); InitCommon(pWorld, pos, rot); if (bBody) { dMass mass; dMassSetCapsuleTotal(&mass, fMassKg, uiDirectionX, 2.0f*fRadius, fLength); dBodySetMass(body, &mass); } }
/*** ロボットアームの生成 ***/ void makeArm() { dMass mass; // 質量パラメータ dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量 dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径 dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z // リンクの生成 for (int i = 0; i < NUM-1; i++) { rlink[i].body = dBodyCreate(world); dBodySetPosition(rlink[i].body, x[i], y[i], z[i]); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]); dBodySetMass(rlink[i].body, &mass); rlink[i].geom = dCreateCapsule(space,r[i],length[i]); dGeomSetBody(rlink[i].geom,rlink[i].body); } rlink[NUM-1].body = dBodyCreate(world); dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]); dMassSetZero(&mass); dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4); dBodySetMass(rlink[NUM-1].body, &mass); rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4); dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body); // ジョイントの生成とリンクへの取り付け joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント dJointAttach(joint[0], rlink[0].body, 0); dJointSetFixed(joint[0]); for (int j = 1; j < NUM; j++) { joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント dJointAttach(joint[j], rlink[j].body, rlink[j-1].body); dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]); dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]); } }
int main(int argc, char *argv[]) { dsFunctions fn; double x[NUM] = {0.00}, y[NUM] = {0.00}; // Center of gravity double z[NUM] = { 0.05, 0.50, 1.50, 2.55}; double m[NUM] = {10.00, 2.00, 2.00, 2.00}; // mass double anchor_x[NUM] = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00}; double axis_x[NUM] = { 0.00, 0.00, 0.00, 0.00}; // axises of joints double axis_y[NUM] = { 0.00, 0.00, 1.00, 1.00}; double axis_z[NUM] = { 1.00, 1.00, 0.00, 0.00}; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &command; fn.path_to_textures = "../../drawstuff/textures"; dInitODE(); // Initialize ODE world = dWorldCreate(); // Create a world dWorldSetGravity(world, 0, 0, -9.8); for (int i = 0; i < NUM; i++) { dMass mass; link[i] = dBodyCreate(world); dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position dMassSetZero(&mass); // Set mass parameter to zero dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]); // Calculate mass parameter dBodySetMass(link[i], &mass); // Set mass } joint[0] = dJointCreateFixed(world, 0); // A fixed joint dJointAttach(joint[0], link[0], 0); // Attach the joint between the ground and the base dJointSetFixed(joint[0]); // Set the fixed joint for (int j = 1; j < NUM; j++) { joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]); dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]); } dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop dCloseODE(); return 0; }
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3], const float r[16]) { dVector3 v; dMatrix3 M; dReal rad; dReal len; dMass add; if (r) set_rotation(M, r); if (dGeomGetClass(geom) != dPlaneClass) { dReal m = get_data(geom)->mass; /* Create a new mass for the given geom. */ switch (dGeomGetClass(geom)) { case dBoxClass: dGeomBoxGetLengths(geom, v); dMassSetBoxTotal(&add, m, v[0], v[1], v[2]); break; case dSphereClass: rad = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&add, m, rad); break; case dCapsuleClass: dGeomCapsuleGetParams(geom, &rad, &len); dMassSetCapsuleTotal(&add, m, 3, rad, len); break; default: dMassSetZero(&add); break; } /* Transform the geom and mass to the given position and rotation. */ if(dGeomGetBody(geom)) { if (p) { dGeomSetOffsetPosition(geom, p[0], p[1], p[2]); dMassTranslate (&add, p[0], p[1], p[2]); } if (r) { dGeomSetOffsetRotation(geom, M); dMassRotate (&add, M); } } else { if (p) dGeomSetPosition(geom, p[0], p[1], p[2]); if (r) dGeomSetRotation(geom, M); } /* Accumulate the new mass with the body's existing mass. */ dMassAdd(mass, &add); } }
void makeRobot_Nleg() { for(int segment = 0; segment < BODY_SEGMENTS; ++segment) { dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT; dReal torso_m = 50.0; // Mass of body // torso_m[segment] = 10.0; dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments //for four legs // dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate) // {-cx1,-cx1,-cx1}}; dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate) {0,0,0}}; dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate) {-cy1,-cy1,-cy1}}; dReal z[num_legs][num_links] = { // Position of each link (z coordinate) {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}, {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} }; dReal r[num_links] = { r1, r2, r3}; // radius of leg segment dReal length[num_links] = { l1, l2, l3}; // Length of leg segment dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment // //for one leg // dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}}; // dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}}; // dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}}; //for four legs dReal axis_x[num_legs][num_links]; dReal axis_y[num_legs][num_links]; dReal axis_z[num_legs][num_links]; for(int i = 0; i < num_legs; ++i) { axis_x[i][0] = 0.0; axis_x[i][1] = 1.0; axis_x[i][2] = 0.0; axis_y[i][0] = 1.0; axis_y[i][1] = 0.0; axis_y[i][2] = 1.0; axis_z[i][0] = 0.0; axis_z[i][1] = 0.0; axis_z[i][2] = 0.0; } // For mation of the body dMass mass; torso[segment].body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass,torso_m, lx, ly, lz); dBodySetMass(torso[segment].body,&mass); torso[segment].geom = dCreateBox(space,lx, ly, lz); dGeomSetBody(torso[segment].geom, torso[segment].body); dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); // Formation of leg dMatrix3 R; // Revolution queue dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { THETA[segment][i][j] = 0; leg[segment][i][j].body = dBodyCreate(world); if (j == 0) dBodySetRotation(leg[segment][i][j].body,R); dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]); dBodySetMass(leg[segment][i][j].body, &mass); //if(i==1 and j==2) //to set the length of one leg differently //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg //else leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body); } } // Formation of joints (and connecting them up) for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { leg[segment][i][j].joint = dJointCreateHinge(world, 0); if (j == 0){ dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop, .50*M_PI); } else dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body); dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]); dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]); } } } #ifdef USE_NLEG_ROBOT // link torsos for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) { dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT; switch (hingeType) { case 1: //Hinge Joint, X axis (back-breaker) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0); break; case 2: //Hinge Joint, Y axis (???) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0); break; case 3: //Hinge Joint, Z axis (snake-like) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0); break; case 4: // Slider, Y axis (??) torso[segment].joint = dJointCreateSlider(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0); break; case 5: // Slider, X axis (extendo) torso[segment].joint = dJointCreateSlider(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0); break; case 6: //Universal Joint torso[segment].joint = dJointCreateUniversal(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0); dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0); break; case 7: //Ball Joint torso[segment].joint = dJointCreateBall(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); break; case 8: // Fixed torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0); dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees dJointSetHingeParam(torso[segment].joint, dParamHiStop, 0.00*M_PI); break; default: assert(false); // not a valid hinge type break; } } #endif }
/*** Formation of robot ***/ void makeRobot() { dReal torso_m = 10.0; // Mass of body dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments //for four legs dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate) {-cx1,-cx1,-cx1},{ cx1, cx1, cx1}}; dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate) {-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}}; dReal z[num_legs][num_links] = { // Position of each link (z coordinate) {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}, {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}, {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}, {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}}; dReal r[num_links] = { r1, r2, r3}; // radius of leg segment dReal length[num_links] = { l1, l2, l3}; // Length of leg segment dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment // //for one leg // dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}}; // dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}}; // dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}}; //for four legs dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}}; dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}}; dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}}; // For mation of the body dMass mass; torso[0].body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass,torso_m, lx, ly, lz); dBodySetMass(torso[0].body,&mass); torso[0].geom = dCreateBox(space, lx, ly, lz); dGeomSetBody(torso[0].geom, torso[0].body); dBodySetPosition(torso[0].body, SX, SY, SZ); // Formation of leg dMatrix3 R; // Revolution queue dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { leg[0][i][j].body = dBodyCreate(world); if (j == 0) dBodySetRotation(leg[0][i][j].body,R); dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]); dBodySetMass(leg[0][i][j].body, &mass); //if(i==1 and j==2) //to set the length of one leg differently //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg //else leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body); } } // Formation of joints (and connecting them up) for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { leg[0][i][j].joint = dJointCreateHinge(world, 0); if (j == 0){ dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop, .5*M_PI); } else dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body); dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]); dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]); } } }
void dMassSetCappedCylinderTotal(dMass *a, dReal b, int c, dReal d, dReal e) { return dMassSetCapsuleTotal(a,b,c,d,e); }