Action ComponentBrainKamikaze::getNextAction_chase(float milliseconds) { if(!targetPlayer || isDead(targetPlayer) || (!canDetectPlayer(*targetPlayer) && getTimeInState()>resumeWanderThreshold)) { nextState = StateWander; } ComponentPhysics* component = dynamic_cast<ComponentPhysics*>(targetPlayer->getComponent("Physics")); if(!component) return Stand; vec3 playerPosition3D = component->getPosition(); vec3 ourPosition3D = getPosition3D(); vec2 playerPosition = playerPosition3D.xy(); vec2 ourPosition = ourPosition3D.xy(); float angle = getAngle(playerPosition, ourPosition); Direction desiredDirection = getDirectionFromAngle(angle); Direction currentFacing = getCurrentFacing(); return getActionFromAngle(angle); }
void GetupModule::cross() { if (getTimeInState() > 0.025) return; float angles[NUM_JOINTS]; for (int i = 0; i < NUM_JOINTS; i++) { angles[i] = 0; } angles[HeadPitch] = DEG_T_RAD*30; angles[LShoulderRoll] = 1.5708; angles[RShoulderRoll] = 1.5708; angles[LShoulderPitch] = -1.5708; angles[RShoulderPitch] = -1.5708; // walk arms //angles[LShoulderPitch] = DEG_T_RAD * -96; // -116 //moving to make it farther outside the body //angles[LShoulderRoll] = DEG_T_RAD * 8; angles[LElbowYaw] = DEG_T_RAD * 0;//25; angles[LElbowRoll] = DEG_T_RAD * 0;//-53; //angles[RShoulderPitch] = DEG_T_RAD * -96; // -116 //moving to make it farther outside the body //angles[RShoulderRoll] = DEG_T_RAD * 8; angles[RElbowYaw] = DEG_T_RAD * 0;//25; angles[RElbowRoll] = DEG_T_RAD * 0;//-53; processJointCommands(100,angles); commands_->setAllStiffness(1.0,10); }
void GetupModule::selectGetup() { int tiltSideThreshold = 5; if (numCrosses > 0) // be more confident after a cross tiltSideThreshold = 2; // decide which getup to do if (walk_request_->tilt_fallen_counter_ < -tiltSideThreshold) { setBackGetup(); } else if (walk_request_->tilt_fallen_counter_ > tiltSideThreshold) { setFrontGetup(); } else if ((numCrosses == 0) && (walk_request_->roll_fallen_counter_ >= 2)) { transitionToState(CROSS); } else if (getTimeInState() < 0.2) { // do nothing, maybe we'll end up somewhere good } else { setBackGetup(); // default to back in the end } }
Action ComponentBrainShooter::getNextAction_chase(float milliseconds) { if(!targetPlayer || isDead(targetPlayer) || (!canDetectPlayer(*targetPlayer) && getTimeInState()>resumeWanderThreshold)) { nextState = StateWander; } ComponentPhysics* component = dynamic_cast<ComponentPhysics*>(targetPlayer->getComponent("Physics")); if(!component) return Stand; vec3 playerPosition3D = component->getPosition(); vec3 ourPosition3D = getPosition3D(); vec2 playerPosition = playerPosition3D.xy(); vec2 ourPosition = ourPosition3D.xy(); float angle = getAngle(playerPosition, ourPosition); Direction desiredDirection = getDirectionFromAngle(angle); Direction currentFacing = getCurrentFacing(); if(currentFacing == desiredDirection && getDistance(playerPosition, ourPosition) < shootDistance && rayCast(*targetPlayer, ourPosition3D, playerPosition3D - ourPosition3D, maxSightDistance)) { getParentBlackBoard().relayMessage(MessagePerformAction(Stand)); return AttackOnce; } else { Action action = getActionFromAngle(angle); return action; } }
bool GetupModule::processFrameChild() { //std::cout << frame_info_->frame_id << " " << getName(state) << std::endl; // set getting up odometry if (isGettingUp()){ // which way are we getting up? odometry_->getting_up_side_ = getUpSide; } else { // no getup odometry_->getting_up_side_ = Getup::NONE; // set getup side back to unknown for the next getup getUpSide = Getup::UNKNOWN; } bool fallenCountHigh = (abs(walk_request_->roll_fallen_counter_) >= 50) || (abs(walk_request_->tilt_fallen_counter_) >= 50); switch (state) { case INITIAL: if (walk_request_->getup_from_keeper_dive_) { transitionToState(CROSS); // goalie needs cross } else { if (fallenCountHigh) transitionToState(STIFFNESS_ON); else transitionToState(PREPARE_ARMS); } break; case PREPARE_ARMS: if ((getTimeInState() > PREPARE_ARMS_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to prepare arms transitionToState(STIFFNESS_OFF); } else { if (getTimeInState() < 0.015) { prepareArms(); } // otherwise do nothing } break; case STIFFNESS_OFF: if ((getTimeInState() > STIFFNESS_OFF_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to worry about the stiffness being off transitionToState(STIFFNESS_ON); } else { commands_->setAllStiffness(0.0,10); } break; case STIFFNESS_ON: if (getTimeInState() > 0.001) { //if (armsStuckBehindBack()) { //std::cout << "Trying to free arms" << std::endl; //transitionToState(FREE_ARMS); //} else { transitionToState(EXECUTE); //} } else { commands_->setAllStiffness(1.0,10); } break; case CROSS: if (getTimeInState() < CROSS_TIME) cross(); else { transitionToState(EXECUTE); numCrosses++; } break; case EXECUTE: if (currMotion == Getup) { selectGetup(); if (currMotion == Getup) // didn't choose return true; // don't do anything else // yay! we chose a getup std::cout << "Starting getup: " << getName(currMotion) << std::endl; stateStartTime = frame_info_->seconds_since_start; // reset the time } if (isMotionDoneExecuting()) { if ((abs(walk_request_->tilt_fallen_counter_) > 2) || (abs(walk_request_->roll_fallen_counter_) > 2)) { // still fallen selectGetup(); transitionToState(STIFFNESS_ON); } else { transitionToState(STAND); } } else { if (getTimeInState() < 0.015) commands_->setAllStiffness(1.0,10); //if (shouldAbortGetup()) { // KG: for now, never abort. Consider adding it back in if needed, but will need to also update method for new getup states // std::cout << "FAILED GETUP" << std::endl; // numRestarts++; // transitionToState(PAUSE_BEFORE_RESTART); //} executeMotionSequence(); } break; case STAND: // do nothing, handled in MotionCore if (getTimeInState() > 0.5) transitionToState(FINISHED); break; case FREE_ARMS: currMotion = backFreeArms; if (isMotionDoneExecuting()) { currMotion = Getup; transitionToState(EXECUTE); } else { if (getTimeInState() < 0.015) commands_->setAllStiffness(1.0,10); executeMotionSequence(); } break; case PAUSE_BEFORE_RESTART: if (getTimeInState() > PAUSE_BEFORE_RESTART_TIME) { transitionToState(STIFFNESS_ON); } else { commands_->setAllStiffness(0.0,10); } break; default: return false; break; } return true; }