/** * This method is called if the joint should be detached. * If the maximum and minimum positions are set the method * stores the position-offset for correct positions in later use. **/ void SliderJoint::detachJoint() { TransformationData entityTrans; if (active && joint && posSet) { dReal position = dJointGetSliderPosition(joint); if (mainEntity != NULL) { entityTrans = mainEntity->getEnvironmentTransformation(); gmtl::Vec3f newAxis = axis; gmtl::Vec3f scale; /* scale[0] = mainEntity->getXScale(); scale[1] = mainEntity->getYScale(); scale[2] = mainEntity->getZScale();*/ scale = entityTrans.scale; newAxis[0] *= scale[0]; newAxis[1] *= scale[1]; newAxis[2] *= scale[2]; gmtl::normalize(newAxis); gmtl::Vec3f newPosVec = newAxis * (float)position; newPosVec[0] /= scale[0]; newPosVec[1] /= scale[1]; newPosVec[2] /= scale[2]; position = gmtl::dot(newPosVec, newAxis); } // if maxPos -= position; minPos -= position; deltaPos += position; } // if } // detachJoint
void dxJointSlider::getInfo1 ( dxJoint::Info1 *info ) { info->nub = 5; // see if joint is powered if ( limot.fmax > 0 ) info->m = 6; // powered slider needs an extra constraint row else info->m = 5; // see if we're at a joint limit. limot.limit = 0; if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) && limot.lostop <= limot.histop ) { // measure joint position dReal pos = dJointGetSliderPosition ( this ); if ( pos <= limot.lostop ) { limot.limit = 1; limot.limit_err = pos - limot.lostop; info->m = 6; } else if ( pos >= limot.histop ) { limot.limit = 2; limot.limit_err = pos - limot.histop; info->m = 6; } } }
dReal ODE_Link::getAngle(){ if(jointType == ODE_Link::ROTATIONAL_JOINT) return dJointGetHingeAngle(odeJointId); else if(jointType == ODE_Link::SLIDE_JOINT) return dJointGetSliderPosition(odeJointId); else return 0; }
static float get_phys_joint_value(dJointID j) { switch (dJointGetType(j)) { case dJointTypeHinge: return (float) DEG(dJointGetHingeAngle (j)); case dJointTypeSlider: return (float) dJointGetSliderPosition(j); case dJointTypeHinge2: return (float) DEG(dJointGetHinge2Angle1 (j)); default: return 0.0f; } }
gfloat soy_joints_slider_get_length (soyjointsSlider* self) { gfloat result; struct dxJoint* _tmp0_; dReal _tmp1_ = 0.0; g_return_val_if_fail (self != NULL, 0.0F); _tmp0_ = ((soyjointsJoint*) self)->joint; _tmp1_ = dJointGetSliderPosition ((struct dxJoint*) _tmp0_); result = (gfloat) _tmp1_; return result; }
void ODE_LinearSpringModel::updateInputValues() { if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) { return; } //The first time an update is run the mJoint is located. //This has to be done here instead of setup() to ensure that the joint is //really available, which can not be guaranteed in setup(). if(mJoint == 0) { SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint(); ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint); if(odeJoint == 0) { MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint); if(adapter != 0) { odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel()); } } if(simJoint != 0 && odeJoint != 0) { if(getType() == MotorModel::HINGE_JOINT) { if(dynamic_cast<HingeJoint*>(simJoint) != 0) { mJoint = odeJoint->getJoint(); } } else if(getType() == MotorModel::SLIDER_JOINT) { if(dynamic_cast<SliderJoint*>(simJoint) != 0) { mJoint = odeJoint->getJoint(); } } } } if(mJoint != 0) { if(getType() == MotorModel::HINGE_JOINT) { double torque = calculateTorque(dJointGetHingeAngle(mJoint)); dJointAddHingeTorque(mJoint, torque); mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque); } else if(getType() == MotorModel::SLIDER_JOINT) { double force = calculateForce(dJointGetSliderPosition(mJoint)); dJointAddSliderForce(mJoint, force); mOwnerSpringAdapter->getCurrentTorqueValue()->set(force); } } }
void OscSlideODE::simulationCallback() { ODEConstraint& me = *static_cast<ODEConstraint*>(special()); dReal pos = dJointGetSliderPosition(me.joint()); dReal rate = dJointGetSliderPositionRate(me.joint()); dReal addforce = - m_response->m_stiffness.m_value*pos - m_response->m_damping.m_value*rate; m_force.m_value = addforce; dJointAddSliderForce(me.joint(), addforce); }
/** *@brief ジョイントの位置、角度取得の関数 * @return ジョイントの位置、角度 */ double ODEJointObj::GetJointPosition() { if(JointType == 0) { double a = dJointGetSliderPosition(joint); return a; } else if(JointType == 2) { double a = dJointGetHingeAngle(joint); return a; } return 0; }
/** * This method copies the value of the field with the passed name * to the result-pointer. It is a little helper to avoid type-casts * when accessing joint-pointers. * @param type name of the field * @param result destination where the value should be stored **/ void SliderJoint::getMemberValue(std::string type, void* result) { float pos = 0; if (type == "position") { if (joint && active) pos = dJointGetSliderPosition(joint); *((float*)result) = pos+deltaPos; } // if else if (type == "internalChanges") { JointInteractionEndEvent* evt = (JointInteractionEndEvent*)result; evt->addJointID(jointID); evt->addFloat(maxPos); evt->addFloat(minPos); evt->addFloat(deltaPos); } // else if } // getMemberValue
void dxJointSlider::getInfo1 ( dxJoint::Info1 *info ) { info->nub = 5; info->m = 5; // see if we're at a joint limit. limot.limit = 0; if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) && limot.lostop <= limot.histop ) { // measure joint position dReal pos = dJointGetSliderPosition ( this ); limot.testRotationalLimit( pos ); info->m+=limot.countRows(); info->nub+=limot.countUBRows(); } }
void ServoMotor::act() { const float currentPos = dJointGetType(joint->joint) == dJointTypeHinge ? dJointGetHingeAngle(joint->joint) : dJointGetSliderPosition(joint->joint); float setpoint = this->setpoint; const float maxValueChange = maxVelocity * Simulation::simulation->scene->stepLength; if(std::abs(setpoint - currentPos) > maxValueChange) { if(setpoint < currentPos) setpoint = currentPos - maxValueChange; else setpoint = currentPos + maxValueChange; } const float newVel = controller.getOutput(currentPos, setpoint); if(dJointGetType(joint->joint) == dJointTypeHinge) dJointSetHingeParam(joint->joint, dParamVel, dReal(newVel)); else dJointSetSliderParam(joint->joint, dParamVel, dReal(newVel)); }
Real32 PhysicsSliderJoint::getPosition(void) { return dJointGetSliderPosition(_JointID); }
void ServoMotor::PositionSensor::updateValue() { data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge ? dJointGetHingeAngle(joint->joint) : dJointGetSliderPosition(joint->joint); }
dReal doStuffAndGetError (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the orientations are the same const dReal *R1 = dBodyGetRotation (body[0]); const dReal *R2 = dBodyGetRotation (body[1]); dReal err1 = dMaxDifference (R1,R2,3,3); // check the body offset is correct dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return (err1 + length (pp)) * 300; } case 1: { // 1 body to static env addOscillatingTorque (0.1); // check the orientation is the identity dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return (err1 + length (p)) * 1e6; } case 2: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the body offset is correct // Should really check body rotation too. Oh well. const dReal *R1 = dBodyGetRotation (body[0]); dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return length(pp) * 300; } case 3: { // 1 body to static env with relative rotation addOscillatingTorque (0.1); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return length (p) * 1e6; } // ********** hinge joint case 200: // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); return dInfinity; case 220: // hinge angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHingeAngle (joint); if (a > 0.5 && a < 1) return 0; else return 10; } return 0; case 221: { // hinge angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHingeAngle (joint); dReal r = dJointGetHingeAngleRate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er) * 4e4; } case 230: // hinge motor rate (and polarity) test case 231: { // ...with stops static dReal a = 0; dReal r = dJointGetHingeAngleRate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHingeParam (joint,dParamVel,cos(a)); if (n==231) return dInfinity; return err * 1e6; } // ********** slider joint case 300: // 2 body addOscillatingTorque (0.05); dampRotationalMotion (0.1); addSpringForce (0.5); return dInfinity; case 320: // slider angle polarity test dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); if (iteration == 40) { dReal a = dJointGetSliderPosition (joint); if (a > 0.2 && a < 0.5) return 0; else return 10; return a; } return 0; case 321: { // slider angle rate test static dReal last_pos = 0; dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); dReal p = dJointGetSliderPosition (joint); dReal r = dJointGetSliderPositionRate (joint); dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) last_pos = p; return fabs(r-er) * 1e9; } case 330: // slider motor rate (and polarity) test case 331: { // ...with stops static dReal a = 0; dReal r = dJointGetSliderPositionRate (joint); dReal err = fabs (0.7*cos(a) - r); if (a < 0.04) err = 0; a += 0.03; dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); if (n==331) return dInfinity; return err * 1e6; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHinge2Angle1 (joint); if (a > 0.5 && a < 0.6) return 0; else return 10; } return 0; case 421: { // hinge-2 steering angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHinge2Angle1 (joint); dReal r = dJointGetHinge2Angle1Rate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er)*2e4; } case 430: // hinge 2 steering motor rate (+polarity) test case 431: { // ...with stops static dReal a = 0; dReal r = dJointGetHinge2Angle1Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel,cos(a)); if (n==431) return dInfinity; return err * 1e6; } case 432: { // hinge 2 wheel motor rate (+polarity) test static dReal a = 0; dReal r = dJointGetHinge2Angle2Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel2,cos(a)); return err * 1e6; } // ********** angular motor joint case 600: { // test euler angle calculations // desired euler angles from last iteration static dReal a1,a2,a3; // find actual euler angles dReal aa1 = dJointGetAMotorAngle (joint,0); dReal aa2 = dJointGetAMotorAngle (joint,1); dReal aa3 = dJointGetAMotorAngle (joint,2); // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); dReal err = dInfinity; if (iteration > 0) { err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); err *= 1e10; } // get random base rotation for both bodies dMatrix3 Rbase; dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); dBodySetRotation (body[0],Rbase); // rotate body 2 by random euler angles w.r.t. body 1 a1 = 3.14 * 2 * (dRandReal()-0.5); a2 = 1.57 * 2 * (dRandReal()-0.5); a3 = 3.14 * 2 * (dRandReal()-0.5); dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; dRFromAxisAndAngle (R1,0,0,1,-a1); dRFromAxisAndAngle (R2,0,1,0,a2); dRFromAxisAndAngle (R3,1,0,0,-a3); dMultiply0 (Rtmp1,R2,R3,3,3,3); dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); dBodySetRotation (body[1],Rtmp1); // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); return err; } // ********** universal joint case 700: { // 2 body: joint constraint dVector3 ax1, ax2; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 701: { // 2 body: angle 1 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 702: { // 2 body: angle 2 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 720: { // universal transmit torque test: constraint error dVector3 ax1, ax2; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 721: { // universal transmit torque test: angle1 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 722: { // universal transmit torque test: angle2 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 730:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 731:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 2e3; } case 732:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 740:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 741:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 742:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e4; } } return dInfinity; }
double CODESliderJoint::getPosition() const { return dJointGetSliderPosition(mID); }