bool AISTSimulatorItem::isForcedPositionActiveFor(BodyItem* bodyItem) const { bool isActive = false; if(impl->forcedBodyPositionFunctionId){ SimulationBody* simBody = const_cast<AISTSimulatorItem*>(this)->findSimulationBody(bodyItem); { boost::unique_lock<boost::mutex> lock(impl->forcedBodyPositionMutex); if(impl->forcedPositionBody == static_cast<DyBody*>(simBody->body())){ isActive = true; } } } return isActive; }
bool AISTSimulatorItem::stepSimulation(const std::vector<SimulationBody*>& activeSimBodies) { impl->world.constraintForceSolver.clearExternalForces(); if(!impl->dynamicsMode.is(KINEMATICS)){ impl->world.calcNextState(); return true; } // Kinematics mode if(!impl->isKinematicWalkingEnabled){ for(size_t i=0; i < activeSimBodies.size(); ++i){ activeSimBodies[i]->body()->calcForwardKinematics(true, true); } } else { for(size_t i=0; i < activeSimBodies.size(); ++i){ SimulationBody* simBody = activeSimBodies[i]; KinematicWalkBody* walkBody = dynamic_cast<KinematicWalkBody*>(simBody); if(!walkBody){ simBody->body()->calcForwardKinematics(true, true); } else { walkBody->traverse.calcForwardKinematics(true, true); LeggedBodyHelper* legged = walkBody->legged; const int supportFootIndex = walkBody->supportFootIndex; int nextSupportFootIndex = supportFootIndex; Link* supportFoot = legged->footLink(supportFootIndex); Link* nextSupportFoot = supportFoot; const int n = legged->numFeet(); for(int i=0; i < n; ++i){ if(i != supportFootIndex){ Link* foot = legged->footLink(i); if(foot->p().z() < nextSupportFoot->p().z()){ nextSupportFootIndex = i; nextSupportFoot = foot; } } } if(nextSupportFoot != supportFoot){ nextSupportFoot->p().z() = supportFoot->p().z(); walkBody->supportFootIndex = nextSupportFootIndex; supportFoot = nextSupportFoot; walkBody->traverse.find(supportFoot, true, true); walkBody->traverse.calcForwardKinematics(true, true); } } } } return true; }
bool GLVisionSimulatorItemImpl::initializeSimulation(SimulatorItem* simulatorItem) { if(!QGLPixelBuffer::hasOpenGLPbuffers()){ os << (format(_("The vision sensor simulation by %1% cannot be performed because the OpenGL pbuffer is not available.")) % self->name()) << endl; return false; } glFormat = QGLFormat::defaultFormat(); glFormat.setDoubleBuffer(false); this->simulatorItem = simulatorItem; worldTimeStep = simulatorItem->worldTimeStep(); currentTime = 0; visionRenderers.clear(); useThread = useThreadProperty; if(useThread){ if(useThreadsForSensorsProperty){ useQueueThreadForAllSensors = false; useThreadsForSensors = true; } else { useQueueThreadForAllSensors = true; useThreadsForSensors = false; } } else { useQueueThreadForAllSensors = false; useThreadsForSensors = false; } isBestEffortMode = isBestEffortModeProperty; renderersInRendering.clear(); cloneMap.clear(); #ifdef CNOID_REFERENCED_USE_ATOMIC_COUNTER cloneMap.setNonNodeCloning(false); cout << "cloneMap.setNonNodeCloning(false);" << endl; #else cloneMap.setNonNodeCloning(true); cout << "cloneMap.setNonNodeCloning(true);" << endl; #endif std::set<string> bodyNameSet; for(size_t i=0; i < bodyNames.size(); ++i){ bodyNameSet.insert(bodyNames[i]); } std::set<string> sensorNameSet; for(size_t i=0; i < sensorNames.size(); ++i){ sensorNameSet.insert(sensorNames[i]); } const vector<SimulationBody*>& simBodies = simulatorItem->simulationBodies(); for(size_t i=0; i < simBodies.size(); ++i){ SimulationBody* simBody = simBodies[i]; Body* body = simBody->body(); if(bodyNameSet.empty() || bodyNameSet.find(body->name()) != bodyNameSet.end()){ for(size_t j=0; j < body->numDevices(); ++j){ Device* device = body->device(j); if(dynamic_cast<Camera*>(device) || dynamic_cast<RangeSensor*>(device)){ if(sensorNameSet.empty() || sensorNameSet.find(device->name()) != sensorNameSet.end()){ addTargetSensor(simBody, i, device); } } } } } if(visionRenderers.empty()){ os << (format(_("%1% has no target sensors")) % self->name()) << endl; return false; } #ifdef Q_OS_LINUX /** The following code is neccessary to avoid a crash when a view which has a widget such as QPlainTextEdit and has not been focused yet is first focused (clikced) during the camera image simulation processed by GLVisionSimulatorItem. The crash only occurs in Linux with the nVidia proprietary X driver. If the user clicks such a view to give the focus before the simulation started, the crash doesn't occur, so here the focus is forced to be given by the following code. */ if(QWidget* textEdit = MessageView::instance()->findChild<QWidget*>("TextEdit")){ textEdit->setFocus(); //! todo restore the previous focus here } #endif vector<VisionRendererPtr>::iterator p = visionRenderers.begin(); while(p != visionRenderers.end()){ VisionRenderer* renderer = p->get(); if(renderer->initialize(simBodies)){ ++p; } else { os << (format(_("%1%: Target sensor \"%2%\" cannot be initialized.")) % self->name() % renderer->device->name()) << endl; p = visionRenderers.erase(p); } } if(!visionRenderers.empty()){ simulatorItem->addPreDynamicsFunction(boost::bind(&GLVisionSimulatorItemImpl::onPreDynamics, this)); simulatorItem->addPostDynamicsFunction(boost::bind(&GLVisionSimulatorItemImpl::onPostDynamics, this)); if(useQueueThreadForAllSensors){ while(!rendererQueue.empty()){ rendererQueue.pop(); } isQueueRenderingTerminationRequested = false; queueThread = boost::thread( boost::bind(&GLVisionSimulatorItemImpl::queueRenderingLoop, this)); } return true; } return false; }