/** * 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; }
static void _soy_joints_universal_axis2_set (soyjointsUniversal* self, soyatomsAxis* value) { soyatomsAxis* _tmp0_; soyatomsAxis* _tmp1_; soyatomsAxis* axis2; soyscenesScene* _tmp2_; struct dxJoint* _tmp3_; gfloat _tmp4_; gfloat _tmp5_; gfloat _tmp6_; gfloat _tmp7_; gfloat _tmp8_; gfloat _tmp9_; soyscenesScene* _tmp10_; g_return_if_fail (self != NULL); g_return_if_fail (value != NULL); _tmp0_ = value; _tmp1_ = soy_atoms_axis_new_normalize (_tmp0_); axis2 = _tmp1_; _tmp2_ = ((soyjointsJoint*) self)->scene; g_rw_lock_writer_lock (&_tmp2_->stepLock); _tmp3_ = ((soyjointsJoint*) self)->joint; _tmp4_ = soy_atoms_axis_get_x (axis2); _tmp5_ = _tmp4_; _tmp6_ = soy_atoms_axis_get_y (axis2); _tmp7_ = _tmp6_; _tmp8_ = soy_atoms_axis_get_z (axis2); _tmp9_ = _tmp8_; dJointSetUniversalAxis2 ((struct dxJoint*) _tmp3_, (dReal) _tmp5_, (dReal) _tmp7_, (dReal) _tmp9_); _tmp10_ = ((soyjointsJoint*) self)->scene; g_rw_lock_writer_unlock (&_tmp10_->stepLock); _g_object_unref0 (axis2); }
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 void set_phys_joint_axis_2(dJointID j, const float v[3]) { switch (dJointGetType(j)) { case dJointTypeHinge2: dJointSetHinge2Axis2 (j, v[0], v[1], v[2]); break; case dJointTypeUniversal: dJointSetUniversalAxis2(j, v[0], v[1], v[2]); break; default: break; } }
void CUniversalJoint::UpdateTransform(const matrix44& Tfm) { vector3 p = Tfm * Anchor; dJointSetUniversalAnchor(ODEJointID, p.x, p.y, p.z); matrix33 m33(Tfm.x_component(), Tfm.y_component(), Tfm.z_component()); vector3 a0 = m33 * AxisParams[0].Axis; vector3 a1 = m33 * AxisParams[1].Axis; dJointSetUniversalAxis1(ODEJointID, a0.x, a0.y, a0.z); dJointSetUniversalAxis2(ODEJointID, a1.x, a1.y, a1.z); }
/* ================================================================================= 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); }
/** * 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
static void soy_joints_universal_real_setup (soyjointsJoint* base, soyatomsPosition* anchor, soyatomsAxis* axis1, soyatomsAxis* axis2) { soyjointsUniversal * self; struct dxJoint* _tmp0_; soyatomsPosition* _tmp1_; gfloat _tmp2_; gfloat _tmp3_; soyatomsPosition* _tmp4_; gfloat _tmp5_; gfloat _tmp6_; soyatomsPosition* _tmp7_; gfloat _tmp8_; gfloat _tmp9_; struct dxJoint* _tmp10_; soyatomsAxis* _tmp11_; gfloat _tmp12_; gfloat _tmp13_; soyatomsAxis* _tmp14_; gfloat _tmp15_; gfloat _tmp16_; soyatomsAxis* _tmp17_; gfloat _tmp18_; gfloat _tmp19_; struct dxJoint* _tmp20_; soyatomsAxis* _tmp21_; gfloat _tmp22_; gfloat _tmp23_; soyatomsAxis* _tmp24_; gfloat _tmp25_; gfloat _tmp26_; soyatomsAxis* _tmp27_; gfloat _tmp28_; gfloat _tmp29_; self = (soyjointsUniversal*) base; _tmp0_ = ((soyjointsJoint*) self)->joint; _tmp1_ = anchor; _tmp2_ = soy_atoms_position_get_x (_tmp1_); _tmp3_ = _tmp2_; _tmp4_ = anchor; _tmp5_ = soy_atoms_position_get_y (_tmp4_); _tmp6_ = _tmp5_; _tmp7_ = anchor; _tmp8_ = soy_atoms_position_get_z (_tmp7_); _tmp9_ = _tmp8_; dJointSetUniversalAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_); _tmp10_ = ((soyjointsJoint*) self)->joint; _tmp11_ = axis1; _tmp12_ = soy_atoms_axis_get_x (_tmp11_); _tmp13_ = _tmp12_; _tmp14_ = axis1; _tmp15_ = soy_atoms_axis_get_y (_tmp14_); _tmp16_ = _tmp15_; _tmp17_ = axis1; _tmp18_ = soy_atoms_axis_get_z (_tmp17_); _tmp19_ = _tmp18_; dJointSetUniversalAxis1 ((struct dxJoint*) _tmp10_, (dReal) _tmp13_, (dReal) _tmp16_, (dReal) _tmp19_); _tmp20_ = ((soyjointsJoint*) self)->joint; _tmp21_ = axis2; _tmp22_ = soy_atoms_axis_get_x (_tmp21_); _tmp23_ = _tmp22_; _tmp24_ = axis2; _tmp25_ = soy_atoms_axis_get_y (_tmp24_); _tmp26_ = _tmp25_; _tmp27_ = axis2; _tmp28_ = soy_atoms_axis_get_z (_tmp27_); _tmp29_ = _tmp28_; dJointSetUniversalAxis2 ((struct dxJoint*) _tmp20_, (dReal) _tmp23_, (dReal) _tmp26_, (dReal) _tmp29_); }
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 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; }