<commit_before>/*
 * StepCompleter.hpp
 *
 *  Created on: Mar 7, 2015
 *      Author: <NAME>
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "free_gait_core/step/StepCompleter.hpp"
#include "free_gait_core/leg_motion/leg_motion.hpp"

namespace free_gait {

StepCompleter::StepCompleter(std::shared_ptr<StepParameters> parameters,
                             std::shared_ptr<AdapterBase> adapter)
    : parameters_(parameters),
      adapter_(adapter)
{
}

StepCompleter::~StepCompleter()
{
}

bool StepCompleter::complete(const State& state, const StepQueue& queue, Step& step) const
{
  for (auto& legMotion : step.legMotions_) {
    switch ((legMotion.second)->getType()) {
      case LegMotionBase::Type::Footstep:
        setParameters(dynamic_cast<Footstep&>(*legMotion.second));
        break;
      case LegMotionBase::Type::LegMode:
        setParameters(dynamic_cast<LegMode&>(*legMotion.second));
        break;
      default:
        break;
    }
    switch (legMotion.second->getTrajectoryType()) {
      case LegMotionBase::TrajectoryType::EndEffector:
        if (!complete(state, step, dynamic_cast<EndEffectorMotionBase&>(*legMotion.second))) return false;
        break;
      case LegMotionBase::TrajectoryType::Joints:
        if (!complete(state, step, dynamic_cast<JointMotionBase&>(*legMotion.second))) return false;
        break;
      default:
        throw std::runtime_error("StepCompleter::complete() could not complete leg motion of this type.");
        break;
    }
  }

  if (step.baseMotion_) {
    switch (step.baseMotion_->getType()) {
      case BaseMotionBase::Type::Auto:
        setParameters(dynamic_cast<BaseAuto&>(*step.baseMotion_));
        break;
      case BaseMotionBase::Type::Trajectory:
        setParameters(dynamic_cast<BaseTrajectory&>(*step.baseMotion_));
        break;
      default:
        break;
    }
    if (!complete(state, step, queue, *(step.baseMotion_))) return false;
  }

  step.isComplete_ = true;
  return step.update();
}

bool StepCompleter::complete(const State& state, const Step& step, EndEffectorMotionBase& endEffectorMotion) const
{
  if (endEffectorMotion.getControlSetup().at(ControlLevel::Position)) {
    // TODO Check frame.
    Position startPositionInBaseFrame = adapter_->getPositionBaseToFootInBaseFrame(endEffectorMotion.getLimb(), state.getJointPositions(endEffectorMotion.getLimb()));
    Position startPositionInWorldFrame = adapter_->getPositionWorldToBaseInWorldFrame() + adapter_->getOrientationWorldToBase().inverseRotate(startPositionInBaseFrame);
    Position startPositionInDesiredFrame = startPositionInWorldFrame; // TODO
    endEffectorMotion.updateStartPosition(startPositionInDesiredFrame);
  }
  return endEffectorMotion.compute(state, step, *adapter_);
}

bool StepCompleter::complete(const State& state, const Step& step, JointMotionBase& jointMotion) const
{
  // Input.
  bool positionIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Position);
  bool velocityIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Velocity);
  bool accelerationIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Acceleration);
  bool effortIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Effort);

  // Output.
  bool positionOut = jointMotion.getControlSetup().at(free_gait::ControlLevel::Position);
  bool velocityOut = jointMotion.getControlSetup().at(free_gait::ControlLevel::Velocity);
  bool accelerationOut = jointMotion.getControlSetup().at(free_gait::ControlLevel::Acceleration);
  bool effortOut = jointMotion.getControlSetup().at(free_gait::ControlLevel::Effort);

  // Check for special mode transitions.
  if (positionIn && !effortIn && positionOut && effortOut) {
    JointPositions startPosition = state.getJointPositions(jointMotion.getLimb());
    jointMotion.updateStartPosition(startPosition);
    JointEfforts startEffort = state.getJointEfforts(jointMotion.getLimb());
    startEffort.setZero();
    jointMotion.updateStartEfforts(startEffort);
  } else if (positionIn && effortIn && !positionOut && effortOut) {
    JointEfforts startEffort = adapter_->getJointEfforts(jointMotion.getLimb());
    jointMotion.updateStartEfforts(startEffort);
  } //else if (positionIn && effortIn && positionOut && !effortOut) {
    // TODO: Decrease torque to zero in special step.
  //} else if (!positionIn && effortIn && positionOut && !effortOut) {
    // TODO: Decrease compression on leg in special step.
  // }
  else {

    // Standard transitions.
    if (jointMotion.getControlSetup().at(ControlLevel::Position)) {
      JointPositions startPosition = state.getJointPositions(jointMotion.getLimb());
      jointMotion.updateStartPosition(startPosition);
    }
    if (jointMotion.getControlSetup().at(ControlLevel::Velocity)) {
      JointVelocities startVelocity = state.getJointVelocities(jointMotion.getLimb());
      jointMotion.updateStartVelocity(startVelocity);
    }
    if (jointMotion.getControlSetup().at(ControlLevel::Acceleration)) {
      JointAccelerations startAcceleration = state.getJointAccelerations(jointMotion.getLimb());
      jointMotion.updateStartAcceleration(startAcceleration);
    }
    if (jointMotion.getControlSetup().at(ControlLevel::Effort)) {
      JointEfforts startEffort = state.getJointEfforts(jointMotion.getLimb());
      jointMotion.updateStartEfforts(startEffort);
    }

  }
  return jointMotion.compute(state, step, *adapter_);
}

bool StepCompleter::complete(const State& state, const Step& step, const StepQueue& queue, BaseMotionBase& baseMotion) const
{
  if (baseMotion.getControlSetup().at(ControlLevel::Position)) {
    // TODO Check frame.
    Pose pose(state.getPositionWorldToBaseInWorldFrame(), state.getOrientationWorldToBase());
    baseMotion.updateStartPose(pose);
  }
  if (baseMotion.getControlSetup().at(ControlLevel::Velocity)) {
    // TODO
  }
  return baseMotion.compute(state, step, queue, *adapter_);<commit_msg>Working on improved contact switches at lift-off and touch down. Some cleanups.<commit_after>/*
 * StepCompleter.hpp
 *
 *  Created on: Mar 7, 2015
 *      Author: <NAME>
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "free_gait_core/step/StepCompleter.hpp"
#include "free_gait_core/leg_motion/leg_motion.hpp"

namespace free_gait {

StepCompleter::StepCompleter(std::shared_ptr<StepParameters> parameters,
                             std::shared_ptr<AdapterBase> adapter)
    : parameters_(parameters),
      adapter_(adapter)
{
}

StepCompleter::~StepCompleter()
{
}

bool StepCompleter::complete(const State& state, const StepQueue& queue, Step& step) const
{
  for (auto& legMotion : step.legMotions_) {
    switch ((legMotion.second)->getType()) {
      case LegMotionBase::Type::Footstep:
        setParameters(dynamic_cast<Footstep&>(*legMotion.second));
        break;
      case LegMotionBase::Type::LegMode:
        setParameters(dynamic_cast<LegMode&>(*legMotion.second));
        break;
      default:
        break;
    }
    switch (legMotion.second->getTrajectoryType()) {
      case LegMotionBase::TrajectoryType::EndEffector:
        if (!complete(state, step, dynamic_cast<EndEffectorMotionBase&>(*legMotion.second))) return false;
        break;
      case LegMotionBase::TrajectoryType::Joints:
        if (!complete(state, step, dynamic_cast<JointMotionBase&>(*legMotion.second))) return false;
        break;
      default:
        throw std::runtime_error("StepCompleter::complete() could not complete leg motion of this type.");
        break;
    }
  }

  if (step.baseMotion_) {
    switch (step.baseMotion_->getType()) {
      case BaseMotionBase::Type::Auto:
        setParameters(dynamic_cast<BaseAuto&>(*step.baseMotion_));
        break;
      case BaseMotionBase::Type::Trajectory:
        setParameters(dynamic_cast<BaseTrajectory&>(*step.baseMotion_));
        break;
      default:
        break;
    }
    if (!complete(state, step, queue, *(step.baseMotion_))) return false;
  }

  step.isComplete_ = true;
  return step.update();
}

bool StepCompleter::complete(const State& state, const Step& step, EndEffectorMotionBase& endEffectorMotion) const
{
  if (endEffectorMotion.getControlSetup().at(ControlLevel::Position)) {
    // TODO Check frame.
    Position startPositionInBaseFrame = adapter_->getPositionBaseToFootInBaseFrame(endEffectorMotion.getLimb(), state.getJointPositions(endEffectorMotion.getLimb()));
    Position startPositionInWorldFrame = adapter_->getPositionWorldToBaseInWorldFrame() + adapter_->getOrientationWorldToBase().inverseRotate(startPositionInBaseFrame);
    Position startPositionInDesiredFrame = startPositionInWorldFrame; // TODO
    endEffectorMotion.updateStartPosition(startPositionInDesiredFrame);
  }
  return endEffectorMotion.compute(state, step, *adapter_);
}

bool StepCompleter::complete(const State& state, const Step& step, JointMotionBase& jointMotion) const
{
  // Input.
  bool positionIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Position);
  bool velocityIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Velocity);
  bool accelerationIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Acceleration);
  bool effortIn = state.getControlSetup(jointMotion.getLimb()).at(ControlLevel::Effort);

  // Output.
  bool positionOut = jointMotion.getControlSetup().at(ControlLevel::Position);
  bool velocityOut = jointMotion.getControlSetup().at(ControlLevel::Velocity);
  bool accelerationOut = jointMotion.getControlSetup().at(ControlLevel::Acceleration);
  bool effortOut = jointMotion.getControlSetup().at(ControlLevel::Effort);

  // Check for special mode transitions.
  if (positionIn && !effortIn && positionOut && effortOut) {
    JointPositions startPosition = state.getJointPositions(jointMotion.getLimb());
    jointMotion.updateStartPosition(startPosition);
    JointEfforts startEffort = state.getJointEfforts(jointMotion.getLimb());
    startEffort.setZero();
    jointMotion.updateStartEfforts(startEffort);
  } else if (positionIn && effortIn && !positionOut && effortOut) {
    JointEfforts startEffort = adapter_->getJointEfforts(jointMotion.getLimb());
    jointMotion.updateStartEfforts(startEffort);
  } //else if (positionIn && effortIn && positionOut && !effortOut) {
    // TODO: Decrease torque to zero in special step.
  //} else if (!positionIn && effortIn && positionOut && !effortOut) {
    // TODO: Decrease compression on leg in special step.
  // }
  else {

    // Standard transitions.
    if (!positionIn && positionOut) {
      JointPositions startPosition = state.getJointPositions(jointMotion.getLimb());
      jointMotion.updateStartPosition(startPosition);
    }
    if (!velocityIn && velocityOut) {
      JointVelocities startVelocity = state.getJointVelocities(jointMotion.getLimb());
      jointMotion.updateStartVelocity(startVelocity);
    }
    if (!accelerationIn && accelerationOut) {
      JointAccelerations startAcceleration = state.getJointAccelerations(jointMotion.getLimb());
      jointMotion.updateStartAcceleration(startAcceleration);
    }
    if (!effortIn && effortOut) {
      JointEfforts startEffort = state.getJointEfforts(jointMotion.getLimb());
      jointMotion.updateStartEfforts(startEffort);
    }

  }
  return jointMotion.compute(state, step, *adapter_);
}

bool StepCompleter::complete(const State& state, const Step& step, const StepQueue& queue, BaseMotionBase& baseMotion) const
{
  if (baseMotion.getControlSetup().at(ControlLevel::Position)) {
    // TODO Check frame.
    Pose pose(state.getPositionWorldToBaseInWorldFrame(), state.getOrientationWorldToBase());
    baseMotion.updateStartPose(pose);
  }
  if (baseMotion.getControlSetup().at(ControlLevel::Velocity)) {
    // TODO
  }
  return baseMotion.compute(state, step, queue, *adapter_);
{'fraction_non_alphanumeric': 0.11926063470627954, 'fraction_numerical': 0.0008440243079000675, 'mean_word_length': 4.749150897622513, 'pattern_counts': {'":': 0, '<': 21, '<?xml version=': 0, '>': 35, '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': '661067', 'n_tokens_mistral': 3685, 'n_tokens_neox': 3453, 'n_words': 608}