/**
* 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);
}
