MABodyMotion.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 "MABodyMotion.hpp"

#include "ml/MAClassifierModels.hpp"
#include "MAComplexIndicators.hpp"
#include "MARobotState.hpp"
#include "MATorso.hpp"
#include "MATypeRanges.hpp"

#include <MCSampleStatistics.hpp>

#include <boost/math/constants/constants.hpp>

MAAccelData::MAAccelData()
{
  Raw.SetMinMax(MA::AccelRawMin, MA::AccelRawMax);
  // Note: Real magnitude for value representation on diagrams
  Raw.SetMagnitude(1000000.0);
  Degree.SetMinMax(MA::AccelMin, MA::AccelMax);
}


std::string MAFloorSurfacesTypeStr(const MA::FloorSurfacesType surface)
{
  if (surface == MA::WoodFlooring)
    return "Wood flooring";
  if (surface == MA::Field)
    return "Field";
  if (surface == MA::Carpet)
    return "Carpet";
  if (surface == MA::FoamMat)
    return "Foam mat";
  if (surface == MA::Vinyl)
    return "Vinyl";
  if (surface == MA::Tiles)
    return "Tiles";
  if (surface == MA::CarpetedFloor)
    return "Carpeted floor";
  if (surface == MA::UserDefinedFloor)
    return "User defined";

  return "Unknown";
}


std::string MAFloorSurfaceSoftnessTypeStr(const MA::FloorSurfaceSoftnessType surface_softness)
{
  if (surface_softness == MA::HardFlooring)
    return "Hard floor surface";
  if (surface_softness == MA::SoftFlooring)
    return "Soft floor surface";

  return "Very soft floor surface";
}


MA::FloorSurfaceSoftnessType MAMapFloorSurfaceToSoftness(const MA::FloorSurfacesType surface)
{
  if (surface == MA::WoodFlooring)
    return MA::HardFlooring;
  if (surface == MA::Field)
    return MA::SoftFlooring;
  if (surface == MA::Carpet)
    return MA::VerySoftFlooring;
  if (surface == MA::FoamMat)
    return MA::SoftFlooring;
  if (surface == MA::Vinyl)
    return MA::HardFlooring;
  if (surface == MA::Tiles)
    return MA::HardFlooring;
  if (surface == MA::CarpetedFloor)
    return MA::SoftFlooring;

  return MA::HardFlooring;
}


MAFloorSurface::MAFloorSurface() : Surface(-500, -500, 3000), CurrentSurface(-500, -500, 3000),
  OwnerFeedback(-500, -500, 3000)
{
}


MABodyMotion::MABodyMotion() : MARobotStateUpdater(MA::UpdateSensorIndicators),
  BodyState((int)MA::UnknownBodyState, (int)MA::UnknownBodyState, (int)MA::Poked),
  MotionControlAge(0, 0, 60),
  BackStrokedFactor(0, 0, 2000),
  LyingOnBackFactor(0, 0, 400),
  LyingOnBackLeftSideFactor(0, 0, 3000),
  LyingOnBackRightSideFactor(0, 0, 3000),
  BodyOnLeftSideFactor(0, 0, 3000),
  BodyOnRightSideFactor(0, 0, 3000),
  LyingFactor(0, 0, 400),
  LyingOnLeftSideFactor(0, 0, 3000),
  LyingOnRightSideFactor(0, 0, 3000),
  SittingFactor(0, 0, 800),
  FlyingFactor(0, 0, 3000),
  PokedFactor(0, 0, 600),
  MotionControlTimer(MC::NoExpiration)
{
  // Indicators
  BodyPitch.SetMinMax(-MA::StandardRangeMax, MA::StandardRangeMax);
  BodyRoll.SetMinMax(-MA::StandardRangeMax, MA::StandardRangeMax);
  UpsideDown.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LateralPosition.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  HeadStand.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LyingOnBack.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LyingOnBackLeftSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LyingOnBackRightSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BodyOnLeftSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  BodyOnRightSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Lying.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LyingOnLeftSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  LyingOnRightSide.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Sitting.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Flying.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
  Poked.SetMinMax(MA::StandardRangeMin, MA::StandardRangeMax);
}


bool MABodyMotion::IsBodyRollInWalkRange() const
{
  return BodyRoll >= -25 && BodyRoll <= 25;
}


void MABodyMotion::StartMotionControlTimer()
{
  MotionControlTimer.Start();
}


void MABodyMotion::StopMotionControlTimer()
{
  MotionControlTimer.Stop();
}


