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

#include "core/MADevice.hpp"
#include "core/MAValueGenerators.hpp"
#include "MASkitDatabase.hpp"

#include <MCContainers.hpp>
#include <MCThreadLocalData.hpp>

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

namespace
{
// Joint name list
MCThreadLocalData<MC::StringList> JointNames(true);

void CheckStaticMtnParserVariables()
{
  if (unlikely(!JointNames.get()))
  {
    JointNames.reset(new MC::StringList);
  }
}
}

MAMotionSkit::MAMotionSkit() : MASkitBase(), SkitDuration(0)
{
}


std::string MAMotionSkit::GetName() const
{
  return Name;
}


std::string MAMotionSkit::GetSkitName() const
{
  MC::StringList Strs;
  std::string TempStr;

  boost::split(Strs, Name, boost::is_any_of("#"));
  TempStr = Strs[1];
  boost::split(Strs, TempStr, boost::is_any_of("_"));
  Strs.erase(Strs.begin());
  TempStr = boost::algorithm::join(Strs, "_");
  return TempStr;
}


int MAMotionSkit::GetDuration() const
{
  return SkitDuration;
}


MA::MotionSkitSPtr MAMotionSkit::CloneMirrored() const
{
  MA::MotionSkitSPtr NewMotionSkit(new MAMotionSkit);

  *NewMotionSkit = *this;
  // Swap LegLF<->LegRF
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 7, 13);
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 8, 14);
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 9, 15);
  // Swap LegLH<->LegRH
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 10, 16);
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 11, 17);
  MCSwapTableColumns<int, MC::IntTable>(NewMotionSkit->Keyframes, 12, 18);
  return NewMotionSkit;
}


void MAMotionSkit::Dump() const
{
  MC_LOG("-- %s motion keyframes --", Name.c_str());
  if (IsStartingPostureSit())
  {
    MC_LOG("Starting posture: SIT");
  }
  if (IsStartingPostureLie())
  {
    MC_LOG("Starting posture: LIE");
  }
  if (IsStartingPostureStand())
  {
    MC_LOG("Starting posture: STAND");
  }
  for (unsigned int i = 0; i < Keyframes.size(); ++i)
  {
    std::string Message;
    int NeckTilt1 = 0;

    Message.append(">> Time: ");
    Message.append(MCToStr(Keyframes[i][0]));
    Message.append(" ms | ");
    for (unsigned int i1 = 1; i1 < Keyframes[i].size(); ++i1)
    {
      Message.append((*JointNames)[i1-1]+": ");
      if ((*JointNames)[i1-1].find("Leg") != std::string::npos)
      {
        if ((*JointNames)[i1-1].find("1") != std::string::npos)
        {
          MALegPosition Position;

          // The LegLH1/LegRH1 joint degree values are inverted for convenience.
          // See also APDevice.cpp
          if ((*JointNames)[i1-1] == "LegLH1" || (*JointNames)[i1-1] == "LegRH1")
          {
            Position.FromJointDegree(-Keyframes[i][i1]);
          } else {
            Position.FromJointDegree(Keyframes[i][i1]);
          }
          Message.append("FB "+MCToStr(MCToStr((int)Position.ForeBackValue))+',');
          Message.append("UD "+MCToStr(MCToStr((int)Position.UpDownValue))+',');
          Message.append("D "+MCToStr(Keyframes[i][i1]));
        }
        if ((*JointNames)[i1-1].find("2") != std::string::npos)
        {
          MASidelong Sidelong;

          Sidelong.FromJointDegree(Keyframes[i][i1]);
          Message.append("SL "+MCToStr(MCToStr((int)Sidelong.Value))+',');
          Message.append("D "+MCToStr(Keyframes[i][i1]));
        }
        if ((*JointNames)[i1-1].find("3") != std::string::npos)
        {
          MABentStretched BentStretched;

          BentStretched.FromJointDegree(Keyframes[i][i1]);
          Message.append("BS "+MCToStr(MCToStr((int)BentStretched.Value))+',');
          Message.append("D "+MCToStr(Keyframes[i][i1]));
        }
      } else
      if ((*JointNames)[i1-1].find("Neck") != std::string::npos)
      {
        if ((*JointNames)[i1-1].find("NeckTilt1") != std::string::npos)
        {
          NeckTilt1 = Keyframes[i][i1];
          Message.append(MCToStr(Keyframes[i][i1]));
        } else
        if ((*JointNames)[i1-1].find("NeckTilt2") != std::string::npos)
        {
          MAHeadPosition Position;

          Position.FromJointDegrees(NeckTilt1, Keyframes[i][i1]);
          Message.append("FB "+MCToStr(MCToStr((int)Position.ForeBackValue))+',');
          Message.append("UD "+MCToStr(MCToStr((int)Position.UpDownValue))+',');
          Message.append("D "+MCToStr(Keyframes[i][i1]));
        } else
        if ((*JointNames)[i1-1].find("NeckPan") != std::string::npos)
        {
          MALeftRightTurned LeftRightTurned;

          LeftRightTurned.FromJointDegree(Keyframes[i][i1]);
          Message.append("LR "+MCToStr(MCToStr((int)LeftRightTurned.Value))+',');
          Message.append("D "+MCToStr(Keyframes[i][i1]));
        }
      } else
      if ((*JointNames)[i1-1] == "Mouth")
      {
        MAMouthOpened MouthOpened;

        MouthOpened.FromJointDegree(Keyframes[i][i1]);
        Message.append("OC "+MCToStr(MCToStr((int)MouthOpened.Value))+',');
        Message.append("D "+MCToStr(Keyframes[i][i1]));
      } else {
        Message.append(MCToStr(Keyframes[i][i1]));
      }
      if (i1 < Keyframes[i].size()-1)
        Message.append(" | ");
    }
    MC_LOG("%s", Message.c_str());
  }
  MC_LOG("Motion duration: %d msec", GetDuration());
  if (IsFinishingPostureSit())
  {
    MC_LOG("Finishing posture: SIT");
  } else
  if (IsFinishingPostureLie())
  {
    MC_LOG("Finishing posture: LIE");
  } else
  if (IsFinishingPostureStand())
  {
    MC_LOG("Finishing posture: STAND");
  } else {
    MC_LOG("Finishing posture: N/A");
  }
}


