| 346 | |
| 347 | static btAlignedObjectArray<btScalar> qDesiredArray; |
| 348 | void InvertedPendulumPDControl::stepSimulation(float deltaTime) |
| 349 | { |
| 350 | static btScalar offset = -0.1 * SIMD_PI; |
| 351 | |
| 352 | m_frameCount++; |
| 353 | if ((m_frameCount & 0xff) == 0) |
| 354 | { |
| 355 | offset = -offset; |
| 356 | } |
| 357 | btScalar target = SIMD_PI + offset; |
| 358 | qDesiredArray.resize(0); |
| 359 | qDesiredArray.resize(m_multiBody->getNumLinks(), target); |
| 360 | |
| 361 | for (int joint = 0; joint < m_multiBody->getNumLinks(); joint++) |
| 362 | { |
| 363 | int dof1 = 0; |
| 364 | btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1]; |
| 365 | btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1]; |
| 366 | btScalar positionError = (qDesiredArray[joint] - qActual); |
| 367 | double desiredVelocity = 0; |
| 368 | btScalar velocityError = (desiredVelocity - qdActual); |
| 369 | btScalar force = kp * positionError + kd * velocityError; |
| 370 | btClamp(force, -maxForce, maxForce); |
| 371 | m_multiBody->addJointTorque(joint, force); |
| 372 | } |
| 373 | |
| 374 | if (m_frameCount == 100) |
| 375 | { |
| 376 | const char* gPngFileName = "pendulum"; |
| 377 | |
| 378 | if (gPngFileName) |
| 379 | { |
| 380 | //printf("gPngFileName=%s\n",gPngFileName); |
| 381 | |
| 382 | sprintf(fileName, "%s%d.png", gPngFileName, m_frameCount); |
| 383 | b3Printf("Made screenshot %s", fileName); |
| 384 | this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName); |
| 385 | } |
| 386 | } |
| 387 | m_dynamicsWorld->stepSimulation(1. / 60., 0); //240,0); |
| 388 | |
| 389 | static int count = 0; |
| 390 | if ((count & 0x0f) == 0) |
| 391 | { |
| 392 | #if 0 |
| 393 | for (int i=0;i<m_jointFeedbacks.size();i++) |
| 394 | { |
| 395 | |
| 396 | b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", |
| 397 | i, |
| 398 | m_jointFeedbacks[i]->m_reactionForces.m_topVec[0], |
| 399 | m_jointFeedbacks[i]->m_reactionForces.m_topVec[1], |
| 400 | m_jointFeedbacks[i]->m_reactionForces.m_topVec[2], |
| 401 | |
| 402 | m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0], |
| 403 | m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1], |
| 404 | m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2] |
| 405 |
nothing calls this directly
no test coverage detected