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

#include "Walk2018.h"
#include "../Walk/FootTrajectoryGenerator.h"
#include "../Walk/ZMPPlanner.h"


#include "Tools/DataStructures/Spline.h"

using namespace naoth;
using namespace InverseKinematic;

Walk2018::Walk2018() : IKMotion(getInverseKinematicsMotionEngineService(), motion::walk, getMotionLock())
{
  DEBUG_REQUEST_REGISTER("Walk:draw_step_plan_geometry", "draw all planed steps, zmp and executed com", false);
  DEBUG_REQUEST_REGISTER("Walk:plot_genTrajectoryWithSplines", "plot spline interpolation to parametrize 3D foot trajectory", false);

  theCoMErrorProvider          = registerModule<CoMErrorProvider>("CoMErrorProvider", true);
  theFootStepPlanner           = registerModule<FootStepPlanner2018>("FootStepPlanner2018", true);   // TODO: call init()
  theZMPPlanner                = registerModule<ZMPPlanner2018>("ZMPPlanner2018", true);             // TODO: call init()
  theZMPPreviewController      = registerModule<ZMPPreviewController>("ZMPPreviewController", true);
  theFootTrajectoryGenerator   = registerModule<FootTrajectoryGenerator2018>("FootTrajectoryGenerator2018", true);
  theHipRotationOffsetModifier = registerModule<HipRotationOffsetModifier>("HipRotationOffsetModifier", true);
  theLiftingFootCompensator    = registerModule<LiftingFootCompensator>("LiftingFootCompensator", true);
  theTorsoRotationStabilizer   = registerModule<TorsoRotationStabilizer>("TorsoRotationStabilizer", true);
  theFeetStabilizer            = registerModule<FeetStabilizer>("FeetStabilizer", true);

  CoMFeetPose currentCOMFeetPose = getInverseKinematicsMotionEngineService().getEngine().getCurrentCoMFeetPose();
  currentCOMFeetPose.localInLeftFoot();
  Vector3d targetZMP(currentCOMFeetPose.com.translation);
  targetZMP.z = getInverseKinematicsMotionEngineService().getEngine().getParameters().walk.hip.comHeight;

  // init state of preview controller
  theZMPPreviewController->getModuleT()->init(currentCOMFeetPose.com.translation);
  int inital_number_of_cycles = static_cast<int>(theZMPPreviewController->getModuleT()->previewSteps());

  // init step buffer using the foot step planner
  theFootStepPlanner->getModuleT()->init(inital_number_of_cycles,currentCOMFeetPose.feet);

  // init zmp reference buffer using the zmp planner
  theZMPPlanner->getModuleT()->init(inital_number_of_cycles, currentCOMFeetPose.com.translation, targetZMP);

  std::cout << "walk start" << std::endl;
}
  
void Walk2018::execute()
{
  // check the integrity 
  getMotionRequest().walkRequest.assertValid();

  theCoMErrorProvider->execute();

  // planing phase
  // add new steps or delete executed ones if necessary
  theFootStepPlanner->execute();

  // NOTE: check the integrity of the step buffer
  ASSERT(!getStepBuffer().empty());
  ASSERT(!getStepBuffer().last().isPlanned());
  theZMPPlanner->execute();

  // running phase
  ASSERT(!getStepBuffer().first().isExecuted());
  calculateTargetCoMFeetPose();

  DEBUG_REQUEST("Walk:draw_step_plan_geometry",
    FIELD_DRAWING_CONTEXT;
    getStepBuffer().draw(getDebugDrawings());
  );

  // set arms
  // Attention: this will be overwritten by the arm motion engine if the ArmMotionRequest's MotionID is not equal to "none" or "arms_synchronised_with_walk"
  // NOTE: we set the arms before the calculation of the com, so  the motion of the com can be adjusted to the arms
  if(parameters().general.useArm) {
    getEngine().armsSynchronisedWithWalk(getRobotInfo(), getTargetCoMFeetPose().pose, getMotorJointData());
  }

  // calculate HipFeetPose which is getting close to the target CoMFeetPose
  bool solved = false;
  getTargetHipFeetPose().pose = getEngine().controlCenterOfMass(getMotorJointData(), getTargetCoMFeetPose().pose, solved, false);

  HipFeetPose cassert(getTargetHipFeetPose().pose);
  cassert.localInHip();
  ASSERT(std::abs(cassert.feet.left.translation.x)  < 150);
  ASSERT(std::abs(cassert.feet.left.translation.y)  < 200);
  ASSERT(std::abs(cassert.feet.left.translation.z)  < 350);
  ASSERT(std::abs(cassert.feet.right.translation.x) < 150);
  ASSERT(std::abs(cassert.feet.right.translation.y) < 200);
  ASSERT(std::abs(cassert.feet.right.translation.z) < 350);

  theTorsoRotationStabilizer->execute();

  calculateJoints();

  // STABILIZATION
  if(parameters().stabilization.stabilizeFeet) {
    theFeetStabilizer->execute();
  }

  updateMotionStatus(getMotionStatus());

  if(getMotionRequest().id != getId() && theZMPPreviewController->getModuleT()->is_stationary()) {
    // reset buffers
    getCoMErrors().reset();
    getCommandBuffer().reset();
    getStepBuffer().reset();
    getZMPReferenceBuffer().reset();

    theHipRotationOffsetModifier->getModuleT()->reset();

    setCurrentState(motion::stopped);
    std::cout << "walk stopped" << std::endl;
  } else {
    setCurrentState(motion::running);
  }
}//end execute