bool MAMotionSkit::IsStartingPostureSit() const
{
  return IsPostureSit(0);
}


bool MAMotionSkit::IsFinishingPostureSit() const
{
  return IsPostureSit(Keyframes.size()-1);
}


bool MAMotionSkit::IsStartingPostureLie() const
{
  return IsPostureLie(0);
}


bool MAMotionSkit::IsFinishingPostureLie() const
{
  return IsPostureLie(Keyframes.size()-1);
}


bool MAMotionSkit::IsStartingPostureStand() const
{
  return IsPostureStand(0);
}


bool MAMotionSkit::IsFinishingPostureStand() const
{
  return IsPostureStand(Keyframes.size()-1);
}


MAMotionSkit* MAMotionSkit::DecodeMtnData(const MCBinaryData& binary_data)
{
  MCBinaryData& BinaryData = const_cast<MCBinaryData&>(binary_data);
  int OldPosition = BinaryData.GetPosition();
  std::string TempStr;
  int TempInt = 0;
  int BlockSize = 0;

  BinaryData.SetPosition(0);
  // Test the file signature
  if (BinaryData.GetString(4) != "OMTN")
  {
    MC_WARNING("Wrong file signature, can't decode motion sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  /*
   * Block 0
   */
  // Skip the block number (block 0)
  BinaryData.GetInt32(true);
  // Skip the block size (block 0)
  BinaryData.GetInt32(true);
  // Skip the number of blocks
  BinaryData.GetInt32(true);
  // Skip the major version number
  BinaryData.GetInt16(true);
  // Skip the minor version number
  BinaryData.GetInt16(true);
  int KeyframeCount = BinaryData.GetInt16(true);
  int FrameRate = BinaryData.GetInt16(true);

  // Skip the options
  BinaryData.GetInt32(true);
  /*
   * Block 1
   */
  // Skip the block number (block 1)
  BinaryData.GetInt32(true);
  // Read the block size (block 1)
  BlockSize = BinaryData.GetInt32(true);
  BlockSize -= 8;
  if (KeyframeCount == 0 || FrameRate == 0 || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Empty motion data while decoding motion sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Read the motion (chunk) name
  TempInt = (int)BinaryData.GetUChar();
  BlockSize -= 1;
  TempStr = BinaryData.GetString(TempInt, "_#-:|");
  BlockSize -= TempInt;
  if (TempStr.empty() || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Can't read motion name (chunk) while decoding motion sequence");
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  MAMotionSkit* MotionSequence = new MAMotionSkit;

  MotionSequence->Name = MA::SkitNamePrefix+TempStr;
  // Read the creator
  TempInt = (int)BinaryData.GetUChar();
  BlockSize -= 1;
  TempStr = BinaryData.GetString(TempInt, "_#-:|");
  BlockSize -= TempInt;
  if (TempStr.empty() || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Can't read the creator while decoding motion sequence");
    delete MotionSequence;
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  // Read the platform
  TempInt = (int)BinaryData.GetUChar();
  BlockSize -= 1;
  TempStr = BinaryData.GetString(TempInt, "-");
  BlockSize -= TempInt;
  if (TempStr != "DRX-1000" || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Incompatible hardware platform, it is not ERS-7 (DRX-1000 != %s)", TempStr.c_str());
    delete MotionSequence;
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  if (BlockSize < 0 || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Can't decode the block 1 successfully while decoding motion sequence");
    delete MotionSequence;
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  if (BlockSize > 0)
    BinaryData.IncrementPosition(BlockSize);
  /*
   * Block 2
   */
  // Skip the block number (block 2)
  BinaryData.GetInt32(true);
  // Read the block size (block 2)
  BlockSize = BinaryData.GetInt32(true);
  BlockSize -= 8;
  // Read the number of joints
  int JointCount = BinaryData.GetInt16(true);

  BlockSize -= 2;
  CheckStaticMtnParserVariables();
  bool JointNamesReady = !JointNames->empty();

  // Read the joint names
  for (int i = 0; i < JointCount; ++i)
  {
    TempInt = (int)BinaryData.GetUChar();
    BlockSize -= 1;
    TempStr = BinaryData.GetString(TempInt, ":/-");
    BlockSize -= TempInt;
    if (TempStr.empty() || BinaryData.IsPositionAtEnd())
    {
      MC_WARNING("Can't decode the joint names while decoding motion sequence");
      delete MotionSequence;
      BinaryData.SetPosition(OldPosition);
      if (!JointNamesReady)
      {
        JointNames->clear();
      }
      return nullptr;
    }
    if (!JointNamesReady)
    {
      JointNames->push_back(MADevice::GetMotorDisplayName(TempStr));
    }
  }
  if (BlockSize < 0 || BinaryData.IsPositionAtEnd())
  {
    MC_WARNING("Can't decode the block 2 successfully while decoding motion sequence");
    delete MotionSequence;
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  if (BlockSize > 0)
    BinaryData.IncrementPosition(BlockSize);
  /*
   * Block 3
   */
  // Skip the block number (block 3)
  BinaryData.GetInt32(true);
  // Read the block size (block 3)
  BlockSize = BinaryData.GetInt32(true);
  BlockSize -= 8;
  for (int i = 0; i < KeyframeCount; ++i)
  {
    MC::IntList KeyframeValues;

    // Read the time delta
    TempInt = BinaryData.GetInt32(true);
    MotionSequence->SkitDuration += TempInt*FrameRate;
    KeyframeValues.push_back(TempInt*FrameRate);
    // Skip three dummy words
    BinaryData.GetInt32(true);
    BinaryData.GetInt32(true);
    BinaryData.GetInt32(true);
    BlockSize -= 16;
    for (int i1 = 0; i1 < JointCount; ++i1)
    {
      float TempFloat = (float)BinaryData.GetInt32(true);

      BlockSize -= 4;
      KeyframeValues.push_back((int)roundf(TempFloat*180.0 /
                                           (1000000.0*boost::math::constants::pi<float>())));
    }
    if (BlockSize < 0 || (BlockSize != 0 && BinaryData.IsPositionAtEnd()))
    {
      MC_WARNING("Can't decode the key frame %d while decoding motion sequence", i);
      delete MotionSequence;
      BinaryData.SetPosition(OldPosition);
      return nullptr;
    }
    // Skip duplicate frames
    if (i > 0 && KeyframeValues[0] == 0)
    {
//      MC_WARNING("Skip duplicate key frame %d", i);
      continue;
    }
    MotionSequence->Keyframes.push_back(KeyframeValues);
  }
  if (BlockSize > 0)
    BinaryData.IncrementPosition(BlockSize);

  BinaryData.SetPosition(OldPosition);
  MC_LOG("Decoded motion sequence: %s", MotionSequence->Name.c_str());
  return MotionSequence;
}


MAGeneratorContainer* MAMotionSkit::GetGeneratorContainer(MA::Leg::LegTypes leg,
                                                          MA::LegJointsType joint) const
{
  if (leg == MA::Leg::LF)
  {
    if (joint == MA::LegJoint1)
      return GetGeneratorContainer(7);
    if (joint == MA::LegJoint2)
      return GetGeneratorContainer(8);
    if (joint == MA::LegJoint3)
      return GetGeneratorContainer(9);
  }
  if (leg == MA::Leg::LH)
  {
    if (joint == MA::LegJoint1)
      return GetGeneratorContainer(10, true);
    if (joint == MA::LegJoint2)
      return GetGeneratorContainer(11);
    if (joint == MA::LegJoint3)
      return GetGeneratorContainer(12);
  }
  if (leg == MA::Leg::RF)
  {
    if (joint == MA::LegJoint1)
      return GetGeneratorContainer(13);
    if (joint == MA::LegJoint2)
      return GetGeneratorContainer(14);
    if (joint == MA::LegJoint3)
      return GetGeneratorContainer(15);
  }
  if (leg == MA::Leg::RH)
  {
    if (joint == MA::LegJoint1)
      return GetGeneratorContainer(16, true);
    if (joint == MA::LegJoint2)
      return GetGeneratorContainer(17);
    if (joint == MA::LegJoint3)
      return GetGeneratorContainer(18);
  }
  return nullptr;
}


MAGeneratorContainer* MAMotionSkit::GetGeneratorContainer(MA::TailJointsType tail_joint) const
{
  if (tail_joint == MA::TailTilt)
  {
    return GetGeneratorContainer(19);
  }
  return GetGeneratorContainer(20);
}


MAGeneratorContainer* MAMotionSkit::GetGeneratorContainer(MA::HeadJointsType head_joint) const
{
  if (head_joint == MA::NeckTilt1)
    return GetGeneratorContainer(1);
  if (head_joint == MA::NeckPan)
    return GetGeneratorContainer(2);
  if (head_joint == MA::NeckTilt2)
    return GetGeneratorContainer(3);
  if (head_joint == MA::MouthJoint)
    return GetGeneratorContainer(4);
  if (head_joint == MA::EarLeft)
  {
    return GetGeneratorContainer(5);
  }
  return GetGeneratorContainer(6);
}


bool MAMotionSkit::IsPostureSit(unsigned int keyframe_index) const
{
  if (keyframe_index > Keyframes.size()-1)
  {
    MC_WARNING("Invalid keyframe for sit posture check (%d > %d)", keyframe_index,
               Keyframes.size()-1);
    return false;
  }
  bool Result = true;

  // LegLF1
  Result = Result && Keyframes[keyframe_index][7] == -25;
  // LegLF2
  Result = Result && Keyframes[keyframe_index][8] == -4;
  // LegLF3
  Result = Result && Keyframes[keyframe_index][9] == 20;
  // LegLH1
  Result = Result && Keyframes[keyframe_index][10] == -78;
  // LegLH2
  Result = Result && Keyframes[keyframe_index][11] == 20;
  // LegLH3
  Result = Result && Keyframes[keyframe_index][12] == 110;
  // LegRF1
  Result = Result && Keyframes[keyframe_index][13] == -25;
  // LegRF2
  Result = Result && Keyframes[keyframe_index][14] == -4;
  // LegRF3
  Result = Result && Keyframes[keyframe_index][15] == 20;
  // LegRH1
  Result = Result && Keyframes[keyframe_index][16] == -78;
  // LegRH2
  Result = Result && Keyframes[keyframe_index][17] == 20;
  // LegRH3
  Result = Result && Keyframes[keyframe_index][18] == 110;
  return Result;
}


bool MAMotionSkit::IsPostureLie(unsigned int keyframe_index) const
{
  if (keyframe_index > Keyframes.size()-1)
  {
    MC_WARNING("Invalid keyframe for lie posture check (%d > %d)", keyframe_index,
               Keyframes.size()-1);
    return false;
  }
  bool Result = true;

  // LegLF1
  Result = Result && (Keyframes[keyframe_index][7] >= 59 && Keyframes[keyframe_index][7] <= 69);
  // LegLF2
  Result = Result && Keyframes[keyframe_index][8] == 0;
  // LegLF3
  Result = Result && Keyframes[keyframe_index][9] == 30;
  // LegLH1
  Result = Result && (Keyframes[keyframe_index][10] >= -129 && Keyframes[keyframe_index][10] <= -119);
  // LegLH2
  Result = Result && Keyframes[keyframe_index][11] == 4;
  // LegLH3
  Result = Result && Keyframes[keyframe_index][12] == 122;
  // LegRF1
  Result = Result && (Keyframes[keyframe_index][13] >= 59 && Keyframes[keyframe_index][13] <= 69);
  // LegRF2
  Result = Result && Keyframes[keyframe_index][14] == 0;
  // LegRF3
  Result = Result && Keyframes[keyframe_index][15] == 30;
  // LegRH1
  Result = Result && (Keyframes[keyframe_index][16] >= -129 && Keyframes[keyframe_index][16] <= -119);
  // LegRH2
  Result = Result && Keyframes[keyframe_index][17] == 4;
  // LegRH3
  Result = Result && Keyframes[keyframe_index][18] == 122;
  return Result;
}


bool MAMotionSkit::IsPostureStand(unsigned int keyframe_index) const
{
  if (keyframe_index > Keyframes.size()-1)
  {
    MC_WARNING("Invalid keyframe for stand posture check (%d > %d)", keyframe_index,
               Keyframes.size()-1);
    return false;
  }
  bool Result = true;

  // LegLF1
  Result = Result && Keyframes[keyframe_index][7] == -5;
  // LegLF2
  Result = Result && Keyframes[keyframe_index][8] == 3;
  // LegLF3
  Result = Result && Keyframes[keyframe_index][9] == 30;
  // LegLH1
  Result = Result && Keyframes[keyframe_index][10] == -5;
  // LegLH2
  Result = Result && Keyframes[keyframe_index][11] == 3;
  // LegLH3
  Result = Result && Keyframes[keyframe_index][12] == 30;
  // LegRF1
  Result = Result && Keyframes[keyframe_index][13] == -5;
  // LegRF2
  Result = Result && Keyframes[keyframe_index][14] == 3;
  // LegRF3
  Result = Result && Keyframes[keyframe_index][15] == 30;
  // LegRH1
  Result = Result && Keyframes[keyframe_index][16] == -5;
  // LegRH2
  Result = Result && Keyframes[keyframe_index][17] == 3;
  // LegRH3
  Result = Result && Keyframes[keyframe_index][18] == 30;
  return Result;
}


MAGeneratorContainer* MAMotionSkit::GetGeneratorContainer(unsigned int column_index,
                                                          bool invert_value) const
{
  if (column_index == 0 || column_index > Keyframes[0].size()-1)
  {
    MC_WARNING("Invalid column index to get a generator container (0 < %d <= %d?)",
               column_index, Keyframes[0].size()-1);
    return nullptr;
  }
  MAGeneratorContainer* Container = nullptr;

  for (unsigned int i = 1; i < Keyframes.size(); ++i)
  {
    MASimpleGenerator* Generator = nullptr;

    if (unlikely(invert_value))
    {
      Generator = new MASimpleGenerator(-(float)Keyframes[i][column_index], Keyframes[i][0]);
    } else {
      Generator = new MASimpleGenerator((float)Keyframes[i][column_index], Keyframes[i][0]);
    }
    if (i == 1)
    {
      Container = new MAGeneratorContainer(*Generator);
    } else {
      Container->AddGenerator(*Generator);
    }
  }
  return Container;
}