Robot::Kicker::Kicker(Robot* robot)
{
rob = robot;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+(rob->cfg->robotSettings.RobotCenterFromKicker+rob->cfg->robotSettings.KickerThickness);
float centery = y;
float centerz = z-(rob->cfg->robotSettings.RobotHeight)*0.5f+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight+rob->cfg->robotSettings.KickerZ;
box = new PBox(centerx,centery,centerz,rob->cfg->robotSettings.KickerThickness,rob->cfg->robotSettings.KickerWidth,rob->cfg->robotSettings.KickerHeight,rob->cfg->robotSettings.KickerMass,0.9,0.9,0.9);
box->setBodyPosition(centerx-x,centery-y,centerz-z,true);
box->space = rob->space;
rob->w->addObject(box);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,box->body);
const dReal *aa = dBodyGetPosition (box->body);
dJointSetHingeAnchor (joint,aa[0],aa[1],aa[2]);
dJointSetHingeAxis (joint,0,-1,0);
dJointSetHingeParam (joint,dParamVel,0);
dJointSetHingeParam (joint,dParamLoStop,0);
dJointSetHingeParam (joint,dParamHiStop,0);
rolling = 0;
kicking = false;
}
void CManipulator::SetStop(dJointID myJoint,SStopPara stopPara)
{
dJointSetHingeParam(myJoint,dParamLoStop,stopPara.lowstop);//-0.5*Pi);
dJointSetHingeParam(myJoint,dParamHiStop,stopPara.histop);//0.5*Pi);
dJointSetHingeParam(myJoint,dParamBounce,stopPara.bounce);//无弹性
dJointSetHingeParam(myJoint,dParamStopCFM,stopPara.cfm);
return;
}
void Car::setSpeed(int ind, dReal speed)
{
ind += FIRST_SPROCKET;
if (axle[ind] != 0) {
dJointSetHingeParam(axle[ind], dParamFMax, max_f); // what should this be?
dJointSetHingeParam(axle[ind], dParamVel, speed);
}
}
void SkidSteeringVehicle::step(dReal stepSize) {
this->velocityLeft.step(stepSize);
this->velocityRight.step(stepSize);
for(int fr = 0; fr < 2; fr++) {
dJointSetHingeParam(this->wheelJoint[fr][0], dParamVel, this->velocityLeft.get());
dJointSetHingeParam(this->wheelJoint[fr][1], dParamVel, this->velocityRight.get());
}
}
/**
* 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 HingeJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
gmtl::Vec3f newAnchor, newAxis, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
TransformationData entityTrans;
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, obj1, obj2);
newAnchor = anchor;
newAxis = axis;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axis by mainEntity-scale value because of possible distortion
newAxis[0] *= scaleVec[0];
newAxis[1] *= scaleVec[1];
newAxis[2] *= scaleVec[2];
gmtl::normalize(newAxis);
// get the Rotation of the mainEntity
entityRot = entityTrans.orientation;
// rotate Axis by mainEntity-rotation
newAxis *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
newAnchor += entityTrans.position;
} // if
dJointSetHingeAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetHingeAxis(joint, newAxis[0], newAxis[1], newAxis[2]);
// ODE parameters for minimizing failure in Hinge Joint
// It should not be correct this way but it works better than without it ;-)
dJointSetHingeParam(joint, dParamCFM, 0);
dJointSetHingeParam(joint, dParamStopERP, 0);
dJointSetHingeParam(joint, dParamStopCFM, 0);
oldAngle = 0;
// set the maximum and minimum Joint-angles (if set).
if (anglesSet)
setODEAngles();
} // attachJoint
void HingeJoint::applyAnchor(dReal x, dReal y, dReal z)
{
dJointSetHingeAnchor(m_joint, x, y, z);
// Set of max and min of joint angle
dJointSetHingeAxis (m_joint, m_axis.x(), m_axis.y(), m_axis.z());
dJointSetHingeParam(m_joint, dParamLoStop, -2.0*M_PI);
dJointSetHingeParam(m_joint, dParamHiStop, 2.0*M_PI);
}
/*** 制 御 ***/
static void control()
{
dReal fMax = 10.0;
for (int i=0; i<WHEEL_NUM;i++)
{
dJointSetHingeParam(wheel[i].joint, dParamVel , wheel[i].v);
dJointSetHingeParam(wheel[i].joint, dParamFMax, fMax);
}
printf("POWER=%d \r",POWER);
}
/*** P制御 ***/
void Pcontrol()
{
dReal k = 10.0, fMax = 100.0; // 比例ゲイン，最大トルク
for (int j = 1; j < NUM; j++) {
dReal tmp = dJointGetHingeAngle(joint[j]); // 関節角の取得
dReal z = THETA[j] - tmp; // 残差
dJointSetHingeParam(joint[j],dParamVel, k*z); // 角速度の設定
dJointSetHingeParam(joint[j],dParamFMax,fMax); // トルクの設定
}
}
void control() { /*** P control ****/
static int step = 0; // Steps of simulation
double k1 = 10.0, fMax = 100.0; // k1: proportional gain, fMax：Max torque[Nm]
printf("\r%6d:",step++);
for (int j = 1; j < NUM; j++) {
double tmpAngle = dJointGetHingeAngle(joint[j]); // Present angle[rad]
double z = THETA[j] - tmpAngle; // z: residual=target angle - present angle
dJointSetHingeParam(joint[j], dParamVel, k1*z); // Set angular velocity[m/s]
dJointSetHingeParam(joint[j], dParamFMax, fMax); // Set max torque[N/m]
}
}
bool CODEHingeJoint::init(dWorld& world, dJointGroupID groupID)
{
bool result = CODEJoint::init(world, groupID);
if (mDryFriction > 0)
{
dJointSetHingeParam(mID, dParamVel, 0);
dJointSetHingeParam(mID, dParamFMax, mDryFriction);
}
return result;
}
void CODEHingeJoint::setParams()
{
double anchorPos[3];
mpBodyAnchors[0]->getPosition(anchorPos);
dJointSetHingeAnchor(mID, anchorPos[0], anchorPos[1], anchorPos[2]);
dJointSetHingeAxis(mID, mAxisX, mAxisY, mAxisZ);
dJointSetHingeParam(mID, dParamLoStop, mLowerLimit);
dJointSetHingeParam(mID, dParamHiStop, mUpperLimit);
if (mStopERP >= 0)
dJointSetHingeParam(mID, dParamStopERP, mStopERP);
if (mStopCFM >= 0)
dJointSetHingeParam(mID, dParamStopCFM, mStopCFM);
}
void JOINT::Create_In_Simulator(dWorldID world, OBJECT *firstObject, OBJECT *secondObject) {
joint = dJointCreateHinge(world,0);
dJointAttach( joint , firstObject->Get_Body() , secondObject->Get_Body() );
dJointSetHingeAnchor(joint,x,y,z);
dJointSetHingeAxis(joint,normalX,normalY,normalZ);
dJointSetHingeParam(joint,dParamLoStop,lowStop);
dJointSetHingeParam(joint,dParamHiStop,highStop);
}
/**
*@brief ジョイントの速度制御の関数
* @param v 目標速度
*/
void ODEJointObj::ControlJointVel(double v)
{
ms->mu->lock();
if(JointType == 0)
{
dJointSetSliderParam(joint, dParamVel, v);
dJointSetSliderParam(joint, dParamFMax, 200);
}
else if(JointType == 2)
{
dJointSetHingeParam(joint,dParamVel, v);
dJointSetHingeParam(joint,dParamFMax,50.);
}
ms->mu->unlock();
}
/* ------------------------
* ロボットの生成・制御・座標
------------------------ */
void ControlOmni(double vx, double vy, int n)
{
/* ローカル変数の定義 */
double wheelV[4] = {vy, vy, vx, vx}; /* 角速度 */
for (int i=0; i< OMNI_NUM; i++)
{
for (int j=0; j<WHEEL_NUM;j++)
{
/* モータの目標速度，目標角速度 */
dJointSetHingeParam(wheel[i][j].joint, dParamVel , wheelV[j]);
/* 目標速度を達成するために発揮可能なトルクの最大値 */
dJointSetHingeParam(wheel[i][j].joint, dParamFMax, 0.1);
/* 1ステップでモータに過剰なトルクがかかるのを防ぐ */
dJointSetHingeParam(wheel[i][j].joint, dParamFudgeFactor, 0.1);
}
}
}
//SetAllMovParams
void JointSlider::SetAllMovParams(double LoStop, double HiStop,
double Velocity, double MaxForce,
double FudgeFactor, double Bounce,
double StopERP, double StopCFM)
{
if (LoStop <= 0)
dJointSetHingeParam(this->_id, dParamLoStop, LoStop);
if (HiStop >= 0)
dJointSetHingeParam(this->_id, dParamHiStop, HiStop);
dJointSetSliderParam(this->_id, dParamVel, Velocity);
dJointSetSliderParam(this->_id, dParamFMax, MaxForce);
dJointSetSliderParam(this->_id, dParamFudgeFactor, FudgeFactor);
dJointSetSliderParam(this->_id, dParamBounce, Bounce);
dJointSetSliderParam(this->_id, dParamStopERP, StopERP);
dJointSetSliderParam(this->_id, dParamStopCFM, StopCFM);
}
void ServoMotor::create(Joint* joint)
{
ASSERT(dJointGetType(joint->joint) == dJointTypeHinge || dJointGetType(joint->joint) == dJointTypeSlider);
this->joint = positionSensor.joint = joint;
if(dJointGetType(joint->joint) == dJointTypeHinge)
dJointSetHingeParam(joint->joint, dParamFMax, maxForce);
else
dJointSetSliderParam(joint->joint, dParamFMax, maxForce);
}
void createHingeLeg(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);
//hinge joint
joint = dJointCreateHinge(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetHingeAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetHingeAxis(joint, 0, 1, 0);
//Max and min angles
dJointSetHingeParam(joint, dParamHiStop, maxAngle);
dJointSetHingeParam(joint, dParamLoStop, minAngle);
}
void ODE_Dynamixel::updateActuators() {
if(mJoint != 0) {
mCurrentPosition = dJointGetAMotorAngle(mJoint, 0);
calculateMotorFrictionAndVelocity();
dJointSetHingeParam(mHingeJoint, dParamFMax, mCalculatedFriction);
dJointSetAMotorParam(mJoint, dParamVel, mCalculatedVelocity);
}
}
void JOINT::Actuate(void) {
if ( motorNeuron == NULL )
return;
double motorNeuronValue = motorNeuron->Get_Value();
double zeroToOne = motorNeuronValue/2.0 + 0.5;
double desiredAngle = zeroToOne * ( highStop - lowStop ) + lowStop;
double currentAngle = dJointGetHingeAngle(joint);
double diff = desiredAngle - currentAngle;
dJointSetHingeParam(joint,dParamVel, speed * diff);
dJointSetHingeParam(joint,dParamFMax,1000000);
}
void sODEJoint::UpdateJoint( float fTimeDelta )
{
if ( bMovingLimit )
{
fMoveTime -= fTimeDelta;
if ( fMoveTime < 0 )
{
bMovingLimit = false;
fMoveTime = 0;
}
float lerp = fMoveTime / fOrigMoveTime;
float fNewLow = fLowStart*lerp + fLowEnd*(1-lerp);
float fNewHigh = fHighStart*lerp + fHighEnd*(1-lerp);
dJointSetHingeParam( oJoint, dParamLoStop, fNewLow );
dJointSetHingeParam( oJoint, dParamHiStop, fNewHigh );
}
}
static void set_phys_joint_attr(dJointID j, int p, float v)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: dJointSetHingeParam (j, p, v); break;
case dJointTypeSlider: dJointSetSliderParam (j, p, v); break;
case dJointTypeHinge2: dJointSetHinge2Param (j, p, v); break;
case dJointTypeUniversal: dJointSetUniversalParam(j, p, v); break;
default: break;
}
}
void sODEJoint::SetMovingHingeLimit( float startlow, float starthigh, float endlow, float endhigh, float time )
{
if ( time <= 0 )
{
dJointSetHingeParam( oJoint, dParamLoStop, endlow );
dJointSetHingeParam( oJoint, dParamHiStop, endhigh );
return;
}
bMovingLimit = true;
fLowStart = startlow;
fHighStart = starthigh;
fLowEnd = endlow;
fHighEnd = endhigh;
fMoveTime = time;
fOrigMoveTime = time;
dJointSetHingeParam( oJoint, dParamLoStop, startlow );
dJointSetHingeParam( oJoint, dParamHiStop, starthigh );
}
void Hinge::createPhysics()
{
ASSERT(axis);
ASSERT(axis->motor);
//
axis->create();
//
PrimaryObject::createPhysics();
// find bodies to connect
Body* parentBody = dynamic_cast<Body*>(parent);
ASSERT(!parentBody || parentBody->body);
ASSERT(!children.empty());
Body* childBody = dynamic_cast<Body*>(*children.begin());
ASSERT(childBody);
ASSERT(childBody->body);
// create joint
joint = dJointCreateHinge(Simulation::simulation->physicalWorld, 0);
dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
//set hinge joint parameter
dJointSetHingeAnchor(joint, pose.translation.x, pose.translation.y, pose.translation.z);
Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
dJointSetHingeAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
if(axis->cfm != -1.f)
dJointSetHingeParam(joint, dParamCFM, dReal(axis->cfm));
if(axis->deflection)
{
const Axis::Deflection& deflection = *axis->deflection;
float minHingeLimit = deflection.min;
float maxHingeLimit = deflection.max;
if(minHingeLimit > maxHingeLimit)
minHingeLimit = maxHingeLimit;
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
float internalTolerance = (maxHingeLimit - minHingeLimit) * 0.1f;
if(dynamic_cast<ServoMotor*>(axis->motor))
{
minHingeLimit -= internalTolerance;
maxHingeLimit += internalTolerance;
}
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
dJointSetHingeParam(joint, dParamHiStop, dReal(maxHingeLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
if(deflection.stopCFM != -1.f)
dJointSetHingeParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
if(deflection.stopERP != -1.f)
dJointSetHingeParam(joint, dParamStopERP, dReal(deflection.stopERP));
}
// create motor
axis->motor->create(this);
OpenGLTools::convertTransformation(rotation, translation, transformation);
}
void control()
{
double k1 = 10.0, fMax = 100.0; // k1:gain, fMax: max torque [Nm]
for(int i=0;i<MAX_JOINTS;i++)
{
current_angleR[i] = dJointGetHingeAngle(jointR[i]); // current joint
//double z = target_angleR[i]*M_PI/180 - current_angleR[i]; // z = target – current
double z = target_angleR[i] - current_angleR[i]; // z = target – current
// Set angular velocity for the joint: w = k1(th_tarjet-th_curr)
dJointSetHingeParam(jointR[i],dParamVel,k1*z);
dJointSetHingeParam(jointR[i],dParamFMax,fMax); // max torque
}
for(int i=0;i<MAX_JOINTS;i++)
{
current_angleL[i] = dJointGetHingeAngle(jointL[i]); // current joint
//double z = target_angleL[i]*M_PI/180 - current_angleL[i]; // z = target – current
double z = target_angleL[i] - current_angleL[i]; // z = target – current
// Set angular velocity for the joint: w = k1(th_tarjet-th_curr)
dJointSetHingeParam(jointL[i],dParamVel,k1*z);
dJointSetHingeParam(jointL[i],dParamFMax,fMax); // max torque
}
}
void ServoMotor::step(float stepSize) {
if(!shouldStep_)
return;
if (isBurntOut_) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
return;
}
if (isVelocityDriven_) {
if (fabs(desiredVelocity_) < 1e-5) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
} else {
dJointSetHingeParam(joint_->getJoint(), dParamVel, desiredVelocity_);
}
} else {
dReal curPosition = dJointGetHingeAngle(joint_->getJoint());
dReal error = curPosition - desiredPosition_;
dReal velocity = -gain_ * error / stepSize;
if (velocity > MAX_VELOCITY) {
velocity = MAX_VELOCITY;
} else if (velocity < MIN_VELOCITY) {
velocity = MIN_VELOCITY;
}
if (fabs(velocity) > 1e-5) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, velocity);
} else {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
}
}
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
void ODE_PID_PassiveActuatorModel::updateInputValues()
{
if(mJoint != 0) {
double desiredAngle = 0.0;
PassiveActuatorAdapter *owner = dynamic_cast<PassiveActuatorAdapter*>(mOwner);
if(owner != 0) {
desiredAngle = (owner->getReferenceAngle() + owner->getReferenceOffsetAngle())
* owner->getGearRatio() + owner->getOffsetAngle();
}
double currentAngle = dJointGetAMotorAngle(mJoint, 0);
mPID_Controller.update(currentAngle, desiredAngle * Math::PI / 180.0);
dJointSetAMotorParam(mJoint, dParamFMax, mMaxForce->get());
dJointSetHingeParam(mHingeJoint, dParamFMax, mFriction->get());
dJointSetAMotorParam(mJoint, dParamVel, mPID_Controller.getVelocity());
}
}
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));
}
/**
* This method sets the minimum and maximum angle of the ODE-hinge joint.
* The method checks if the first or the second EntityTransform is the
* mainEntity, because then the angles have to be set into different
* directions. The maxAngle-parameter is set two times, because ODE only
* accepts it if the maximum angle is above the minimum angle. To avoid
* checking this we easily set it two times.
**/
void HingeJoint::setODEAngles()
{
if (!firstIsMain)
{
dJointSetHingeParam(joint, dParamHiStop, maxAngle - deltaAngle - deltaAngleODE);
dJointSetHingeParam(joint, dParamLoStop, minAngle - deltaAngle - deltaAngleODE);
dJointSetHingeParam(joint, dParamHiStop, maxAngle - deltaAngle - deltaAngleODE);
// printf("\nHingeJoint::setODEAngles(): HiStop = %f LoStop = %f\n", maxAngle - deltaAngle - deltaAngleODE, minAngle - deltaAngle - deltaAngleODE);
} else
{
dJointSetHingeParam(joint, dParamHiStop, maxAngle + deltaAngle + deltaAngleODE);
dJointSetHingeParam(joint, dParamLoStop, minAngle + deltaAngle + deltaAngleODE);
dJointSetHingeParam(joint, dParamHiStop, maxAngle + deltaAngle + deltaAngleODE);
// printf("\nHingeJoint::setODEAngles(): HiStop = %f LoStop = %f\n", maxAngle + deltaAngle + deltaAngleODE, minAngle + deltaAngle + deltaAngleODE);
} // else
} // setODEAngles
void sODEJoint::SetHingeLimit( float low, float high )
{
dJointSetHingeParam( oJoint, dParamLoStop, low );
dJointSetHingeParam( oJoint, dParamHiStop, high );
}
