MAWalkMaster.cpp

/*
 *  This file is part of the AiBO+ project
 *
 *  Copyright (C) 2005-2016 Csaba Kertész (csaba.kertesz@gmail.com)
 *
 *  AiBO+ is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  AiBO+ is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Street #330, Boston, MA 02111-1307, USA.
 *
 */

#include "MAWalkMaster.hpp"

#include "core/MARandomness.hpp"
#include "types/MABodyMotion.hpp"
#include "types/MAComplexIndicators.hpp"
#include "types/MAGoals.hpp"
#include "types/MARobotState.hpp"
#include "types/MATorso.hpp"
#include "types/MATypeRanges.hpp"
#include "controllers/MAHeadController.hpp"
#include "controllers/MALegController.hpp"
#include "ml/MAClassifierModels.hpp"
#include "ml/MADataCollector.hpp"
#include "ml/MASurfaceAnalyzer.hpp"
#include "MAAvoidObstacleMaster.hpp"
#include "MABehaviorManager.hpp"
#include "MABodyAdjustStandingPosture.hpp"
#include "MABodyStateMaster.hpp"
#include "MABodyStroked.hpp"
#include "MAHeadLookAround.hpp"
#include "MAHeadLookAroundMaster.hpp"
#include "MARecognizeFloorSurface.hpp"
#include "MAWalkFreefall.hpp"
#include "MAWalkInitialPosition.hpp"
#include "MAWaitFloorFeedback.hpp"

#include <MCDataContainer.hpp>

MAWalkMaster::MAWalkMaster() : MABehavior(MCGetClassName(this)),
  MARobotStateUpdater(MA::UpdateComplexIndicators), WalkStartTimestamp(-1),
  RecognizedSurface((int)MA::UnknownFloor), RecognizedCurrentSurface((int)MA::UnknownFloor),
  SurfaceRecognitionTimer(new MCTimer(MC::SimpleExpiration))
{
  // The default walk period is 2400 and the dog is the same posture after every ~2030 msecs
  SurfaceRecognitionTimer->SetDuration(2410);
  RegisterUpdater(*this);
  // Avoid to activate the body state change behavior
  DesiredConnections[MCGetClassName<MABodyStateMaster>()] = -1.0;
  DesiredConnections[MCGetClassName<MABodyStroked>()] = -1.0;
  DesiredConnections[MCGetClassName<MAHeadLookAroundMaster>()] = -1.0;
}


void MAWalkMaster::StartWalk()
{
  if (GetState() == MABehavior::Activated)
  {
    WalkStartTimestamp = GetElapsedActivatedStateTime();
    // Set a random walk duration if it has not been set yet (exploration mode)
    if (MA::RobotState->Goals->DesiredForwardWalkDuration < 2000 &&
        MA::RobotState->Goals->DesiredBackwardWalkDuration < 2000)
    {
      // Set a random walk duration
      // Note: The first second is used to go into initial walk posture and ~10 seconds is
      // needed to get the underlying surface recognized.
      MA_RANDOM_POINT_1(NewDuration, 11000, 60000);

      MA::RobotState->Goals->DesiredForwardWalkDuration = NewDuration;
      MA::RobotState->Goals->DesiredBackwardWalkDuration = 0;
    }
  }
}


MABehavior::StimulusLevelType MAWalkMaster::GetCurrentStimulus()
{
  return MABehavior::OverStimulated;
}


void MAWalkMaster::ActivatingActions()
{
  MABEHAVIOR_CREATE(MAWalkInitialPosition, this);
  // Stop look around
  MABEHAVIOR_DELETE(MAHeadLookAround)
}


