/*
=================================================================================
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);
}
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 UniversalJoint::applyFriction(bool initial)
{
if(!initial)
{
double velocity = 0.0;
double gravityFactor = fabs((simulation->getPhysicsParameters())->getGravity());
double fmax;
if(axis1->motor == 0)
{
fmax = axis1->friction * gravityFactor;
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(fmax));
dJointSetUniversalParam(physicalJoint, dParamVel, dReal(velocity));
}
if(axis2->motor == 0)
{
fmax = axis2->friction * gravityFactor;
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(fmax));
dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(velocity));
}
}
}
// 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);
}
void UniversalJoint::act(bool initial)
{
if(!initial)
{
if(axis1->motor)
{
if(axis1->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis1->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, desiredVelocity);
}
else if(axis1->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis1->motor->getControllerOutput(dJointGetUniversalAngle1(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, dReal(controllerOutput));
}
}
if(axis2->motor)
{
if(axis2->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis2->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, desiredVelocity);
}
else if(axis2->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis2->motor->getControllerOutput(dJointGetUniversalAngle2(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(controllerOutput));
}
}
}
}
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;
}
