btScalar Vehicle::autoClutch(btScalar clutch_rpm, btScalar last_clutch, btScalar dt) const
{
btScalar clutch_value = 1; // default clutch value
btScalar clutch_engage_limit = 10 * dt; // default engage rate limit
// keep engine rpm above stall
btScalar rpm_min = engine.getStartRPM();
btScalar rpm_clutch = transmission.getClutchRPM();
if (rpm_clutch < rpm_min)
{
btScalar rpm = engine.getRPM();
if (rpm < rpm_min * 1.25)
{
btScalar rpm_stall = engine.getStallRPM();
btScalar ramp = 0.8 * (rpm - rpm_stall) / (rpm_min - rpm_stall);
btScalar torque_limit = engine.getTorque() * ramp;
clutch_value = torque_limit / clutch.getTorqueMax();
btClamp(clutch_value, btScalar(0), btScalar(1));
}
}
// declutch when shifting
const btScalar shift_time = transmission.getShiftTime();
if (remaining_shift_time > shift_time * 0.5)
{
clutch_value = 0.0;
}
else if (remaining_shift_time > 0.0)
{
clutch_value *= (1.0 - remaining_shift_time / (shift_time * 0.5));
}
if (brake_value > 1E-3)
{
// declutch when braking
clutch_value = 0.0;
}
else if (engine.getThrottle() < 1E-3)
{
// declutch to avoid driven wheels traction loss
// helps with wheel lock to roll transitions after hard braking
for (int i = 0; i < wheel.size(); ++i)
{
btScalar slide = std::abs(wheel[i].tire.getSlide());
btScalar slide_limit = 0.25 * wheel[i].tire.getIdealSlide();
if (slide > slide_limit)
{
clutch_value = 0;
break;
}
}
}
// rate limit the autoclutch
btScalar clutch_delta = clutch_value - last_clutch;
btClamp(clutch_delta, -clutch_engage_limit, clutch_engage_limit);
clutch_value = last_clutch + clutch_delta;
return clutch_value;
}
void ImportMJCFSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
btVector3 gravity(0, 0, -10);
gravity[m_upAxis] = m_grav;
m_dynamicsWorld->setGravity(gravity);
for (int i = 0; i < m_data->m_numMotors; i++)
{
if (m_data->m_jointMotors[i])
{
btScalar pos = m_data->m_motorTargetPositions[i];
int link = m_data->m_jointMotors[i]->getLinkA();
btScalar lowerLimit = m_data->m_mb->getLink(link).m_jointLowerLimit;
btScalar upperLimit = m_data->m_mb->getLink(link).m_jointUpperLimit;
if (lowerLimit < upperLimit)
{
btClamp(pos, lowerLimit, upperLimit);
}
m_data->m_jointMotors[i]->setPositionTarget(pos);
}
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetPositions[i]);
}
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
}
}
void ROC::Collision::SetMotionType(int f_type)
{
if(m_rigidBody)
{
m_motionType = f_type;
btClamp(m_motionType, static_cast<int>(CMT_Default), static_cast<int>(CMT_Kinematic));
switch(m_motionType)
{
case CMT_Default:
{
m_rigidBody->setCollisionFlags(0);
m_rigidBody->setActivationState(ACTIVE_TAG);
} break;
case CMT_Static:
{
m_rigidBody->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
m_rigidBody->setActivationState(ACTIVE_TAG);
} break;
case CMT_Kinematic:
{
m_rigidBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
m_rigidBody->setActivationState(DISABLE_DEACTIVATION);
} break;
}
}
}
void ROC::Collision::SetFriction(float f_val)
{
if(m_rigidBody)
{
btClamp(f_val, 0.f, 1.f);
m_rigidBody->setFriction(f_val);
m_rigidBody->activate(true);
}
}
void Tire::getSigmaHatAlphaHat(btScalar load, btScalar & sh, btScalar & ah) const
{
btScalar rload = load / max_load * tablesize - 1.0;
btClamp(rload, btScalar(0.0), btScalar(tablesize - 1.0));
int lbound = btMin(int(rload), tablesize - 2);
btScalar blend = rload - lbound;
sh = sigma_hat[lbound] * (1.0 - blend) + sigma_hat[lbound + 1] * blend;
ah = alpha_hat[lbound] * (1.0 - blend) + alpha_hat[lbound + 1] * blend;
}
btScalar Tire::getSqueal() const
{
btScalar squeal = 0.0;
if (vx * vx > 1E-2 && slide * slide > 1E-6)
{
btScalar vx_body = vx / slide;
btScalar vx_ideal = ideal_slide * vx_body;
btScalar vy_ideal = btTan(-ideal_slip / 180 * M_PI) * vx_body;
btScalar vx_squeal = btFabs(vx / vx_ideal);
btScalar vy_squeal = btFabs(vy / vy_ideal);
// start squeal at 80% of the ideal slide/slip, max out at 160%
squeal = 1.25 * btMax(vx_squeal, vy_squeal) - 1.0;
btClamp(squeal, btScalar(0), btScalar(1));
}
return squeal;
}
bool ROC::Collision::Create(int f_type, const glm::vec3 &f_size, float f_mass)
{
if(!m_rigidBody)
{
btClamp(f_type, static_cast<int>(CT_Sphere), static_cast<int>(CT_Cone));
btVector3 l_inertia;
btCollisionShape *l_shape = nullptr;
switch(f_type)
{
case CT_Sphere:
l_shape = new btSphereShape(f_size.x);
break;
case CT_Box:
l_shape = new btBoxShape(btVector3(f_size.x, f_size.y, f_size.z));
break;
case CT_Cylinder:
l_shape = new btCylinderShape(btVector3(f_size.x, f_size.y, f_size.z));
break;
case CT_Capsule:
l_shape = new btCapsuleShape(f_size.x, f_size.y);
break;
case CT_Cone:
l_shape = new btConeShape(f_size.x, f_size.y);
break;
}
if(l_shape)
{
l_shape->calculateLocalInertia(f_mass, l_inertia);
btTransform l_transform;
l_transform.setIdentity();
btDefaultMotionState *l_fallMotionState = new btDefaultMotionState(l_transform);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(f_mass, l_fallMotionState, l_shape, l_inertia);
m_rigidBody = new btRigidBody(fallRigidBodyCI);
m_rigidBody->setUserPointer(this);
}
}
return (m_rigidBody != nullptr);
}
/** Initialises the track object based on the specified XML data.
* \param xml_node The XML data.
* \param parent The parent scene node.
* \param model_def_loader Used to load level-of-detail nodes.
*/
void TrackObject::init(const XMLNode &xml_node, scene::ISceneNode* parent,
ModelDefinitionLoader& model_def_loader,
TrackObject* parent_library)
{
m_init_xyz = core::vector3df(0,0,0);
m_init_hpr = core::vector3df(0,0,0);
m_init_scale = core::vector3df(1,1,1);
m_enabled = true;
m_initially_visible = false;
m_presentation = NULL;
m_animator = NULL;
m_parent_library = parent_library;
m_physical_object = NULL;
xml_node.get("id", &m_id );
xml_node.get("model", &m_name );
xml_node.get("xyz", &m_init_xyz );
xml_node.get("hpr", &m_init_hpr );
xml_node.get("scale", &m_init_scale);
xml_node.get("enabled", &m_enabled );
m_interaction = "static";
xml_node.get("interaction", &m_interaction);
xml_node.get("lod_group", &m_lod_group);
m_is_driveable = false;
xml_node.get("driveable", &m_is_driveable);
bool lod_instance = false;
xml_node.get("lod_instance", &lod_instance);
m_soccer_ball = false;
xml_node.get("soccer_ball", &m_soccer_ball);
std::string type;
xml_node.get("type", &type );
m_type = type;
m_initially_visible = true;
xml_node.get("if", &m_visibility_condition);
if (m_visibility_condition == "false")
{
m_initially_visible = false;
}
if (!m_initially_visible)
setEnabled(false);
if (xml_node.getName() == "particle-emitter")
{
m_type = "particle-emitter";
m_presentation = new TrackObjectPresentationParticles(xml_node, parent);
}
else if (xml_node.getName() == "light")
{
m_type = "light";
m_presentation = new TrackObjectPresentationLight(xml_node, parent);
}
else if (xml_node.getName() == "library")
{
xml_node.get("name", &m_name);
m_presentation = new TrackObjectPresentationLibraryNode(this, xml_node, model_def_loader);
if (parent_library != NULL)
{
Track::getCurrentTrack()->addMetaLibrary(parent_library, this);
}
}
else if (type == "sfx-emitter")
{
// FIXME: at this time sound emitters are just disabled in multiplayer
// otherwise the sounds would be constantly heard
if (race_manager->getNumLocalPlayers() < 2)
m_presentation = new TrackObjectPresentationSound(xml_node, parent);
}
else if (type == "action-trigger")
{
std::string action;
xml_node.get("action", &action);
m_name = action; //adds action as name so that it can be found by using getName()
m_presentation = new TrackObjectPresentationActionTrigger(xml_node, parent_library);
}
else if (type == "billboard")
{
m_presentation = new TrackObjectPresentationBillboard(xml_node, parent);
}
else if (type=="cutscene_camera")
{
m_presentation = new TrackObjectPresentationEmpty(xml_node);
}
else
{
// Colorization settings
std::string model_name;
xml_node.get("model", &model_name);
#ifndef SERVER_ONLY
if (CVS->isGLSL())
{
scene::IMesh* mesh = NULL;
// Use the first material in mesh to determine hue
Material* colorized = NULL;
if (model_name.size() > 0)
{
mesh = irr_driver->getMesh(model_name);
if (mesh != NULL)
{
for (u32 j = 0; j < mesh->getMeshBufferCount(); j++)
{
SP::SPMeshBuffer* mb = static_cast<SP::SPMeshBuffer*>
(mesh->getMeshBuffer(j));
std::vector<Material*> mbs = mb->getAllSTKMaterials();
for (Material* m : mbs)
{
if (m->isColorizable() && m->hasRandomHue())
{
colorized = m;
break;
}
}
if (colorized != NULL)
{
break;
}
}
}
}
else
{
std::string group_name = "";
xml_node.get("lod_group", &group_name);
// Try to get the first mesh from lod groups
mesh = model_def_loader.getFirstMeshFor(group_name);
if (mesh != NULL)
{
for (u32 j = 0; j < mesh->getMeshBufferCount(); j++)
{
SP::SPMeshBuffer* mb = static_cast<SP::SPMeshBuffer*>
(mesh->getMeshBuffer(j));
std::vector<Material*> mbs = mb->getAllSTKMaterials();
for (Material* m : mbs)
{
if (m->isColorizable() && m->hasRandomHue())
{
colorized = m;
break;
}
}
if (colorized != NULL)
{
break;
}
}
}
}
// If at least one material is colorizable, add RenderInfo for it
if (colorized != NULL)
{
const float hue = colorized->getRandomHue();
if (hue > 0.0f)
{
m_render_info = std::make_shared<RenderInfo>(hue);
}
}
}
#endif
scene::ISceneNode *glownode = NULL;
bool is_movable = false;
if (lod_instance)
{
m_type = "lod";
TrackObjectPresentationLOD* lod_node =
new TrackObjectPresentationLOD(xml_node, parent, model_def_loader, m_render_info);
m_presentation = lod_node;
LODNode* node = (LODNode*)lod_node->getNode();
if (type == "movable" && parent != NULL)
{
// HACK: unparent movables from their parent library object if any,
// because bullet provides absolute transforms, not transforms relative
// to the parent object
node->updateAbsolutePosition();
core::matrix4 absTransform = node->getAbsoluteTransformation();
node->setParent(irr_driver->getSceneManager()->getRootSceneNode());
node->setPosition(absTransform.getTranslation());
node->setRotation(absTransform.getRotationDegrees());
node->setScale(absTransform.getScale());
}
glownode = node->getAllNodes()[0];
}
else
{
m_type = "mesh";
m_presentation = new TrackObjectPresentationMesh(xml_node,
m_enabled,
parent,
m_render_info);
scene::ISceneNode* node = ((TrackObjectPresentationMesh *)m_presentation)->getNode();
if (type == "movable" && parent != NULL)
{
// HACK: unparent movables from their parent library object if any,
// because bullet provides absolute transforms, not transforms relative
// to the parent object
node->updateAbsolutePosition();
core::matrix4 absTransform = node->getAbsoluteTransformation();
node->setParent(irr_driver->getSceneManager()->getRootSceneNode());
node->setPosition(absTransform.getTranslation());
// Doesn't seem necessary to set rotation here, TODO: not sure why
//node->setRotation(absTransform.getRotationDegrees());
node->setScale(absTransform.getScale());
is_movable = true;
}
glownode = node;
}
std::string render_pass;
xml_node.get("renderpass", &render_pass);
if (m_interaction != "ghost" && m_interaction != "none" &&
render_pass != "skybox" )
{
m_physical_object = PhysicalObject::fromXML(type == "movable",
xml_node,
this);
}
if (parent_library != NULL)
{
if (is_movable)
parent_library->addMovableChild(this);
else
parent_library->addChild(this);
}
video::SColor glow;
if (xml_node.get("glow", &glow) && glownode)
{
float r, g, b;
r = glow.getRed() / 255.0f;
g = glow.getGreen() / 255.0f;
b = glow.getBlue() / 255.0f;
SP::SPMeshNode* spmn = dynamic_cast<SP::SPMeshNode*>(glownode);
if (spmn)
{
spmn->setGlowColor(video::SColorf(r, g, b));
}
}
bool is_in_shadowpass = true;
if (xml_node.get("shadow-pass", &is_in_shadowpass) && glownode)
{
SP::SPMeshNode* spmn = dynamic_cast<SP::SPMeshNode*>(glownode);
if (spmn)
{
spmn->setInShadowPass(is_in_shadowpass);
}
}
bool forcedbloom = false;
if (xml_node.get("forcedbloom", &forcedbloom) && forcedbloom && glownode)
{
float power = 1;
xml_node.get("bloompower", &power);
btClamp(power, 0.5f, 10.0f);
irr_driver->addForcedBloomNode(glownode, power);
}
}
if (type == "animation" || xml_node.hasChildNamed("curve"))
{
m_animator = new ThreeDAnimation(xml_node, this);
}
reset();
if (!m_initially_visible)
setEnabled(false);
if (parent_library != NULL && !parent_library->isEnabled())
setEnabled(false);
} // TrackObject
void InvertedPendulumPDControl::stepSimulation(float deltaTime)
{
static btScalar offset = -0.1*SIMD_PI;
m_frameCount++;
if ((m_frameCount&0xff)==0 )
{
offset = -offset;
}
btScalar target= SIMD_PI+offset;
qDesiredArray.resize(0);
qDesiredArray.resize(m_multiBody->getNumLinks(),target);
for (int joint = 0; joint<m_multiBody->getNumLinks(); joint++)
{
int dof1 = 0;
btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1];
btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1];
btScalar positionError = (qDesiredArray[joint]-qActual);
double desiredVelocity = 0;
btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce);
m_multiBody->addJointTorque(joint, force);
}
if (m_frameCount==100)
{
const char* gPngFileName = "pendulum";
if (gPngFileName)
{
//printf("gPngFileName=%s\n",gPngFileName);
sprintf(fileName,"%s%d.png",gPngFileName,m_frameCount);
b3Printf("Made screenshot %s",fileName);
this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName);
}
}
m_dynamicsWorld->stepSimulation(1./60.,0);//240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
#if 0
for (int i=0; i<m_jointFeedbacks.size(); i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
#endif
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
virtual void stepSimulation(float deltaTime)
{
float dt = deltaTime;
btClamp(dt,0.0001f,0.01f);
m_time+=dt;
m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
m_targetOri.setValue(0, 1.0, 0, 0);
m_targetPos.setValue(0.2*b3Cos( m_time), 0.2*b3Sin( m_time), 1.1);
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
b3JointStates2 jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
//skip the base positions (7 values)
b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
for (int i=0;i<numJoints;i++)
{
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// compute body position and orientation
b3LinkState linkState;
m_robotSim.getLinkState(0, 6, &linkState);
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
b3Vector3DoubleData targetPosDataOut;
m_targetPos.serializeDouble(targetPosDataOut);
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
b3Vector3DoubleData targetOriDataOut;
m_targetOri.serializeDouble(targetOriDataOut);
b3Vector3DoubleData worldOriDataOut;
m_worldOri.serializeDouble(worldOriDataOut);
b3RobotSimulatorInverseKinematicArgs ikargs;
b3RobotSimulatorInverseKinematicsResults ikresults;
ikargs.m_bodyUniqueId = m_kukaIndex;
// ikargs.m_currentJointPositions = q_current;
// ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
//ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION;
ikargs.m_flags |= B3_HAS_JOINT_DAMPING;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_endEffectorLinkIndex = 6;
// Settings based on default KUKA arm setting
ikargs.m_lowerLimits.resize(numJoints);
ikargs.m_upperLimits.resize(numJoints);
ikargs.m_jointRanges.resize(numJoints);
ikargs.m_restPoses.resize(numJoints);
ikargs.m_jointDamping.resize(numJoints,0.5);
ikargs.m_lowerLimits[0] = -2.32;
ikargs.m_lowerLimits[1] = -1.6;
ikargs.m_lowerLimits[2] = -2.32;
ikargs.m_lowerLimits[3] = -1.6;
ikargs.m_lowerLimits[4] = -2.32;
ikargs.m_lowerLimits[5] = -1.6;
ikargs.m_lowerLimits[6] = -2.4;
ikargs.m_upperLimits[0] = 2.32;
ikargs.m_upperLimits[1] = 1.6;
ikargs.m_upperLimits[2] = 2.32;
ikargs.m_upperLimits[3] = 1.6;
ikargs.m_upperLimits[4] = 2.32;
ikargs.m_upperLimits[5] = 1.6;
ikargs.m_upperLimits[6] = 2.4;
ikargs.m_jointRanges[0] = 5.8;
ikargs.m_jointRanges[1] = 4;
ikargs.m_jointRanges[2] = 5.8;
ikargs.m_jointRanges[3] = 4;
ikargs.m_jointRanges[4] = 5.8;
ikargs.m_jointRanges[5] = 4;
ikargs.m_jointRanges[6] = 6;
ikargs.m_restPoses[0] = 0;
ikargs.m_restPoses[1] = 0;
ikargs.m_restPoses[2] = 0;
ikargs.m_restPoses[3] = SIMD_HALF_PI;
ikargs.m_restPoses[4] = 0;
ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
ikargs.m_restPoses[6] = 0;
ikargs.m_jointDamping[0] = 10.0;
ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
//copy the IK result to the desired state of the motor/actuator
for (int i=0;i<numJoints;i++)
{
b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 100.0;
t.m_kp= 1.0;
t.m_targetVelocity = 0;
t.m_kd = 1.0;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}
}
}
m_robotSim.stepSimulation();
}
btVector3 Tire::getForce(
btScalar normal_force,
btScalar friction_coeff,
btScalar inclination,
btScalar ang_velocity,
btScalar lon_velocity,
btScalar lat_velocity)
{
if (normal_force < 1E-3 || friction_coeff < 1E-3)
{
slide = slip = 0;
ideal_slide = ideal_slip = 1;
fx = fy = fz = mz = 0;
vx = vy = 0;
return btVector3(0, 0, 0);
}
// pacejka in kN
normal_force = normal_force * 1E-3;
// limit input
btClamp(normal_force, btScalar(0), btScalar(max_load));
btClamp(inclination, -max_camber, max_camber);
// get ideal slip ratio
btScalar sigma_hat(0);
btScalar alpha_hat(0);
getSigmaHatAlphaHat(normal_force, sigma_hat, alpha_hat);
btScalar Fz = normal_force;
btScalar gamma = inclination; // positive when tire top tilts to the right, viewed from rear
btScalar denom = btMax(btFabs(lon_velocity), btScalar(1E-3));
btScalar lon_slip_velocity = lon_velocity - ang_velocity;
btScalar sigma = -lon_slip_velocity / denom; // longitudinal slip: negative in braking, positive in traction
btScalar tan_alpha = lat_velocity / denom;
btScalar alpha = -atan(tan_alpha) * 180.0 / M_PI; // sideslip angle: positive in a right turn(opposite to SAE tire coords)
btScalar max_Fx(0), max_Fy(0), max_Mz(0);
// combining method 1: beckman method for pre-combining longitudinal and lateral forces
// seems to be a variant of Bakker 1987:
// refined Kamm Circle for non-isotropic tire characteristics
// and slip normalization to guarantee simultaneous sliding
btScalar s = sigma / sigma_hat;
btScalar a = alpha / alpha_hat;
btScalar rho = btMax(btSqrt(s * s + a * a), btScalar(1E-4)); // normalized slip
btScalar Fx = (s / rho) * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx);
btScalar Fy = (a / rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy);
// Bakker (with unknown factor e(rho) to get the correct direction for large slip):
// btScalar Fx = -s * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx);
// btScalar Fy = -a * e(rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy);
/*
// combining method 2: orangutan
btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
btScalar one = (sigma < 0.0) ? -1.0 : 1.0;
btScalar pure_sigma = sigma / (one + sigma);
btScalar pure_alpha = -tan_alpha / (one + sigma);
btScalar pure_combined = sqrt(pure_sigma * pure_sigma + pure_alpha * pure_alpha);
btScalar kappa_combined = pure_combined / (1.0 - pure_combined);
btScalar alpha_combined = atan((one + sigma) * pure_combined);
btScalar Flimitx = PacejkaFx(kappa_combined, Fz, friction_coeff, max_Fx);
btScalar Flimity = PacejkaFy(alpha_combined * 180.0 / M_PI, Fz, gamma, friction_coeff, max_Fy);
btScalar x = fabs(Fx) / (fabs(Fx) + fabs(Fy));
btScalar y = 1.0 - x;
btScalar Flimit = (fabs(x * Flimitx) + fabs(y * Flimity));
btScalar Fmag = sqrt(Fx * Fx + Fy * Fy);
if (Fmag > Flimit)
{
btScalar scale = Flimit / Fmag;
Fx *= scale;
Fy *= scale;
}
*/
/*
// combining method 3: traction circle
btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
// determine to what extent the tires are long (x) gripping vs lat (y) gripping
// 1.0 when Fy is zero, 0.0 when Fx is zero
btScalar longfactor = 1.0;
btScalar combforce = btFabs(Fx) + btFabs(Fy);
if (combforce > 1) longfactor = btFabs(Fx) / combforce;
// determine the maximum force for this amount of long vs lat grip by linear interpolation
btScalar maxforce = btFabs(max_Fx) * longfactor + (1.0 - longfactor) * btFabs(max_Fy);
// scale down forces to fit into the maximum
if (combforce > maxforce)
{
Fx *= maxforce / combforce;
Fy *= maxforce / combforce;
}
*/
/*
// combining method 4: traction ellipse (prioritize Fx)
// issue: assumes that adhesion limits are fully reached for combined forces
btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
if (Fx < max_Fx)
{
Fy = Fy * sqrt(1.0 - (Fx / max_Fx) * (Fx / max_Fx));
}
else
{
Fx = max_Fx;
Fy = 0;
}
*/
/*
// combining method 5: traction ellipse (prioritize Fy)
// issue: assumes that adhesion limits are fully reached for combined forces
btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
if (Fy < max_Fy)
{
Fx = Fx * sqrt(1.0 - (Fy / max_Fy) * (Fy / max_Fy));
}
else
{
Fy = max_Fy;
Fx = 0;
}
*/
/*
// no combining
btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
*/
btScalar Mz = PacejkaMz(sigma, alpha, Fz, gamma, friction_coeff, max_Mz);
camber = inclination;
slide = sigma;
slip = alpha;
ideal_slide = sigma_hat;
ideal_slip = alpha_hat;
fx = Fx;
fy = Fy;
fz = Fz;
mz = Mz;
vx = lon_slip_velocity;
vy = lat_velocity;
// Fx positive during traction, Fy positive in a right turn, Mz positive in a left turn
return btVector3(Fx, Fy, Mz);
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_options == eCLIENTEXAMPLE_SERVER)
{
for (int i=0;i<100;i++)
{
m_physicsServer.processClientCommands();
}
}
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255);
// imageData.m_rgbColorData[pixelIndex+3]);
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int commandId = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (commandId ==CMD_RESET_SIMULATION)
{
m_selectedBody = -1;
m_numMotors=0;
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)
{
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
while (!b3CanSubmitCommand(m_physicsClientHandle))
{
m_physicsServer.processClientCommands();
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
}
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_options == eCLIENTEXAMPLE_SERVER)
{
for (int i = 0; i < 100; i++)
{
m_physicsServer.processClientCommands();
}
}
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType == CMD_CAMERA_IMAGE_COMPLETED)
{
// static int counter=0;
// char msg[1024];
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle, &imageData);
if (m_canvas)
{
//compute depth image range
float minDepthValue = 1e20f;
float maxDepthValue = -1e20f;
for (int i = 0; i < camVisualizerWidth; i++)
{
for (int j = 0; j < camVisualizerHeight; j++)
{
int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth)));
int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight)));
btClamp(xIndex, 0, imageData.m_pixelWidth);
btClamp(yIndex, 0, imageData.m_pixelHeight);
if (m_canvasDepthIndex >= 0)
{
int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue > -1e20)
{
maxDepthValue = btMax(maxDepthValue, depthValue);
minDepthValue = btMin(minDepthValue, depthValue);
}
}
}
}
for (int i = 0; i < camVisualizerWidth; i++)
{
for (int j = 0; j < camVisualizerHeight; j++)
{
int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth)));
int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight)));
btClamp(yIndex, 0, imageData.m_pixelHeight);
btClamp(xIndex, 0, imageData.m_pixelWidth);
int bytesPerPixel = 4; //RGBA
if (m_canvasRGBIndex >= 0)
{
int rgbPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth) * bytesPerPixel;
m_canvas->setPixel(m_canvasRGBIndex, i, j,
imageData.m_rgbColorData[rgbPixelIndex],
imageData.m_rgbColorData[rgbPixelIndex + 1],
imageData.m_rgbColorData[rgbPixelIndex + 2],
255); //alpha set to 255
}
if (m_canvasDepthIndex >= 0)
{
int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue > -1e20)
{
int rgb = 0;
if (maxDepthValue != minDepthValue)
{
rgb = (depthValue - minDepthValue) * (255. / (btFabs(maxDepthValue - minDepthValue)));
if (rgb < 0 || rgb > 255)
{
//printf("rgb=%d\n",rgb);
}
}
m_canvas->setPixel(m_canvasDepthIndex, i, j,
rgb,
rgb,
255, 255); //alpha set to 255
}
else
{
m_canvas->setPixel(m_canvasDepthIndex, i, j,
0,
0,
0, 255); //alpha set to 255
}
}
if (m_canvasSegMaskIndex >= 0 && (0 != imageData.m_segmentationMaskValues))
{
int segmentationMaskPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex];
btVector4 palette[4] = {btVector4(32, 255, 32, 255),
btVector4(32, 32, 255, 255),
btVector4(255, 255, 32, 255),
btVector4(32, 255, 255, 255)};
if (segmentationMask >= 0)
{
int obIndex = segmentationMask & ((1 << 24) - 1);
int linkIndex = (segmentationMask >> 24) - 1;
btVector4 rgb = palette[(obIndex + linkIndex) & 3];
m_canvas->setPixel(m_canvasSegMaskIndex, i, j,
rgb.x(),
rgb.y(),
rgb.z(), 255); //alpha set to 255
}
else
{
m_canvas->setPixel(m_canvasSegMaskIndex, i, j,
0,
0,
0, 255); //alpha set to 255
}
}
}
}
if (m_canvasRGBIndex >= 0)
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex >= 0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >= 0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
btVector3 Tire::getForce(
btScalar normal_load,
btScalar friction_coeff,
btScalar camber,
btScalar rot_velocity,
btScalar lon_velocity,
btScalar lat_velocity)
{
if (normal_load * friction_coeff < 1E-6)
{
slip = slip_angle = 0;
ideal_slip = ideal_slip_angle = 1;
fx = fy = fz = mz = 0;
vx = vy = 0;
return btVector3(0, 0, 0);
}
// limit input
btClamp(normal_load, btScalar(0), btScalar(max_load));
btClamp(camber, -max_camber, max_camber);
// sigma and alpha
btScalar denom = btMax(btFabs(lon_velocity), btScalar(1E-3));
btScalar lon_slip_velocity = lon_velocity - rot_velocity;
btScalar sigma = -lon_slip_velocity / denom;
btScalar alpha = btAtan(lat_velocity / denom);
// force parameters
btScalar Fz = normal_load;
btScalar Fz0 = nominal_load;
btScalar dFz = (Fz - Fz0) / Fz0;
// pure slip
btScalar Dy, BCy, Shf;
btScalar Fx0 = PacejkaFx(sigma, Fz, dFz, friction_coeff);
btScalar Fy0 = PacejkaFy(alpha, camber, Fz, dFz, friction_coeff, Dy, BCy, Shf);
btScalar Mz0 = PacejkaMz(alpha, camber, Fz, dFz, friction_coeff, Fy0, BCy, Shf);
// combined slip
btScalar Gx = PacejkaGx(sigma, alpha);
btScalar Gy = PacejkaGy(sigma, alpha);
btScalar Svy = PacejkaSvy(sigma, alpha, camber, dFz, Dy);
btScalar Fx = Gx * Fx0;
btScalar Fy = Gy * Fy0 + Svy;
// ideal slip and angle
btScalar sigma_hat(0), alpha_hat(0);
getSigmaHatAlphaHat(normal_load, sigma_hat, alpha_hat);
slip = sigma;
slip_angle = alpha;
ideal_slip = sigma_hat;
ideal_slip_angle = alpha_hat;
fx = Fx;
fy = Fy;
fz = Fz;
mz = Mz0;
vx = lon_slip_velocity;
vy = lat_velocity;
return btVector3(Fx, Fy, Mz0);
}
