gOcraFullPostureTaskManager.cpp

Go to the documentation of this file.

#include "gocra/Tasks/Managers/gOcraFullPostureTaskManager.h"

namespace gocra
{

gOcraFullPostureTaskManager::gOcraFullPostureTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, int _fullStateType, double _stiffness, double _damping)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName)
{
    _init(_fullStateType, _stiffness, _damping);
}

gOcraFullPostureTaskManager::gOcraFullPostureTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, int _fullStateType, double _stiffness, double _damping, const Eigen::VectorXd& _init_q)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName)
{
    _init(_fullStateType, _stiffness, _damping);
    setPosture(_init_q);
}

void gOcraFullPostureTaskManager::_init(int fullStateType, double stiffness, double damping)
{
    featState = new ocra::FullModelState(name + ".FullModelState", model, fullStateType);
    featDesState = new ocra::FullTargetState(name + ".FullTargetState", model, fullStateType);
    feat = new ocra::FullStateFeature(name + ".FullStateFeature", *featState);
    featDes = new ocra::FullStateFeature(name + ".FullStateFeature_Des", *featDesState);

    // The feature initializes as Zero for posture
    task = &(ctrl.createGHCJTTask(name, *feat, *featDes));

    ctrl.addTask(task);


    task->setStiffness(stiffness);
    task->setDamping(damping);
    task->setWeight(1);
    task->activateAsObjective();
}

void gOcraFullPostureTaskManager::setPosture(const Eigen::VectorXd& q)
{
    setPosture(q, Eigen::VectorXd::Zero(featDesState->getSize()), Eigen::VectorXd::Zero(featDesState->getSize()));
}

void gOcraFullPostureTaskManager::setPosture(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot, const Eigen::VectorXd& qddot)
{
    // Need to check size of q
    if (q.size() != featDesState->getSize())
    {
        throw std::runtime_error("[gOcraFullPostureTaskManager::setPosture(Eigen::VectorXd&, Eigen::VectorXd&, Eigen::VectorXd&)] Input pose and required dimension does not match");
    }
    else if (qdot.size() != featDesState->getSize())
    {
        throw std::runtime_error("[gOcraFullPostureTaskManager::setPosture(Eigen::VectorXd&, Eigen::VectorXd&, Eigen::VectorXd&)] Input pose velocity and required dimension does not match");
    }
    else if (qddot.size() != featDesState->getSize())
    {
        throw std::runtime_error("[gOcraFullPostureTaskManager::setPosture(Eigen::VectorXd&, Eigen::VectorXd&, Eigen::VectorXd&)] Input pose acceleration and required dimension does not match");
    }

    featDesState->set_q(q);
    featDesState->set_qdot(qdot);
    featDesState->set_qddot(qddot);
}



void gOcraFullPostureTaskManager::setStiffness(double stiffness)
{
    task->setStiffness(stiffness);
}

double gOcraFullPostureTaskManager::getStiffness()
{
    Eigen::MatrixXd K = task->getStiffness();
    return K(0, 0);
}

void gOcraFullPostureTaskManager::setDamping(double damping)
{
    task->setDamping(damping);
}

double gOcraFullPostureTaskManager::getDamping()
{
    Eigen::MatrixXd C = task->getDamping();
    return C(0, 0);
}

void gOcraFullPostureTaskManager::activate()
{
    task->activateAsObjective();
}

void gOcraFullPostureTaskManager::deactivate()
{
    task->deactivate();
}

Eigen::VectorXd gOcraFullPostureTaskManager::getTaskError()
{
    return task->getError();
}


}