void MAWalkMaster::ActivatedStateUpdate()
{
  // Check if forward walk is in progress
  if ((int)MA::RobotState->ComplexIndicators->ElapsedForwardWalkTime == 0)
    return;

  // Create the surface analyzer
  if ((MA::RobotState->ComplexIndicators->ElapsedForwardWalkTime >= 150*32) &&
      !SurfaceAnalyzer.get())
  {
    SurfaceAnalyzer.reset(new MASurfaceAnalyzer(150));
    MABEHAVIOR_CREATE(MARecognizeFloorSurface, this);
  }
  // Update the surface analyzer
  if (SurfaceAnalyzer.get())
  {
    SurfaceAnalyzer->AddSamples(*MA::RobotState);
  }
  if (SurfaceAnalyzer.get() && SurfaceAnalyzer->IsValid() &&
      RecognizedSurface == (int)MA::UnknownFloor)
  {
    // Do the initial surface analysis
    MC::FloatList SurfaceFeatureVector = SurfaceAnalyzer->GetFeatureVector();

    MCPrintContainerAsFloats(SurfaceFeatureVector, "Surface feature vector", 4);
    // Add the surface vector to data collection as custom data
    MC::BinaryDataSPtr BinaryData(MCEncodeToBinaryData(SurfaceFeatureVector));
    MC::FloatList Confidence;

    MA::DataCollector->AddCustomDataToActiveJob(MA::FloorSurfaceModelID, MA::FloorSurfaceVectorID,
                                                *BinaryData);
    // Recognize the surface
    RecognizedSurface = (int)MA::ClassifierModels->Recognize(MA::FloorSurfacesID, SurfaceFeatureVector,
                                                             Confidence);
    RecognizedCurrentSurface = RecognizedSurface;
    if (RecognizedSurface == (int)MA::WoodFlooring)
      MC_LOG("Recognized: wood flooring");
    else
    if (RecognizedSurface == (int)MA::Field)
      MC_LOG("Recognized: field");
    else
    if (RecognizedSurface == (int)MA::CarpetedFloor)
      MC_LOG("Recognized: carpeted floor");
    else
    if (RecognizedSurface == (int)MA::Carpet)
    {
      MA_RANDOM_POINT_1(Reaction, 1, 5);

      MC_LOG("Recognized: carpet");
      if (Reaction == 5)
      {
        // Calculate a random remaining walk duration before the happy feedback
        MA_RANDOM_POINT_1(Divider, 1, 9);
        int Duration = MA::RobotState->Goals->DesiredForwardWalkDuration-
                           MA::RobotState->ComplexIndicators->ElapsedForwardWalkTime;

        Duration = (int)((float)Duration / Divider)+MA::RobotState->ComplexIndicators->ElapsedForwardWalkTime;
        MA::RobotState->Goals->DesiredForwardWalkDuration = Duration;
        MA::RobotState->Goals->HappyOnCarpetFeedback = 1;
      }
    } else
    if (RecognizedSurface == (int)MA::Vinyl)
      MC_LOG("Recognized: vinyl");
    else
    if (RecognizedSurface == (int)MA::Tiles)
    {
      MC_LOG("Recognized: tiles");
    } else {
      MC_LOG("Recognized: unknown");
    }
    // Add the recognized surface to data collection as custom data
    BinaryData.reset(MCEncodeToBinaryData(RecognizedSurface));
    MA::DataCollector->AddCustomDataToActiveJob(MA::FloorSurfaceModelID, MA::RecognizedFloorSurfaceID,
                                                *BinaryData);
    // Start to wait the owner feedback about the actual surface
    MABEHAVIOR_CREATE(MAWaitFloorFeedback, nullptr);
    SurfaceRecognitionTimer->Start();
  } else
  // The surface is recognized after every ~2400 msec when the robot body is in the same posture.
  if (SurfaceAnalyzer.get() && SurfaceAnalyzer->IsValid() &&
      SurfaceRecognitionTimer->Timeout())
  {
    SurfaceRecognitionTimer->Start();
    // Do the continuous surface analysis to detect any anomalies
    MC::FloatList SurfaceFeatureVector = SurfaceAnalyzer->GetFeatureVector();
    MC::FloatList Confidence;

    MCPrintContainerAsFloats(SurfaceFeatureVector, "Surface feature vector", 4);
    RecognizedCurrentSurface = (int)MA::ClassifierModels->Recognize(MA::FloorSurfacesID,
                                                                    SurfaceFeatureVector, Confidence);
    if (RecognizedCurrentSurface == (int)MA::WoodFlooring)
      MC_LOG("Recognized: wood flooring");
    else
    if (RecognizedCurrentSurface == (int)MA::Field)
      MC_LOG("Recognized: field");
    else
    if (RecognizedCurrentSurface == (int)MA::Carpet)
      MC_LOG("Recognized: carpet");
    else
    if (RecognizedCurrentSurface == (int)MA::Vinyl)
      MC_LOG("Recognized: vinyl");
    else
    if (RecognizedCurrentSurface == (int)MA::Tiles)
    {
      MC_LOG("Recognized: tiles");
    } else {
      MC_LOG("Recognized: unknown");
    }
  }
}