void MABodyMotion::UpdateRobotState(MARobotState& state)
{
  // Sensor indicators
  UpsideDown = (int)(((float)AccelZ.Degree+90) / 180.0*100.0);
  LateralPosition = (int)(((float)AccelY.Degree+90) / 180.0*100.0);
  HeadStand = (int)(((float)AccelX.Degree+90) / 180.0*100.0);
  InMotion = state.LegLF->InMotion > 0 || state.LegLH->InMotion > 0 || state.LegRF->InMotion > 0 ||
             state.LegRH->InMotion > 0 || state.Head->InMotion > 0;
  MotionControlAge = (MotionControlTimer.IsStarted() ? MotionControlTimer.GetElapsedTime() / 1000 : 0);

  GenerateLyingOnBack(state);
  GenerateLyingOnBackLeftSide(state);
  GenerateLyingOnBackRightSide(state);
  GenerateBodyOnLeftSide(state);
  GenerateBodyOnRightSide(state);
  GenerateLying(state);
  GenerateLyingOnLeftSide(state);
  GenerateLyingOnRightSide(state);
  GenerateSitting(state);
  GenerateFlying(state);
  GeneratePoked(state);
  // Note: The pitch and roll generation uses the smoothed accelerometer data
  GenerateBodyPitchAndRoll();
}


void MABodyMotion::GenerateBodyPitchAndRoll()
{
  float Alpha = atan2(-(float)AccelY.Raw / 10000.0, -(float)AccelZ.Raw / 10000.0);
  float Beta = atan2(AccelX.Raw, -((float)AccelY.Raw / 10000.0*sin(Alpha)+(float)AccelZ.Raw / 10000.0*cos(Alpha)));

  BodyRoll = (int)(atan(Alpha)*180.0 / boost::math::constants::pi<float>());
  BodyPitch = (int)(atan(Beta)*180.0 / boost::math::constants::pi<float>());
}


void MABodyMotion::GenerateLyingOnBack(MARobotState& state)
{
  if (UpsideDown >= 75 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      state.Torso->BackGrabbed == 0)
  {
    // Back lying "state" can be reached in 400 ms
    LyingOnBackFactor += state.Torso->CycleTime;
  } else {
    LyingOnBackFactor -= state.Torso->CycleTime*3;
  }
  LyingOnBack = LyingOnBackFactor.GetScaledValue();
}


