//----------------------------------------------------------------------------- // flushAllMotions() //----------------------------------------------------------------------------- void LLMotionController::flushAllMotions() { std::vector<std::pair<LLUUID,F32> > active_motions; active_motions.reserve(mActiveMotions.size()); for (motion_list_t::iterator iter = mActiveMotions.begin(); iter != mActiveMotions.end(); ) { motion_list_t::iterator curiter = iter++; LLMotion* motionp = *curiter; F32 dtime = mAnimTime - motionp->mActivationTimestamp; active_motions.push_back(std::make_pair(motionp->getID(),dtime)); motionp->deactivate(); // don't call deactivateMotionInstance() because we are going to reactivate it } mActiveMotions.clear(); // delete all motion instances deleteAllMotions(); // kill current hand pose that was previously called out by // keyframe motion mCharacter->removeAnimationData("Hand Pose"); // restart motions for (std::vector<std::pair<LLUUID,F32> >::iterator iter = active_motions.begin(); iter != active_motions.end(); ++iter) { startMotion(iter->first, iter->second); } }
BOOL AnimationExplorer::postBuild() { mAnimationScrollList=getChild<LLScrollListCtrl>("animation_list"); mStopButton=getChild<LLButton>("stop_btn"); mRevokeButton=getChild<LLButton>("revoke_btn"); mStopAndRevokeButton=getChild<LLButton>("stop_and_revoke_btn"); mNoOwnedAnimationsCheckBox=getChild<LLCheckBoxCtrl>("no_owned_animations_check"); mAnimationScrollList->setCommitCallback(boost::bind(&AnimationExplorer::onSelectAnimation,this)); mStopButton->setCommitCallback(boost::bind(&AnimationExplorer::onStopPressed,this)); mRevokeButton->setCommitCallback(boost::bind(&AnimationExplorer::onRevokePressed,this)); mStopAndRevokeButton->setCommitCallback(boost::bind(&AnimationExplorer::onStopAndRevokePressed,this)); mNoOwnedAnimationsCheckBox->setCommitCallback(boost::bind(&AnimationExplorer::onOwnedCheckToggled,this)); mPreviewCtrl=findChild<LLView>("animation_preview"); if(mPreviewCtrl) { mAnimationPreview=new LLPreviewAnimation( mPreviewCtrl->getRect().getWidth(),mPreviewCtrl->getRect().getHeight() ); mAnimationPreview->setZoom(2.0f); startMotion(LLUUID::null); } else { LL_WARNS("AnimationExplorer") << "Could not find animation preview control to place animation texture" << LL_ENDL; } // request list of recent animations update(); return TRUE; }
void GetupModule::initGetup() { getUpSide = Getup::UNKNOWN; startMotion(Getup); numCrosses = 0; numRestarts = 0; //std::cout << "INIT GETUP" << std::endl; }
void AnimationExplorer::update() { // stop playing preview animations when reloading the list startMotion(LLUUID::null); mAnimationScrollList->deleteAllItems(); RecentAnimationList::instance().requestList(this); }
void mouse(int button, int state, int x, int y) { if(state == GLUT_DOWN) startMotion(x, y, button, glutGet(GLUT_ELAPSED_TIME)); else if (state == GLUT_UP) stopMotion(x, y, button, glutGet(GLUT_ELAPSED_TIME)); glutPostRedisplay(); }
void mouseButton(int button,int state,int x,int y){ if(button==GLUT_RIGHT_BUTTON) exit(0); if(button==GLUT_LEFT_BUTTON) switch(state){ case GLUT_DOWN: y=winHeight-y;startMotion(x,y);break; case GLUT_UP : stopMotion(x,y);break; } }
void AnimationExplorer::onSelectAnimation() { LLScrollListItem* item=mAnimationScrollList->getFirstSelected(); if(!item) { return; } S32 column=mAnimationScrollList->getColumn("animation_id")->mIndex; mCurrentAnimationID=item->getColumn(column)->getValue().asUUID(); column=mAnimationScrollList->getColumn("played_by")->mIndex; mCurrentObject=item->getColumn(column)->getValue().asUUID(); startMotion(mCurrentAnimationID); }
//right click exits program, left click down begins startMotion, left click up triggers stopMotion void mouse(int button, int state, int x, int y) { //exit program if(button==GLUT_RIGHT_BUTTON) { exit(0); } if(button==GLUT_LEFT_BUTTON) { switch(state) { case GLUT_DOWN: y = winHeight - y; startMotion(x, y); break; case GLUT_UP: stopMotion(x,y); break; } } }
void mouseButton(int button, int state, int x, int y) { switch(button) { case GLUT_LEFT_BUTTON: trackballXform = (GLfloat*)objectXform; break; case GLUT_MIDDLE_BUTTON: trackballXform = (GLfloat*)lightXform; break; } switch(state) { case GLUT_DOWN: startMotion(0, 1, x, y); break; case GLUT_UP: stopMotion(0, 1, x, y); break; } }
// Called when a mouse button is pressed or released void mouseCallback(int button, int state, int x, int y) { switch (button) { case GLUT_LEFT_BUTTON: trackballXform = (float *)objectXform; break; case GLUT_RIGHT_BUTTON: case GLUT_MIDDLE_BUTTON: trackballXform = (float *)lightXform; break; } switch (state) { case GLUT_DOWN: if (button == GLUT_RIGHT_BUTTON) { zoomState = true; lastPos[1] = (float) y; } else if (button == GLUT_MIDDLE_BUTTON) { shiftState = true; lastPos[0] = (float) x; lastPos[1] = (float) y; } else startMotion(0, 1, x, y); break; case GLUT_UP: trackballXform = (float *)lightXform; // turns off mouse effects if (button == GLUT_RIGHT_BUTTON) { zoomState = false; } else if (button == GLUT_MIDDLE_BUTTON) { shiftState = false; } else stopMotion(0, 1, x, y); break; } }
void setModeAction(){ int log = 0; int *modeAct = &mode1act[modeCounter][0]; int counterMax = mode1act[0][1]; // if (log == 1) printf("setModeAction modeCounter:%d\n", modeCounter); switch(mModeAct){ case MODE_ACT_1: // do nothing break; case MODE_ACT_2: modeAct = &mode2act[modeCounter][0]; counterMax = mode2act[0][1]; break; case MODE_ACT_3: modeAct = &mode3act[modeCounter][0]; counterMax = mode3act[0][1]; break; case MODE_ACT_4: modeAct = &mode4act[modeCounter][0]; counterMax = mode4act[0][1]; break; case MODE_ACT_5: modeAct = &mode5act[modeCounter][0]; counterMax = mode5act[0][1]; break; case MODE_ACT_6: modeAct = &mode6act[modeCounter][0]; counterMax = mode6act[0][1]; break; case MODE_ACT_7: modeAct = &mode7act[modeCounter][0]; counterMax = mode7act[0][1]; break; case MODE_ACT_8: modeAct = &mode8act[modeCounter][0]; counterMax = mode8act[0][1]; break; default: break; } // if (log == 1) printf("setModeAction motionTimes:%d, counterMax:%d\n", motionTimes, counterMax); if( motionTimes <= 0 ){ if (log == 1) printf("setModeAction 2\n"); if( mMode == MODE_1 ){ if( preMotionNumber != ACT_WALK2 && preMotionNumber != ACT_PRE_WALK && modeAct[0] == ACT_WALK2 ){ startMotion( ACT_PRE_WALK, 1 ); if (log == 1) printf("setModeAction 3\n"); }else{ startMotion( modeAct[0], modeAct[1] ); if( ++modeCounter > counterMax ){ if (log == 1) printf("setModeAction 4\n"); // PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; // mMode = MODE_0; modeCounter = 1; judgeModeAct(); } else { if (log == 1) printf("setModeAction 5\n"); } } } } }
int main(void){ int log = 0; //Start Switch // DDRA = 0x00; // PORTA = 0x12; //Start PORT A for switch and IR sensors DDRA = 0xFC; PORTA = 0xFE; //LED Initial DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; MotorInit(); initSerial(); char * readData = NULL; int isFinish = 0; sensorInit(); if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); while(1){ sensorTest(0); sensorTest(1); sensorTest(2); setMode(); if( checkSerialRead() > 0 ){ readData = getReadBuffer(); if( readData != NULL ){ // printf( "readData=%s\n", &readData[0] ); split( &readData[0] ); switch( serCmd[0] ){ case EVT_ACTION: ServoControl( serCmd[1] ); // setSpeedTest( serCmd[1] ); sendAck(1); break; case EVT_START_MOTION: startMotion( serCmd[1], serCmd[2] ); PORTC = ~(1 << (LED_MAX - 2)); sendAck(1); break; case EVT_STOP_MOTION: stopMotion(); sendAck(1); break; case EVT_FORCE_MOTION: forceMotion( serCmd[1], serCmd[2] ); break; case EVT_GET_NOW_ANGLE: getAngle(); break; case EVT_SET_ANGLE: setAngle(); case EVT_GET_ACT_ANGLE: if( serCmd[1] >= ACT_MAX ){ sendAck(0); }else{ sendActAngle(serCmd[1]); } break; case EVT_GET_LOAD: getLoad(); // printf( "%d\n", movingTime ); break; case EVT_GET_VOLTAGE: getVoltage(); break; case EVT_TORQUE_DISABLE: dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); break; case EVT_WATCH_DOG: watchDogCnt = 0; break; case EVT_MOTION_EDIT: break; case 999: // printf( "finish\n"); sendAck(999); isFinish = 1; break; default: sendAck(0); } if( isFinish > 0 ){ MotorControl( 0, 0 ); break; } memset( readData, 0x00, SERIAL_BUFFER_SIZE ); } } memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE ); if (~PINA & SW_START ) { if (log == 1) printf( "main() 0\n"); if( iStart > 0 ){ iStart = 0; PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; if (isCaptureMode != 1) ServoControl( 0 ); } }else{ if( iStart == 0 ){ PORTC &= ~LED_PLAY; iStart = 1; } if( modeWait <= 0 ){ if (log == 1) printf( "main() 1\n"); setModeAction(); if (isMovetest == 1) { moveTest(); } else { move(); } }else{ if (log == 1) printf( "main() 2\n"); modeWait -= MAIN_DELAY; } } if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) { if (log == 1) printf( "### main() sensorValue[0] == 0\n"); PORTC |= LED_PROGRAM; //edit }else if (sensorValueOld[0] != sensorValue[0]){ if (log == 1) printf( "### main() sensorValue[0] == 1\n"); PORTC &= ~LED_PROGRAM; //edit } if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) { if (log == 1) printf( "### main() sensorValue[1] == 0\n"); PORTC |= LED_MANAGE; //mon }else if (sensorValueOld[1] != sensorValue[1]){ if (log == 1) printf( "### main() sensorValue[1] == 1\n"); PORTC &= ~LED_MANAGE; //mon } if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) { if (log == 1) printf( "### main() sensorValue[2] == 0\n"); PORTC |= LED_AUX; //AUX }else if (sensorValueOld[2] != sensorValue[2]){ if (log == 1) printf( "### main() sensorValue[2] == 1\n"); PORTC &= ~LED_AUX; //AUX } sensorValueOld[0] = sensorValue[0]; sensorValueOld[1] = sensorValue[1]; sensorValueOld[2] = sensorValue[2]; // walk pattern LED // brinkLED(); _delay_ms(MAIN_DELAY); watchDogCnt++; caputureCount1++; if (caputureCount1 == 25){ if (isCaptureMode == 1) getAngle(); caputureCount1 = 0; } } }
int main(int argc, char **argv) { int i = 0; char ip[8][64] = {"192.168.16.213", "192.168.16.213", "192.168.16.213", "192.168.16.213", "192.168.16.213", "192.168.16.213"}; int ipNum = 2; #if 1 #ifndef X86 int resolution = PIC_HD1080; #endif catchSignal(); // memroySet(); if(argc > 1) { for(i = 1; i < argc; i++) { memset(ip[i - 1], 0, sizeof(ip[i - 1])); strcpy(ip[i - 1], argv[i]); } ipNum = argc - 1; } if(ipNum < 2) { ipNum = 2; } databaseLibInit(); //writeLog(LOG_MAJOR_SYSTEM, 0, 0, "system", "127.0.0.1", "system start"); system("rm -rf /config/*.log"); P2pSnUpdate(); //P2P┐пок P2pInitStart(); startWtdThread(); configLibInit(); gpioLibInit(); streamLibInit(); //logLibInit(); startGuiSerVer(); //gpioSetIcPower(1); //PHONE_ThrStart(); //databaseLibInit(); // gpioSetBeep(0);// // gpioSetDiskPower(0); setpriority(PRIO_PROCESS, getpid(), -20); /* */ // myEvLibInit(); myThreadLibInit(); diskLibInit(); threadPoolInit(6 * configGetDisplayNum(), 6 * configGetDisplayNum());/**/ decodeLibInit(0, configGetDisplayNum()); networkLibInit(); appLibInit(); startBoaThread(); //diskLibInit(); playbackLibInit(); recordLibInit(); p2pTHRStart(0); ///StartDdnsThread(); ///TE_platformCreate(); #ifndef X86 hi_audio_init(PT_G711U); #endif alarmLibInit(); sleep(1); rtspServerStart(); usleep(100*1000); discovery(); appLibStart(); sleep(2); startMotion(); startProcessManage(); ////startSmtpThread(); #ifndef HI3531 SnapThreadStart(); #endif setPthreadPriority(PTHREAD_SCHEDULED_PRIORITY_LOW_EST); #endif //appNtpSettime(); while(1) { sleep(60); } stopWtdthread(); decodeLibdestroy(configGetDisplayNum()); networkLibDestroy(); appLibDestroy(); myEvLibDestroy(); diskLibDestroy(); hi_audio_destroy(); alarmLibDestroy(); gpioLibDestroy(); streamLibUninit(); return 0; }
ompl::base::PlannerStatus ompl::geometric::TRRTConnect::solve(const base::PlannerTerminationCondition &ptc) { //Basic error checking checkValidity(); //Goal information base::GoalSampleableRegion *goal(dynamic_cast<base::GoalSampleableRegion*> (pdef_->getGoal().get())); if (!goal) { OMPL_ERROR("%s: Unknown type of goal",getName().c_str()); return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; } //Input States //Loop through valid input states and add to tree while (const base::State *st = pis_.nextStart()) { //Create a motion Motion *motion(new Motion(si_)); si_->copyState(motion->state,st); motion->root = motion->state; //Add motion to the tree tStart_->add(motion); si_->copyState(tStart_.root,motion->root); //Check if now the tree has a motion inside BoxZos if (!tStart_.stateInBoxZos_) { tStart_.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())-> boxZosDistance(motion->state) < DBL_EPSILON; if (tStart_.stateInBoxZos_) tStart_.temp_ = initTemperature_; } //Update frontier nodes and non frontier nodes count ++tStart_.frontierCount_;//Participates in the tree expansion } //Check that input states exist if (tStart_->size() == 0) { OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); return base::PlannerStatus::INVALID_START; } //Check that goal states exist if (!goal->couldSample()) { OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } //Create state sampler if the first run if (!sampler_) sampler_ = si_->allocStateSampler(); OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),int(tStart_->size() + tGoal_->size())); Motion *rmotion(new Motion(si_)); base::State *rstate(rmotion->state); TreeGrowingInfo tgi; tgi.xstate = si_->allocState(); bool startTree(true); bool solved(false); double temp(0.0); unsigned int iter(0); extendCount = 0; connectCount = 0; //Begin sampling while (!ptc) { //Choose tree to extend TreeData &tree(startTree ? tStart_ : tGoal_); startTree = !startTree; TreeData &otherTree(startTree ? tStart_ : tGoal_); if (((2*pis_.getSampledGoalsCount()) < (tGoal_->size())) || (tGoal_->size() == 0)) { const base::State *st((tGoal_->size() == 0) ? pis_.nextGoal(ptc) : pis_.nextGoal()); if (st) { //Create a motion Motion *motion(new Motion(si_)); si_->copyState(motion->state,st); motion->root = motion->state; //Add motion to the tree tGoal_->add(motion); si_->copyState(tGoal_.root,motion->root); //Check if now the tree has a motion inside BoxZos if (!tGoal_.stateInBoxZos_) { tGoal_.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())-> boxZosDistance(motion->state) < DBL_EPSILON; if (tGoal_.stateInBoxZos_) tGoal_.temp_ = initTemperature_; } //Update frontier nodes and non frontier nodes count ++tGoal_.frontierCount_;//Participates in the tree expansion } if (tGoal_->size() == 0) { OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str()); break; } } //Sample random state if (rng_.uniform01() < 0.05) { si_->copyState(rstate,otherTree.root); } else { sampler_->sampleUniform(rstate); } //Extend tree ExtendResult extendResult(extend(tree,tgi,rmotion)); temp += tree.temp_; iter++; if (extendResult != TRAPPED) { //Remember which motion was just added Motion *addedMotion(tgi.xmotion); //If reached, it means we used rstate directly, no need to copy again if (extendResult == ADVANCED) si_->copyState(rstate,tgi.xstate); //Attempt to connect trees otherTree.temp_ *= tempChangeFactor_; bool connected(connect(otherTree,tgi,rmotion)); otherTree.temp_ /= tempChangeFactor_; Motion *startMotion(startTree ? tgi.xmotion : addedMotion); Motion *goalMotion(startTree ? addedMotion : tgi.xmotion); //If we connected the trees in a valid way (start and goal pair is valid) if (connected && goal->isStartGoalPairValid(startMotion->root, goalMotion->root)) { //It must be the case that either the start tree or the goal tree //has made some progress so one of the parents is not NULL. //We go one step 'back' to avoid having a duplicate state //on the solution path if (startMotion->parent) { startMotion = startMotion->parent; } else { goalMotion = goalMotion->parent; } connectionPoint_ = std::make_pair(startMotion->state,goalMotion->state); //Construct the solution path Motion *solution(startMotion); std::vector<Motion*> mpath1; while (solution) { mpath1.push_back(solution); solution = solution->parent; } solution = goalMotion; std::vector<Motion*> mpath2; while (solution) { mpath2.push_back(solution); solution = solution->parent; } //Set the solution path PathGeometric *path(new PathGeometric(si_)); path->getStates().reserve(mpath1.size() + mpath2.size()); for (int i(mpath1.size() - 1); i >= 0; --i) { path->append(mpath1[i]->state); } for (std::size_t i(0); i < mpath2.size(); ++i) { path->append(mpath2[i]->state); } pdef_->addSolutionPath(base::PathPtr(path),false,0.0); solved = true; break; } } } //Clean up si_->freeState(tgi.xstate); si_->freeState(rstate); delete rmotion; OMPL_INFORM("%s: Created %u states (%u start + %u goal)",getName().c_str(), tStart_->size()+tGoal_->size(),tStart_->size(),tGoal_->size()); OMPL_INFORM("%s: Average temperature %f ",getName().c_str(),temp/double(iter)); //std::cout << extendCount << " " << connectCount << " " << iter << std::endl; double costs(0); std::vector<Motion*> start; tStart_->list(start); for (unsigned int i = 0; i < start.size(); ++i) { if (start.at(i)->parent) { costs += opt_->motionCost(start.at(i)->parent->state,start.at(i)->state).value() /si_->distance(start.at(i)->parent->state,start.at(i)->state); } } std::vector<Motion*> data; tGoal_->list(data); for (unsigned int i = 0; i < data.size(); ++i) { if (data.at(i)->parent) { costs += opt_->motionCost(data.at(i)->state,data.at(i)->parent->state).value() /si_->distance(data.at(i)->parent->state,data.at(i)->state); } } //std::cout << "avg cost " << costs/double(start.size()+data.size()-2) << std::endl; return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }