gOcraPartialPostureTaskManager.cpp

Go to the documentation of this file.

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

namespace gocra
{

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

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

void gOcraPartialPostureTaskManager::_init(int _fullStateType, VectorXi& _dofIndices, double _stiffness, double _damping)
{
    featState = new ocra::PartialModelState(name + ".PartialModelState", model, _dofIndices, _fullStateType);
    featDesState = new ocra::PartialTargetState(name + ".PartialTargetState", model, _dofIndices, _fullStateType);
    feat = new ocra::PartialStateFeature(name + ".PartialStateFeature", *featState);
    featDes = new ocra::PartialStateFeature(name + ".PartialStateFeature_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 gOcraPartialPostureTaskManager::setPosture(Eigen::VectorXd& q)
{
    // Need to check size of q
    if (q.size() != featDesState->getSize())
    {
        throw std::runtime_error("[gOcraPartialPostureTaskManager::setPosture(Eigen::VectorXd&)] Input pose and required dimension does not match");
    }
    featDesState->set_q(q);
    featDesState->set_qdot(Eigen::VectorXd::Zero(featDesState->getSize()));
    featDesState->set_qddot(Eigen::VectorXd::Zero(featDesState->getSize()));
}

void gOcraPartialPostureTaskManager::setPosture(Eigen::VectorXd& q, Eigen::VectorXd& qdot, Eigen::VectorXd& qddot)
{
    // Need to check size of q
    if (q.size() != featDesState->getSize())
    {
        throw std::runtime_error("[gOcraPartialPostureTaskManager::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("[gOcraPartialPostureTaskManager::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("[gOcraPartialPostureTaskManager::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 gOcraPartialPostureTaskManager::activate()
{
    task->activateAsObjective();
}

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


}