<commit_before>/**
* @file Walk.cpp
*
* @author <a href="mailto:<EMAIL>"><NAME></a>
* @author <a href="mailto:<EMAIL>"><NAME></a>
*
*/

#include "Walk.h"
#include "Walk/ZMPPlanner.h"
#include "Walk/FootTrajectoryGenerator.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugBufferedOutput.h"

using namespace InverseKinematic;
using namespace naoth;

unsigned int Walk::theStepID = 0;

Walk::Walk()
  :IKMotion(getInverseKinematicsMotionEngineService(), motion::walk, getMotionLock()),
  theWalkParameters(getEngine().getParameters().walk),
  theWaitLandingCount(0),
  theUnsupportedCount(0),
  isStopping(false),
  stoppingStepFinished(false)
{
}
  
void Walk::execute()
{
  calculateError();

  {
    if ( !theWalkParameters.stabilization.enableWaitLanding || !waitLanding() )
    {
      plan(getMotionRequest());
      theCoMFeetPose = executeStep();
    }

    // plot planned trajectory
    PLOT("Walk:theCoMFeetPose:com:x",theCoMFeetPose.com.translation.x);
    PLOT("Walk:theCoMFeetPose:com:y",theCoMFeetPose.com.translation.y);
    PLOT("Walk:theCoMFeetPose:com:z",theCoMFeetPose.com.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.left:x",theCoMFeetPose.feet.left.translation.x);
    PLOT("Walk:theCoMFeetPose:feet.left:y",theCoMFeetPose.feet.left.translation.y);
    PLOT("Walk:theCoMFeetPose:feet.left:z",theCoMFeetPose.feet.left.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.right:x",theCoMFeetPose.feet.right.translation.x);
    PLOT("Walk:theCoMFeetPose:feet.right:y",theCoMFeetPose.feet.right.translation.y);
    PLOT("Walk:theCoMFeetPose:feet.right:z",theCoMFeetPose.feet.right.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.left:rotationX",theCoMFeetPose.feet.left.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:feet.left:rotationY",theCoMFeetPose.feet.left.rotation.getYAngle());

    PLOT("Walk:theCoMFeetPose:feet.right:rotationX",theCoMFeetPose.feet.right.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:feet.right:rotationY",theCoMFeetPose.feet.right.rotation.getYAngle());

    PLOT("Walk:theCoMFeetPose:com:rotationX",theCoMFeetPose.com.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:com:rotationY",theCoMFeetPose.com.rotation.getYAngle());
    PLOT("Walk:theCoMFeetPose:com:rotationZ",theCoMFeetPose.com.rotation.getZAngle());

    if(!stepBuffer.empty()) 
    {
      // HACK:
      CoMFeetPose tmp(theCoMFeetPose);
      if(stepBuffer.front().footStep.liftingFoot() == FootStep::LEFT) 
      {
        tmp.localInRightFoot();
        theCoMFeetPose.com.translation.z += theWalkParameters.hip.comHeightOffset * tmp.feet.left.translation.z;
        theCoMFeetPose.com.rotateX( theWalkParameters.hip.comRotationOffsetX *tmp.feet.left.translation.z );

        PLOT("Walk:theCoMFeetPose:total_rotationY",tmp.com.rotation.getYAngle());
      } 
      else if(stepBuffer.front().footStep.liftingFoot() == FootStep::RIGHT) 
      {
        tmp.localInLeftFoot();
        theCoMFeetPose.com.translation.z += theWalkParameters.hip.comHeightOffset * tmp.feet.right.translation.z;
        theCoMFeetPose.com.rotateX( -theWalkParameters.hip.comRotationOffsetX*tmp.feet.right.translation.z );

        PLOT("Walk:theCoMFeetPose:total_rotationY",tmp.com.rotation.getYAngle());
      }

      // buffer the pose
      commandFootIdBuffer.add(stepBuffer.front().footStep.liftingFoot());
      commandPoseBuffer.add(theCoMFeetPose);
    }

    bool solved = false;
    HipFeetPose c = getEngine().controlCenterOfMass(getMotorJointData(), theCoMFeetPose, solved, false);

    PLOT("Walk:com.solved", solved);
    PLOT("Walk:c:hip.z",c.hip.translation.z);

    PLOT("Walk:theCoMFeetPose:left_diff",c.feet.left.translation.y - c.hip.translation.y-50);
    PLOT("Walk:theCoMFeetPose:right_diff",c.feet.right.translation.y - c.hip.translation.y+50);


    // apply online stabilization
    if(getCalibrationData().calibrated && theWalkParameters.stabilization.rotationStabilize)
    {
      /*
      getEngine().rotationStabilize(
        getRobotInfo(),
        getGroundContactModel(),
        getInertialSensorData(),
        c.hip);*/
      
      if(!stepBuffer.empty()) 
      {
        if(stepBuffer.front().footStep.liftingFoot() == FootStep::LEFT) {
          c.localInRightFoot();
        } else if(stepBuffer.front().footStep.liftingFoot() == FootStep::RIGHT) {
          c.localInLeftFoot();
        } else {
          c.localInHip();
        }
      }
      else
      {
        c.localInLeftFoot(); // NOTE: the same as the stand
      }
      
      getEngine().rotationStabilize(<commit_msg>tests<commit_after>/**
* @file Walk.cpp
*
* @author <a href="mailto:<EMAIL>"><NAME></a>
* @author <a href="mailto:<EMAIL>"><NAME></a>
*
*/

#include "Walk.h"
#include "Walk/ZMPPlanner.h"
#include "Walk/FootTrajectoryGenerator.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugBufferedOutput.h"

using namespace InverseKinematic;
using namespace naoth;

unsigned int Walk::theStepID = 0;

Walk::Walk()
  :IKMotion(getInverseKinematicsMotionEngineService(), motion::walk, getMotionLock()),
  theWalkParameters(getEngine().getParameters().walk),
  theWaitLandingCount(0),
  theUnsupportedCount(0),
  isStopping(false),
  stoppingStepFinished(false)
{
}
  
void Walk::execute()
{
  calculateError();

  {
    if ( !theWalkParameters.stabilization.enableWaitLanding || !waitLanding() )
    {
      plan(getMotionRequest());
      theCoMFeetPose = executeStep();
    }

    // plot planned trajectory
    PLOT("Walk:theCoMFeetPose:com:x",theCoMFeetPose.com.translation.x);
    PLOT("Walk:theCoMFeetPose:com:y",theCoMFeetPose.com.translation.y);
    PLOT("Walk:theCoMFeetPose:com:z",theCoMFeetPose.com.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.left:x",theCoMFeetPose.feet.left.translation.x);
    PLOT("Walk:theCoMFeetPose:feet.left:y",theCoMFeetPose.feet.left.translation.y);
    PLOT("Walk:theCoMFeetPose:feet.left:z",theCoMFeetPose.feet.left.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.right:x",theCoMFeetPose.feet.right.translation.x);
    PLOT("Walk:theCoMFeetPose:feet.right:y",theCoMFeetPose.feet.right.translation.y);
    PLOT("Walk:theCoMFeetPose:feet.right:z",theCoMFeetPose.feet.right.translation.z);

    PLOT("Walk:theCoMFeetPose:feet.left:rotationX",theCoMFeetPose.feet.left.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:feet.left:rotationY",theCoMFeetPose.feet.left.rotation.getYAngle());

    PLOT("Walk:theCoMFeetPose:feet.right:rotationX",theCoMFeetPose.feet.right.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:feet.right:rotationY",theCoMFeetPose.feet.right.rotation.getYAngle());

    PLOT("Walk:theCoMFeetPose:com:rotationX",theCoMFeetPose.com.rotation.getXAngle());
    PLOT("Walk:theCoMFeetPose:com:rotationY",theCoMFeetPose.com.rotation.getYAngle());
    PLOT("Walk:theCoMFeetPose:com:rotationZ",theCoMFeetPose.com.rotation.getZAngle());

    if(!stepBuffer.empty()) 
    {
      // HACK:
      CoMFeetPose tmp(theCoMFeetPose);
      if(stepBuffer.front().footStep.liftingFoot() == FootStep::LEFT) 
      {
        tmp.localInRightFoot();
        theCoMFeetPose.com.translation.z += theWalkParameters.hip.comHeightOffset * tmp.feet.left.translation.z;
        theCoMFeetPose.com.rotateX( theWalkParameters.hip.comRotationOffsetX *tmp.feet.left.translation.z );

        PLOT("Walk:theCoMFeetPose:total_rotationY",tmp.com.rotation.getYAngle());
      } 
      else if(stepBuffer.front().footStep.liftingFoot() == FootStep::RIGHT) 
      {
        tmp.localInLeftFoot();
        theCoMFeetPose.com.translation.z += theWalkParameters.hip.comHeightOffset * tmp.feet.right.translation.z;
        theCoMFeetPose.com.rotateX( -theWalkParameters.hip.comRotationOffsetX*tmp.feet.right.translation.z );

        PLOT("Walk:theCoMFeetPose:total_rotationY",tmp.com.rotation.getYAngle());
      }

      // buffer the pose
      commandFootIdBuffer.add(stepBuffer.front().footStep.liftingFoot());
      commandPoseBuffer.add(theCoMFeetPose);
    }

    bool solved = false;
    HipFeetPose c = getEngine().controlCenterOfMass(getMotorJointData(), theCoMFeetPose, solved, false);

    PLOT("Walk:com.solved", solved);
    PLOT("Walk:c:hip.z",c.hip.translation.z);

    PLOT("Walk:theCoMFeetPose:left_diff",c.feet.left.translation.y - c.hip.translation.y-50);
    PLOT("Walk:theCoMFeetPose:right_diff",c.feet.right.translation.y - c.hip.translation.y+50);


    // apply online stabilization
    if(getCalibrationData().calibrated && theWalkParameters.stabilization.rotationStabilize)
    {
      /*
      getEngine().rotationStabilize(
        getRobotInfo(),
        getGroundContactModel(),
        getInertialSensorData(),
        c.hip);*/

      /*
      getEngine().rotationStabilize(
        getInertialModel(),
        getGyrometerData(),
        c.hip);
      */

      if(!stepBuffer.empty()) 
      {
        if(stepBuffer.front().footStep.liftingFoot() == FootStep::LEFT) {
          c.localInRightFoot();
        } else if(stepBuffer.front().footStep.liftingFoot() == FootStep::RIGHT) {
          c.localInLeftFoot();
        } else {
          c.localInHip();
        }
      }
      else
      {
        c.localInLeftFoot(); // NOTE: the same as the stand
      }
      
      getEngine().rotationStabilize(
{'fraction_non_alphanumeric': 0.1495559887372753, 'fraction_numerical': 0.0015161360190599957, 'mean_word_length': 4.147714604236343, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1466736', 'n_tokens_mistral': 3440, 'n_tokens_neox': 3317, 'n_words': 283}