Recovery Controller code

recovery.cpp

#include "hyq_cheetah/RecoveryStandController.hpp"
#include "hyq_cheetah/helpers/config.hpp"
#include <fstream>
#include <iomanip>
#include <nlohmann/json.hpp>

namespace hyq_cheetah
{

RecoveryControllerOutputData RecoveryStandController::Run(const StateEstimatorData &controllerInputData, const uint64_t &timeSinceStart, bool recover)
{
    assert(controllerInputData.jointPositions.size() == numJoints);
    assert(controllerInputData.jointVelocities.size() == numJoints);

    if (recover == true)
    {
        if (recover != recoverState_)
        {
            timeSinceStart_ = timeSinceStart;
            recoveryUpdateCount = 0;
            recoverState_ = recover;
        }
        if ((timeSinceStart - timeSinceStart_) * recoveryUpdateRate / 1000000000 >= recoveryUpdateCount)
        {
            if (recoveryUpdateCount < foldMaxIter_)
            {
                double progress = double(recoveryUpdateCount) / double(foldMaxIter_);
                for (int i = 0; i < (int)numJoints; i++)
                {
                    auto current_q = controllerInputData.jointPositions[i];
                    auto current_qd = controllerInputData.jointVelocities[i];
                    intermediatePos_[i] = progress * foldPosition_[i % 3] + (1.0 - progress) * current_q;
                }
            }
            else if (recoveryUpdateCount > foldMaxIter_ && recoveryUpdateCount < (foldMaxIter_ + standMaxIter_))
            {
                double progress = double(recoveryUpdateCount - foldMaxIter_) / double(standMaxIter_);
                std::cout << "stand progress:" << progress << std::endl;
                for (int i = 0; i < (int)numJoints; i++)
                {
                    auto current_q = controllerInputData.jointPositions[i];
                    auto current_qd = controllerInputData.jointVelocities[i];
                    intermediatePos_[i] = progress * standingPosition_[i % 3] + (1.0 - progress) * current_q;
                }
            }
            else if (recoveryUpdateCount > (foldMaxIter_ + standMaxIter_))
            {
                recoverState_ = false;
            }
            recoveryUpdateCount = (timeSinceStart - timeSinceStart_) * recoveryUpdateRate / 1000000000 + 1;
        }
        for (int i = 0; i < (int)numJoints; i++)
        {
            auto current_q = controllerInputData.jointPositions[i];
            auto current_qd = controllerInputData.jointVelocities[i];
            auto torque = pidControllers[i]->ComputePD(intermediatePos_[i], current_q, current_qd);
            //jointCommandMsg.add_commands(torque);
            controllerOutputData.commands[i] = torque;
            //jointCommandMsg.add_joint_names(stateEstimatorMsg.joint_names(i));
        }
    }
    else
    {
        for (int i = 0; i < (int)numJoints; i++)
        {
            auto current_q = controllerInputData.jointPositions[i];
            auto current_qd = controllerInputData.jointVelocities[i];
            auto torque = pidControllers[i]->ComputePD(standingPosition_[i % 3], current_q, current_qd);
            //jointCommandMsg.add_commands(torque);
            controllerOutputData.commands[i] = torque;
            //jointCommandMsg.add_joint_names(stateEstimatorMsg.joint_names(i));
        }
    }
    return controllerOutputData;
}

RecoveryStandController::RecoveryStandController(const std::string &robotName)
{
    std::string configFilePath = config::install_config_path + robotName + std::string("_config.json");
    std::ifstream i_file(configFilePath);
    nlohmann::json dataJson;
    i_file >> dataJson;
    double pGain = dataJson["recoveryStandController"]["pGain"];
    double dGain = dataJson["recoveryStandController"]["dGain"];
    standingPosition_ = dataJson["recoveryStandController"]["standingPosition"];
    foldPosition_ = dataJson["recoveryStandController"]["foldPosition"];
    foldMaxIter_ = dataJson["recoveryStandController"]["foldMaxIter"];
    standMaxIter_ = dataJson["recoveryStandController"]["standMaxIter"];
    std::cout << "pGain: " << pGain << std::endl;
    std::cout << "dGain: " << dGain << std::endl;

    for (size_t i = 0; i < numJoints; i++)
    {
        pidControllers[i] = std::make_unique<PIDController>(PIDController::Gains{pGain, 0.0, dGain, 20.0});
    }
}

bool RecoveryStandController::getRecoverState()
{
    return recoverState_;
}
} // namespace hyq_cheetah