void MABodyMotion::GenerateLyingOnBackLeftSide(MARobotState& state)
{
  if (UpsideDown >= 65 && LateralPosition < 25 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      state.Torso->BackGrabbed == 0)
  {
    // Back lying on left side "state" can be reached in 3000 ms
    LyingOnBackLeftSideFactor += state.Torso->CycleTime;
  } else {
    LyingOnBackLeftSideFactor -= state.Torso->CycleTime*3;
  }
  LyingOnBackLeftSide = LyingOnBackLeftSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateLyingOnBackRightSide(MARobotState& state)
{
  if (UpsideDown >= 65 && LateralPosition > 75 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      state.Torso->BackGrabbed == 0)
  {
    // Back lying on right side "state" can be reached in 3000 ms
    LyingOnBackRightSideFactor += state.Torso->CycleTime;
  } else {
    LyingOnBackRightSideFactor -= state.Torso->CycleTime*3;
  }
  LyingOnBackRightSide = LyingOnBackRightSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateBodyOnLeftSide(MARobotState& state)
{
  bool PawsFree = true;

  PawsFree = PawsFree && state.LegLF->Paw == 0;
  PawsFree = PawsFree && state.LegLH->Paw == 0;
  PawsFree = PawsFree && state.LegRF->Paw == 0;
  PawsFree = PawsFree && state.LegRH->Paw == 0;

  if (UpsideDown > 20 && UpsideDown < 95 && LateralPosition < 10 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      PawsFree && state.Torso->BackGrabbed == 0)
  {
    // Body on left side "state" can be reached in 3000 ms
    BodyOnLeftSideFactor += state.Torso->CycleTime;
  } else {
    BodyOnLeftSideFactor -= state.Torso->CycleTime*3;
  }
  BodyOnLeftSide = BodyOnLeftSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateBodyOnRightSide(MARobotState& state)
{
  bool PawsFree = true;

  PawsFree = PawsFree && state.LegLF->Paw == 0;
  PawsFree = PawsFree && state.LegLH->Paw == 0;
  PawsFree = PawsFree && state.LegRF->Paw == 0;
  PawsFree = PawsFree && state.LegRH->Paw == 0;

  if (UpsideDown > 20 && UpsideDown < 95 && LateralPosition > 90 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      PawsFree && state.Torso->BackGrabbed == 0)
  {
    // Body on right side "state" can be reached in 3000 ms
    BodyOnRightSideFactor += state.Torso->CycleTime;
  } else {
    BodyOnRightSideFactor -= state.Torso->CycleTime*3;
  }
  BodyOnRightSide = BodyOnRightSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateLying(MARobotState& state)
{
  bool Result = true;
  int LegsMovedToStand = 0;

  LegsMovedToStand += (state.LegLF->Moving && state.LegLF->UpDown < -80);
  LegsMovedToStand += (state.LegLH->Moving && state.LegLH->UpDown < -80);
  LegsMovedToStand += (state.LegRF->Moving && state.LegRF->UpDown < -80);
  LegsMovedToStand += (state.LegRH->Moving && state.LegRH->UpDown < -80);

  Result = Result && !state.BodyMotion->SkitPlayback;
  Result = Result && UpsideDown < 15 && HeadStand >= 8;
  Result = Result && Sitting <= 10;
  Result = Result && LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10;
  Result = Result && !state.ComplexIndicators->IsStandingPosture && LegsMovedToStand < 4;
  Result = Result && !state.ComplexIndicators->PoseTransition;
  Result = Result && !state.ComplexIndicators->IsLocomotionActive();
  if (Result)
  {
    // Lying "state" can be reached in 3000 msec
    LyingFactor += state.Torso->CycleTime;
  } else {
    LyingFactor -= state.Torso->CycleTime;
  }
  Lying = LyingFactor.GetScaledValue();
}


void MABodyMotion::GenerateLyingOnLeftSide(MARobotState& state)
{
  if (UpsideDown > 0 && UpsideDown < 15 && LateralPosition > 0 && LateralPosition < 15 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      state.Torso->BackGrabbed == 0)
  {
    // Lying on left side "state" can be reached in 3000 ms
    LyingOnLeftSideFactor += state.Torso->CycleTime;
  } else {
    LyingOnLeftSideFactor -= state.Torso->CycleTime*3;
  }
  LyingOnLeftSide = LyingOnLeftSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateLyingOnRightSide(MARobotState& state)
{
  if (UpsideDown > 0 && UpsideDown < 15 && LateralPosition > 80 && LateralPosition < 95 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      state.Torso->BackGrabbed == 0)
  {
    // Lying on right side "state" can be reached in 3000 ms
    LyingOnRightSideFactor += state.Torso->CycleTime;
  } else {
    LyingOnRightSideFactor -= state.Torso->CycleTime*3;
  }
  LyingOnRightSide = LyingOnRightSideFactor.GetScaledValue();
}


void MABodyMotion::GenerateSitting(MARobotState& state)
{
  if (UpsideDown < 15 && HeadStand >= 0 && HeadStand <= 12 &&
      LateralPosition.GetValueChange() < 5 && UpsideDown.GetValueChange() < 10 &&
      (state.LegLF->Paw == 1 || state.LegRF->Paw == 1))
  {
    // Sitting "state" can be reached in 800 msec
    SittingFactor += state.Torso->CycleTime;
  } else {
    SittingFactor -= state.Torso->CycleTime;
  }
  Sitting = SittingFactor.GetScaledValue();
}


void MABodyMotion::GenerateFlying(MARobotState& state)
{
  int Multiplier = state.Torso->BackGrabbed > 80 ? 3 : 1;
  bool PawsFree = true;

  PawsFree = PawsFree && state.LegLF->Paw == 0;
  PawsFree = PawsFree && state.LegLH->Paw == 0;
  PawsFree = PawsFree && state.LegRF->Paw == 0;
  PawsFree = PawsFree && state.LegRH->Paw == 0;

  if ((int)BodyState == MA::PickupMode && PawsFree)
  {
    // Flying "state" can be reached in 1-3 seconds and moderated to zero
    // after 3 seconds
    FlyingFactor += (int)state.Torso->CycleTime*Multiplier;
  } else {
    FlyingFactor -= (int)state.Torso->CycleTime;
  }
  Flying = FlyingFactor.GetScaledValue();
}


void MABodyMotion::GeneratePoked(MARobotState& state)
{
  // When any of the legs is moving, the robot does some locomotion or
  // self-movements of the body, it is not taken into account as "poked" state.
  if ((int)BodyState == MA::Poked && state.BodyMotion->Flying == 0 &&
      !(bool)state.LegLF->Moving && !(bool)state.LegLH->Moving &&
      !(bool)state.LegRF->Moving && !(bool)state.LegRH->Moving &&
      !state.ComplexIndicators->IsLocomotionActive() && UpsideDown < 20)
  {
    // Poked "state" can be reached in 600 milliseconds
    PokedFactor += state.Torso->CycleTime;
  } else {
    PokedFactor -= state.Torso->CycleTime*2;
  }
  Poked = PokedFactor.GetScaledValue();
}