bool MAWalkMaster::IsFailed()
{
  bool Failed = MA::RobotState->ComplexIndicators->BodyOutOfWalkingRange > 90;

  Failed = Failed || MA::RobotState->Torso->CycleTime > MA::WalkCycleTimeMax;
  if (MA::RobotState->Torso->CycleTime > MA::WalkCycleTimeWarning)
  {
    MC_WARNING("Cycle time is over the safe level (%d > %d)",
               (int)MA::RobotState->Torso->CycleTime, MA::WalkCycleTimeWarning);
  } else
  if (MA::RobotState->Torso->CycleTime > MA::WalkCycleTimeMax)
  {
    // The freefall behavior must be created in advance to block the MABodyStateMaster behavior after
    // MAWalkMaster fails.
    MABEHAVIOR_CREATE(MAWalkFreefall, nullptr, true);
    MC_WARNING("Cycle time is over the emergency level (%d > %d)",
               (int)MA::RobotState->Torso->CycleTime, MA::WalkCycleTimeMax);
  }
  if (!MA::RobotState->BodyMotion->IsBodyRollInWalkRange())
  {
    // The freefall behavior must be created in advance to block the MABodyStateMaster behavior after
    // MAWalkMaster fails.
    MABEHAVIOR_CREATE(MAWalkFreefall, nullptr, false);
  }
  return Failed;
}


void MAWalkMaster::FinishingActions()
{
  // It may happen that the robot falls down, but this master behavior does not fail...
  if (!MA::RobotState->BodyMotion->IsBodyRollInWalkRange() || MABEHAVIOR_EXISTS(MAAvoidObstacleMaster))
  {
    MABEHAVIOR_CREATE(MAWalkFreefall, nullptr, false);
  }
  MABEHAVIOR_CREATE(MABodyAdjustStandingPosture, nullptr);
}


void MAWalkMaster::UpdateRobotState(MARobotState& state)
{
  if (WalkStartTimestamp != -1 && GetState() == MABehavior::Activated)
  {
    int ElapsedTime = MCMax(0, GetElapsedActivatedStateTime()-WalkStartTimestamp);

    if (state.Goals->DesiredForwardWalkDuration > 0)
    {
      state.ComplexIndicators->ElapsedForwardWalkTime = ElapsedTime;
    } else {
      state.ComplexIndicators->ElapsedBackwardWalkTime = ElapsedTime;
    }
  } else {
    if (state.Goals->DesiredForwardWalkDuration > 0)
    {
      state.ComplexIndicators->ElapsedForwardWalkTime = 0;
    } else {
      state.ComplexIndicators->ElapsedBackwardWalkTime = 0;
    }
  }
  state.BodyMotion->Floor.Surface = RecognizedSurface;
  state.BodyMotion->Floor.CurrentSurface = RecognizedCurrentSurface;
}