void Walk2018::calculateTargetCoMFeetPose()
{
  theZMPPreviewController->execute();

  Vector3d com = getTargetCoMFeetPose().pose.com.translation;

  DEBUG_REQUEST("Walk:draw_step_plan_geometry",
    FIELD_DRAWING_CONTEXT;
    getDebugDrawings().pen(Color::BLUE, 1.0);
    getDebugDrawings().fillOval(com.x, com.y, 10, 10);
  );

  theFootTrajectoryGenerator->execute();

  theHipRotationOffsetModifier->execute();

  theLiftingFootCompensator->execute();

  // buffer target CoM feet pose
  getCommandBuffer().footIds.add(getStepBuffer().first().footStep.liftingFoot());
  getCommandBuffer().poses.add(getTargetCoMFeetPose().pose);

  getStepBuffer().first().executingCycle++;
}

void Walk2018::calculateJoints() {
    getEngine().solveHipFeetIK(getTargetHipFeetPose().pose);
    getEngine().copyLegJoints(getMotorJointData().position);

    // set stiffness for the arms
    for (size_t i = JointData::RShoulderRoll; i <= JointData::LElbowYaw; ++i) {
      getMotorJointData().stiffness[i] = parameters().general.stiffnessArms;
    }
    getMotorJointData().stiffness[JointData::LWristYaw] = parameters().general.stiffnessArms;
    getMotorJointData().stiffness[JointData::RWristYaw] = parameters().general.stiffnessArms;


      // set the legs stiffness for walking
    for (size_t i = JointData::RHipYawPitch; i <= JointData::LAnkleRoll; ++i) {
      getMotorJointData().stiffness[i] = parameters().general.stiffness;
    }

    // <NAME>: force the hip joint
    if (getMotorJointData().position[JointData::LHipRoll] < 0) {
      getMotorJointData().position[JointData::LHipRoll] *= parameters().general.hipRollSingleSupFactorLeft; // if = 1 no damping or amplifing
    }
    if (getMotorJointData().position[JointData::RHipRoll] > 0) {
      getMotorJointData().position[JointData::RHipRoll] *= parameters().general.hipRollSingleSupFactorRight; // if = 1 no damping or amplifing
    }
}

