/** * Creates a new ODE_UniversalJoint. * * @param body1 the first body to connect the joint to. * @param body2 the second body to connect the joint to. * @return the new ODE_UniversalJoint. */ dJointID ODE_UniversalJoint::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get()) || mJointAxis2Point1->get().equals(mJointAxis2Point2->get())) { Core::log("Invalid axes for ODE_UniversalJoint."); return 0; } //if one of the bodyIDs is null, the joint is connected to a static object. dJointID newJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup); dJointAttach(newJoint, body1, body2); Vector3D anchor = mAnchorPoint->get() ; Vector3D axis1; Vector3D axis2; axis1.set(mJointAxis1Point2->getX() - mJointAxis1Point1->getX(), mJointAxis1Point2->getY() - mJointAxis1Point1->getY(), mJointAxis1Point2->getZ() - mJointAxis1Point1->getZ()); axis2.set(mJointAxis2Point2->getX() - mJointAxis2Point1->getX(), mJointAxis2Point2->getY() - mJointAxis2Point1->getY(), mJointAxis2Point2->getZ() - mJointAxis2Point1->getZ()); dJointSetUniversalAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetUniversalAxis1(newJoint, axis1.getX(), axis1.getY(), axis1.getZ()); dJointSetUniversalAxis2(newJoint, axis2.getX(), axis2.getY(), axis2.getZ()); return newJoint; }
OscUniversalODE::OscUniversalODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase *parent, OscObject *object1, OscObject *object2, double x, double y, double z, double a1x, double a1y, double a1z, double a2x, double a2y, double a2z) : OscUniversal(name, parent, object1, object2, x, y, z, a1x, a1y, a1z, a2x, a2y, a2z) { m_response = new OscResponse("response",this); dJointID odeJoint = dJointCreateUniversal(odeWorld,0); m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace, object1, object2); dJointSetUniversalAnchor(odeJoint, x, y, z); dJointSetUniversalAxis1(odeJoint, a1x, a1y, a1z); dJointSetUniversalAxis2(odeJoint, a2x, a2y, a2z); printf("[%s] Universal joint created between %s and %s at (%f, %f, %f) for axes (%f, %f, %f) and (%f,%f,%f)\n", simulation()->type_str(), object1->c_name(), object2?object2->c_name():"world", x, y, z, a1x, a1y, a1z, a2x, a2y, a2z); }
static dJointID create_phys_joint(int t) { switch (t) { case dJointTypeBall: return dJointCreateBall (world, 0); case dJointTypeHinge: return dJointCreateHinge (world, 0); case dJointTypeSlider: return dJointCreateSlider (world, 0); case dJointTypeUniversal: return dJointCreateUniversal(world, 0); case dJointTypeHinge2: return dJointCreateHinge2 (world, 0); default: return 0; } }
static void soy_joints_universal_real_create (soyjointsJoint* base) { soyjointsUniversal * self; soyscenesScene* _tmp0_; struct dxWorld* _tmp1_; struct dxJoint* _tmp2_; self = (soyjointsUniversal*) base; _tmp0_ = ((soyjointsJoint*) self)->scene; _tmp1_ = _tmp0_->world; _tmp2_ = dJointCreateUniversal (_tmp1_, NULL); _dJointDestroy0 (((soyjointsJoint*) self)->joint); ((soyjointsJoint*) self)->joint = _tmp2_; }
IoODEUniversal *IoODEUniversal_rawClone(IoODEUniversal *proto) { IoObject *self = IoODEJoint_rawClone(proto); if(DATA(proto)->jointGroup) { IoODEJointGroup *jointGroup = DATA(proto)->jointGroup; JOINTGROUP = jointGroup; IoODEJointGroup_addJoint(jointGroup, self); JOINTID = dJointCreateUniversal(WORLDID, JOINTGROUPID); } return self; }
/* ================================================================================= createUniversalLeg Use parameters to create leg body/geom and attach to body with universal joint **Warning** mass is not set ================================================================================= */ void createUniversalLeg(ODEObject &leg, ODEObject &bodyAttachedTo, dJointID& joint, dReal xPos, dReal yPos, dReal zPos, dReal xRot, dReal yRot, dReal zRot, dReal radius, dReal length, dReal maxAngle, dReal minAngle, dReal anchorXPos, dReal anchorYPos, dReal anchorZPos) { dMatrix3 legOrient; dRFromEulerAngles(legOrient, xRot, yRot, zRot); //position and orientation leg.Body = dBodyCreate(World); dBodySetPosition(leg.Body, xPos, yPos, zPos); dBodySetRotation(leg.Body, legOrient); dBodySetLinearVel(leg.Body, 0, 0, 0); dBodySetData(leg.Body, (void *)0); //mass dMass legMass; dMassSetCapsule(&legMass, 1, 3, radius, length); //dBodySetMass(leg.Body, &legMass); //geometry leg.Geom = dCreateCapsule(Space, radius, length); dGeomSetBody(leg.Geom, leg.Body); //universal joint joint = dJointCreateUniversal(World, jointgroup); //attach and anchor dJointAttach(joint, bodyAttachedTo.Body, leg.Body); dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos); //axes dJointSetUniversalAxis1(joint, 0, 0, 1); dJointSetUniversalAxis2(joint, 0, 1, 0); //Max and min angles dJointSetUniversalParam(joint, dParamHiStop, maxAngle); dJointSetUniversalParam(joint, dParamLoStop, minAngle); dJointSetUniversalParam(joint, dParamHiStop2, maxAngle); dJointSetUniversalParam(joint, dParamLoStop2, minAngle); }
// NOTE: it is important that rigid bodies are added // (happens in CJoint::Attach()) before joint transforms are set!!! void CUniversalJoint::Attach(dWorldID WorldID, dJointGroupID GroupID, const matrix44& ParentTfm) { ODEJointID = dJointCreateUniversal(WorldID, GroupID); for (int i = 0; i < 2; i++) { //???to some CJoiint::UtilFunc? const CJointAxis& CurrAxis = AxisParams[i]; if (CurrAxis.IsLoStopEnabled) dJointSetUniversalParam(ODEJointID, dParamLoStop + dParamGroup * i, CurrAxis.LoStop); if (CurrAxis.IsHiStopEnabled) dJointSetUniversalParam(ODEJointID, dParamHiStop + dParamGroup * i, CurrAxis.HiStop); dJointSetUniversalParam(ODEJointID, dParamVel + dParamGroup * i, CurrAxis.Velocity); dJointSetUniversalParam(ODEJointID, dParamFMax + dParamGroup * i, CurrAxis.FMax); dJointSetUniversalParam(ODEJointID, dParamFudgeFactor + dParamGroup * i, CurrAxis.FudgeFactor); dJointSetUniversalParam(ODEJointID, dParamBounce + dParamGroup * i, CurrAxis.Bounce); dJointSetUniversalParam(ODEJointID, dParamCFM + dParamGroup * i, CurrAxis.CFM); dJointSetUniversalParam(ODEJointID, dParamStopERP + dParamGroup * i, CurrAxis.StopERP); dJointSetUniversalParam(ODEJointID, dParamStopCFM + dParamGroup * i, CurrAxis.StopCFM); } CJoint::Attach(WorldID, GroupID, ParentTfm); UpdateTransform(ParentTfm); }
/** * This method is called if the joint should be attached. * It creates the ODE-joint, calculates the current anchor-position * and axis-orientation and attaches the Joint. * @param obj1 first ODE-object to attach with * @param obj2 second ODE-object to attach with **/ void UniversalJoint::attachJoint(dBodyID obj1, dBodyID obj2) { gmtl::Vec3f newAnchor, newAxis1, newAxis2, scaleVec; gmtl::Quatf entityRot; gmtl::AxisAnglef axAng; TransformationData entityTrans; joint = dJointCreateUniversal(world, 0); dJointAttach(joint, obj1, obj2); newAxis1 = axis1; newAxis2 = axis2; newAnchor = anchor; if (mainEntity != NULL) { entityTrans = mainEntity->getEnvironmentTransformation(); // get the scale values of the mainEntity // scaleVec[0] = mainEntity->getXScale(); // scaleVec[1] = mainEntity->getYScale(); // scaleVec[2] = mainEntity->getZScale(); scaleVec = entityTrans.scale; // scale Anchor-offset by mainEntity-scale value newAnchor[0] *= scaleVec[0]; newAnchor[1] *= scaleVec[1]; newAnchor[2] *= scaleVec[2]; // scale Axes by mainEntity-scale value because of possible distortion newAxis1[0] *= scaleVec[0]; newAxis1[1] *= scaleVec[1]; newAxis1[2] *= scaleVec[2]; gmtl::normalize(newAxis1); newAxis2[0] *= scaleVec[0]; newAxis2[1] *= scaleVec[1]; newAxis2[2] *= scaleVec[2]; gmtl::normalize(newAxis2); // get the Rotation of the mainEntity // axAng[0] = mainEntity->getRotAngle(); // axAng[1] = mainEntity->getXRot(); // axAng[2] = mainEntity->getYRot(); // axAng[3] = mainEntity->getZRot(); // gmtl::set(entityRot, axAng); entityRot = entityTrans.orientation; // rotate Axes by mainEntity-rotation newAxis1 *= entityRot; newAxis2 *= entityRot; // rotate Anchor-offset by mainEntity-rotation newAnchor *= entityRot; // transform new Anchor to world coordinates // newAnchor[0] += mainEntity->getXTrans(); // newAnchor[1] += mainEntity->getYTrans(); // newAnchor[2] += mainEntity->getZTrans(); newAnchor += entityTrans.position; } // if dJointSetUniversalAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]); dJointSetUniversalAxis1(joint, newAxis1[0], newAxis1[1], newAxis1[2]); dJointSetUniversalAxis2(joint, newAxis2[0], newAxis2[1], newAxis2[2]); } // attachJoint
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 }
// called by Webots at the beginning of the simulation void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) { int i; // store global objects for later use world = w; space = s; contact_joint_group = j; // get floor geometry floor_geom = getGeom(floor_name); if (!floor_geom) return; // get foot geometry and body id's for (i = 0; i < N_FEET; i++) { foot_geom[i] = getGeom(foot_name[i]); if (!foot_geom[i]) return; foot_body[i] = dGeomGetBody(foot_geom[i]); if (!foot_body[i]) return; } // create universal joints for linear actuators for (i = 0; i < 10; i++) { dBodyID upper_piston = getBody(upper_piston_name[i]); dBodyID lower_piston = getBody(lower_piston_name[i]); dBodyID upper_link = getBody(upper_link_name[i]); dBodyID lower_link = getBody(lower_link_name[i]); if (!upper_piston || !lower_piston || !upper_link || !lower_link) return; // create a ball and socket joint (3 DOFs) to attach the lower piston body to the lower link // we don't need a universal joint here, because the piston's passive rotation is prevented // by the universal joint at its upper end. dJointID lower_balljoint = dJointCreateBall(world, 0); dJointAttach(lower_balljoint, lower_piston, lower_link); // transform attachement point from local to global coordinate system // warning: this is a hard-coded translation dVector3 lower_ball; dBodyGetRelPointPos(lower_piston, 0, 0, -0.075, lower_ball); // set attachement point (anchor) dJointSetBallAnchor(lower_balljoint, lower_ball[0], lower_ball[1], lower_ball[2]); // create a universal joint (2 DOFs) to attach upper piston body to upper link // we need to use a universal joint to prevent the piston from passively rotating around its long axis dJointID upper_ujoint = dJointCreateUniversal(world, 0); dJointAttach(upper_ujoint, upper_piston, upper_link); // transform attachement point from local to global coordinate system // warning: this is a hard-coded translation dVector3 upper_ball; dBodyGetRelPointPos(upper_piston, 0, 0, 0, upper_ball); // set attachement point (anchor) dJointSetUniversalAnchor(upper_ujoint, upper_ball[0], upper_ball[1], upper_ball[2]); // set the universal joint axes dVector3 upper_xaxis; dVector3 upper_yaxis; dBodyVectorToWorld(upper_piston, 1, 0, 0, upper_xaxis); dBodyVectorToWorld(upper_piston, 0, 1, 0, upper_yaxis); dJointSetUniversalAxis1(upper_ujoint, upper_xaxis[0], upper_xaxis[1], upper_xaxis[2]); dJointSetUniversalAxis2(upper_ujoint, upper_yaxis[0], upper_yaxis[1], upper_yaxis[2]); } }
int setupTest (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 1: { // 1 body to static env constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } case 2: { // 2 body with relative rotation constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,-0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 3: { // 1 body to static env with relative rotation constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0.25*M_PI,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } // ********** hinge joint case 200: // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,1,-1,1.41421356); return 1; case 220: // hinge angle polarity test case 221: // hinge angle rate test constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); max_iterations = 50; return 1; case 230: // hinge motor rate (and polarity) test case 231: // ...with stops constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); dJointSetHingeParam (joint,dParamFMax,1); if (n==231) { dJointSetHingeParam (joint,dParamLoStop,-0.5); dJointSetHingeParam (joint,dParamHiStop,0.5); } return 1; case 250: // limit bounce test (gravity down) case 251: { // ...gravity up constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,1,0); dJointSetHingeParam (joint,dParamLoStop,-0.9); dJointSetHingeParam (joint,dParamHiStop,0.7854); dJointSetHingeParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** slider case 300: // 2 body constructWorldForTest (0,2, 0,0,1, 0.2,0.2,1.2, 0,0,1, -1,1,0, 0,0.25*M_PI); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,1,1,1); return 1; case 320: // slider angle polarity test case 321: // slider angle rate test constructWorldForTest (0,2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); max_iterations = 50; return 1; case 330: // slider motor rate (and polarity) test case 331: // ...with stops constructWorldForTest (0, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamFMax,100); if (n==331) { dJointSetSliderParam (joint,dParamLoStop,-0.4); dJointSetSliderParam (joint,dParamHiStop,0.4); } return 1; case 350: // limit bounce tests case 351: { constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamLoStop,-0.5); dJointSetSliderParam (joint,dParamHiStop,0.5); dJointSetSliderParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test case 421: // hinge-2 steering angle rate test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); max_iterations = 50; return 1; case 430: // hinge 2 steering motor rate (+polarity) test case 431: // ...with stops case 432: // hinge 2 wheel motor rate (+polarity) test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); dJointSetHinge2Param (joint,dParamFMax,1); dJointSetHinge2Param (joint,dParamFMax2,1); if (n==431) { dJointSetHinge2Param (joint,dParamLoStop,-0.5); dJointSetHinge2Param (joint,dParamHiStop,0.5); } return 1; // ********** angular motor joint case 600: // test euler angle calculations constructWorldForTest (0,2, -SIDE*0.5,0,1, SIDE*0.5,0,1, 0,0,1, 0,0,1, 0,0); joint = dJointCreateAMotor (world,0); dJointAttach (joint,body[0],body[1]); dJointSetAMotorNumAxes (joint,3); dJointSetAMotorAxis (joint,0,1, 0,0,1); dJointSetAMotorAxis (joint,2,2, 1,0,0); dJointSetAMotorMode (joint,dAMotorEuler); max_iterations = 200; return 1; // ********** universal joint case 700: // 2 body case 701: case 702: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); return 1; case 720: // universal transmit torque test case 721: case 722: case 730: // universal torque about axis 1 case 731: case 732: case 740: // universal torque about axis 2 case 741: case 742: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint,0,0,1); dJointSetUniversalAxis2 (joint, 1, -1,0); max_iterations = 100; return 1; } return 0; }
void CODEUniversalJoint::create(dWorld& world, dJointGroupID groupID) { mID = dJointCreateUniversal(world, groupID); }
void UniversalJoint::createJointPhysics() { physicalJoint = dJointCreateUniversal(*simulation->getPhysicalWorld(), 0); PhysicalObject* o1 = getPhysicalParentObject(); PhysicalObject* o2 = getPhysicalChildObject(this); assert (o1 && o2); dJointAttach(physicalJoint, *(o2->getBody()), *(o1->getBody())); //set joint parameter dJointSetUniversalAnchor(physicalJoint, dReal(anchorPoint.v[0]), dReal(anchorPoint.v[1]), dReal(anchorPoint.v[2])); Vector3d physAxis(axis1->direction); physAxis.rotate(rotation); dJointSetUniversalAxis1(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2])); physAxis = axis2->direction; physAxis.rotate(rotation); dJointSetUniversalAxis2(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2])); if(axis1->cfm != -1.0) dJointSetUniversalParam(physicalJoint, dParamCFM, dReal(axis1->cfm)); if(axis2->cfm != -1.0) dJointSetUniversalParam(physicalJoint, dParamCFM2, dReal(axis2->cfm)); if(axis1->limited || (axis1->motor != 0 && axis1->motor->getMotorType() == ANGULAR)) { double minAxisLimit(Functions::toRad(axis1->minValue)); double maxAxisLimit(Functions::toRad(axis1->maxValue)); //Set physical limits to higher values (+10%) to avoid strange hinge effects. //Otherwise, sometimes the motor exceeds the limits. assert(maxAxisLimit >= minAxisLimit); double totalAxisRange(maxAxisLimit - minAxisLimit); double internalTolerance(totalAxisRange / 10); minAxisLimit -= internalTolerance; maxAxisLimit += internalTolerance; dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit)); dJointSetUniversalParam(physicalJoint, dParamHiStop, dReal(maxAxisLimit)); // this has to be done due to the way ODE sets joint stops dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit)); if(axis1->fullScaleDeflectionCFM != -1.0) dJointSetUniversalParam(physicalJoint, dParamStopCFM, dReal(axis1->fullScaleDeflectionCFM)); if(axis1->fullScaleDeflectionERP != -1.0) dJointSetUniversalParam(physicalJoint, dParamStopERP, dReal(axis1->fullScaleDeflectionERP)); } if(axis2->limited || (axis2->motor != 0 && axis2->motor->getMotorType() == ANGULAR)) { double minAxisLimit(Functions::toRad(axis2->minValue)); double maxAxisLimit(Functions::toRad(axis2->maxValue)); //Set physical limits to higher values (+10%) to avoid strange hinge effects. //Otherwise, sometimes the motor exceeds the limits. assert(maxAxisLimit >= minAxisLimit); double totalAxisRange(maxAxisLimit - minAxisLimit); double internalTolerance(totalAxisRange / 10); minAxisLimit -= internalTolerance; maxAxisLimit += internalTolerance; dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit)); dJointSetUniversalParam(physicalJoint, dParamHiStop2, dReal(maxAxisLimit)); // this has to be done due to the way ODE sets joint stops dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit)); if(axis2->fullScaleDeflectionCFM != -1.0) dJointSetUniversalParam(physicalJoint, dParamStopCFM2, dReal(axis2->fullScaleDeflectionCFM)); if(axis2->fullScaleDeflectionERP != -1.0) dJointSetUniversalParam(physicalJoint, dParamStopERP2, dReal(axis2->fullScaleDeflectionERP)); } }
dJointID ODE_U_FrictionTorqueMotorModel::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxis1Point1 == 0 || mJointAxis1Point2 == 0 || mJointAxis2Point1 == 0 || mJointAxis2Point2 == 0) { Core::log("ODE_U_FrictionTorqueMotorModel: Could not find all required parmaeter Values."); return 0; } if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get(), -1) || mJointAxis2Point1->get().equals(mJointAxis2Point2->get(), -1)) { Core::log("Invalid axes for ODE_U_FrictionTorqueMotorModel: " + mJointAxis1Point1->getValueAsString() + " " + mJointAxis1Point2->getValueAsString() + " " + mJointAxis2Point1->getValueAsString() + " " + mJointAxis2Point2->getValueAsString()); return 0; } Vector3D anchor = mAnchor->get(); Vector3D axis1 = mJointAxis1Point2->get() - mJointAxis1Point1->get(); Vector3D axis2 = mJointAxis2Point2->get() - mJointAxis2Point1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorNumAxes(joint, 2); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get()); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis1.normalize(); Vector3D perpedicular1; if(axis1.getY() != 0.0 || axis1.getX() != 0.0) { perpedicular1.set(-axis1.getY(), axis1.getX(), 0); } else { perpedicular1.set(0, -axis1.getZ(), axis1.getY()); } perpedicular1.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. // For different constellations of static and dynamic bodies, the turn direction // of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis1.getX(), -axis1.getY(), -axis1.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis1.getX(), axis1.getY(), axis1.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular1.getX(), -perpedicular1.getY(), -perpedicular1.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular1.getX(), perpedicular1.getY(), perpedicular1.getZ()); } mUniversalJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup); dJointAttach(mUniversalJoint, body1, body2); dJointSetUniversalAnchor(mUniversalJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetUniversalAxis1(mUniversalJoint, axis1.getX(), axis1.getY(), axis1.getZ()); dJointSetUniversalAxis2(mUniversalJoint, axis2.getX(), axis2.getY(), axis2.getZ()); double minAngle1 = (mAxis1MinAngle->get() * Math::PI) / 180.0; double minAngle2 = (mAxis2MinAngle->get() * Math::PI) / 180.0; double maxAngle1 = (mAxis1MaxAngle->get() * Math::PI) / 180.0; double maxAngle2 = (mAxis2MaxAngle->get() * Math::PI) / 180.0; // if(body1 == 0) { // double tmp = // mMinAngle = -1.0 * mMaxAngle; // mMaxAngle = -1.0 * tmp; // } dJointSetUniversalParam(mUniversalJoint, dParamLoStop, minAngle1); dJointSetUniversalParam(mUniversalJoint, dParamHiStop, maxAngle1); dJointSetUniversalParam(mUniversalJoint, dParamLoStop2, minAngle2); dJointSetUniversalParam(mUniversalJoint, dParamHiStop2, maxAngle2); dJointSetUniversalParam(mUniversalJoint, dParamVel, 0.0); dJointSetUniversalParam(mUniversalJoint, dParamVel2, 0.0); return joint; }