//-----------------------------------------------------------------------------
// 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;
}