void Walk2018::updateMotionStatus(MotionStatus& motionStatus) const
{
  if ( getStepBuffer().empty() )
  {
    motionStatus.plannedMotion.lFoot = Pose2D();
    motionStatus.plannedMotion.rFoot = Pose2D();
    motionStatus.plannedMotion.hip = Pose2D();
  }
  else
  {
    // TODO: check<commit_msg>small clean up in Walk2018<commit_after>/**
* @file Walk.cpp
*
* @author <a href="mailto:<EMAIL>-berlin.de"><NAME></a>
* @author <a href="mailto:mellmann@informatik.hu-berlin.de"><NAME></a>
* @author <a href="mailto:kaden@informatik.hu-berlin.de"><NAME></a>
*
*/

#include "Walk2018.h"
#include "../Walk/FootTrajectoryGenerator.h"
#include "../Walk/ZMPPlanner.h"


#include "Tools/DataStructures/Spline.h"

using namespace naoth;
using namespace InverseKinematic;

Walk2018::Walk2018() : IKMotion(getInverseKinematicsMotionEngineService(), motion::walk, getMotionLock())
{
  DEBUG_REQUEST_REGISTER("Walk:draw_step_plan_geometry", "draw all planed steps, zmp and executed com", false);
  DEBUG_REQUEST_REGISTER("Walk:plot_genTrajectoryWithSplines", "plot spline interpolation to parametrize 3D foot trajectory", false);

  theCoMErrorProvider          = registerModule<CoMErrorProvider>("CoMErrorProvider", true);
  theFootStepPlanner           = registerModule<FootStepPlanner2018>("FootStepPlanner2018", true);
  theZMPPlanner                = registerModule<ZMPPlanner2018>("ZMPPlanner2018", true);
  theZMPPreviewController      = registerModule<ZMPPreviewController>("ZMPPreviewController", true);
  theFootTrajectoryGenerator   = registerModule<FootTrajectoryGenerator2018>("FootTrajectoryGenerator2018", true);
  theHipRotationOffsetModifier = registerModule<HipRotationOffsetModifier>("HipRotationOffsetModifier", true);
  theLiftingFootCompensator    = registerModule<LiftingFootCompensator>("LiftingFootCompensator", true);
  theTorsoRotationStabilizer   = registerModule<TorsoRotationStabilizer>("TorsoRotationStabilizer", true);
  theFeetStabilizer            = registerModule<FeetStabilizer>("FeetStabilizer", true);

  CoMFeetPose currentCOMFeetPose = getInverseKinematicsMotionEngineService().getEngine().getCurrentCoMFeetPose();
  currentCOMFeetPose.localInLeftFoot();
  Vector3d targetZMP(currentCOMFeetPose.com.translation);
  targetZMP.z = getInverseKinematicsMotionEngineService().getEngine().getParameters().walk.hip.comHeight;

  // init state of preview controller
  theZMPPreviewController->getModuleT()->init(currentCOMFeetPose.com.translation);
  int inital_number_of_cycles = static_cast<int>(theZMPPreviewController->getModuleT()->previewSteps());

  // init step buffer using the foot step planner
  theFootStepPlanner->getModuleT()->init(inital_number_of_cycles,currentCOMFeetPose.feet);

  // init zmp reference buffer using the zmp planner
  theZMPPlanner->getModuleT()->init(inital_number_of_cycles, currentCOMFeetPose.com.translation, targetZMP);

  std::cout << "walk start" << std::endl;
}
  
void Walk2018::execute()
{
  // check the integrity 
  getMotionRequest().walkRequest.assertValid();

  theCoMErrorProvider->execute();

  // planing phase
  // add new steps or delete executed ones if necessary
  theFootStepPlanner->execute();

  DEBUG_REQUEST("Walk:draw_step_plan_geometry",
    FIELD_DRAWING_CONTEXT;
    getStepBuffer().draw(getDebugDrawings());
  );

  // NOTE: check the integrity of the step buffer
  ASSERT(!getStepBuffer().empty());
  ASSERT(!getStepBuffer().last().isPlanned());
  theZMPPlanner->execute();

  // running phase
  ASSERT(!getStepBuffer().first().isExecuted());
  calculateTargetCoMFeetPose();

  // set arms
  // Attention: this will be overwritten by the arm motion engine if the ArmMotionRequest's MotionID is not equal to "none" or "arms_synchronised_with_walk"
  // NOTE: we set the arms before the calculation of the com, so  the motion of the com can be adjusted to the arms
  if(parameters().general.useArm) {
    getEngine().armsSynchronisedWithWalk(getRobotInfo(), getTargetCoMFeetPose().pose, getMotorJointData());
  }

  // calculate HipFeetPose which is getting close to the target CoMFeetPose
  bool solved = false;
  getTargetHipFeetPose().pose = getEngine().controlCenterOfMass(getMotorJointData(), getTargetCoMFeetPose().pose, solved, false);

  HipFeetPose cassert(getTargetHipFeetPose().pose);
  cassert.localInHip();
  ASSERT(std::abs(cassert.feet.left.translation.x)  < 150);
  ASSERT(std::abs(cassert.feet.left.translation.y)  < 200);
  ASSERT(std::abs(cassert.feet.left.translation.z)  < 350);
  ASSERT(std::abs(cassert.feet.right.translation.x) < 150);
  ASSERT(std::abs(cassert.feet.right.translation.y) < 200);
  ASSERT(std::abs(cassert.feet.right.translation.z) < 350);

  theTorsoRotationStabilizer->execute();

  calculateJoints();

  updateMotionStatus(getMotionStatus());

  if(getMotionRequest().id != getId() && theZMPPreviewController->getModuleT()->is_stationary()) {
    // reset buffers
    getCoMErrors().reset();
    getCommandBuffer().reset();
    getStepBuffer().reset();
    getZMPReferenceBuffer().reset();

    theHipRotationOffsetModifier->getModuleT()->reset();

    setCurrentState(motion::stopped);
    std::cout << "walk stopped" << std::endl;
  } else {
    setCurrentState(motion::running);
  }
}//end execute

void Walk2018::calculateTargetCoMFeetPose()
{
  theZMPPreviewController->execute();

  Vector3d com = getTargetCoMFeetPose().pose.com.translation;

  DEBUG_REQUEST("Walk:draw_step_plan_geometry",
    FIELD_DRAWING_CONTEXT;
    getDebugDrawings().pen(Color::BLUE, 1.0);
    getDebugDrawings().fillOval(com.x, com.y, 10, 10);
  );

  theFootTrajectoryGenerator->execute();

  theHipRotationOffsetModifier->execute();

  theLiftingFootCompensator->execute();

  // buffer target CoM feet pose
  getCommandBuffer().footIds.add(getStepBuffer().first().footStep.liftingFoot());
  getCommandBuffer().poses.add(getTargetCoMFeetPose().pose);

  getStepBuffer().first().executingCycle++;
}

void Walk2018::calculateJoints() {
    getEngine().solveHipFeetIK(getTargetHipFeetPose().pose);
    getEngine().copyLegJoints(getMotorJointData().position);

    // set stiffness for the arms
    for (size_t i = JointData::RShoulderRoll; i <= JointData::LElbowYaw; ++i) {
      getMotorJointData().stiffness[i] = parameters().general.stiffnessArms;
    }
    getMotorJointData().stiffness[JointData::LWristYaw] = parameters().general.stiffnessArms;
    getMotorJointData().stiffness[JointData::RWristYaw] = parameters().general.stiffnessArms;

    // set the legs stiffness for walking
    for (size_t i = JointData::RHipYawPitch; i <= JointData::LAnkleRoll; ++i) {
      getMotorJointData().stiffness[i] = parameters().general.stiffness;
    }

    // WIEDERLICHER HACK: force the hip joint
    if (getMotorJointData().position[JointData::LHipRoll] < 0) {
      getMotorJointData().position[JointData::LHipRoll] *= parameters().general.hipRollSingleSupFactorLeft; // if = 1 no damping or amplifing
    }
    if (getMotorJointData().position[JointData::RHipRoll] > 0) {
      getMotorJointData().position[JointData::RHipRoll] *= parameters().general.hipRollSingleSupFactorRight; // if = 1 no damping or amplifing
    }

    // STABILIZATION
    theFeetStabilizer->execute();
}

void Walk2018::updateMotionStatus(MotionStatus& motionStatus) const
{
  if ( getStepBuffer().empty() )
  {
    motionStatus.plannedMotion.lFoot = Pose2D();
    motionStatus.plannedMotion.rFoot = Pose2D();
    motionStatus.plannedMotion.hip = Pose2D();
  }
  else
  {
    // TODO: check
{'fraction_non_alphanumeric': 0.13255441963975242, 'fraction_numerical': 0.012240072327700118, 'mean_word_length': 5.465827338129497, 'pattern_counts': {'":': 0, '<': 80, '<?xml version=': 0, '>': 90, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 2, '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': '21470', 'n_tokens_mistral': 4892, 'n_tokens_neox': 4542, 'n_words': 743}