gOcraCoMTaskManager.cpp

Go to the documentation of this file.

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

namespace gocra
{

gOcraCoMTaskManager::gOcraCoMTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, ocra::ECartesianDof _axes, double _stiffness, double _damping)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), axes(_axes)
{
    _init(_stiffness, _damping);
}

gOcraCoMTaskManager::gOcraCoMTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, ocra::ECartesianDof _axes, double _stiffness, double _damping, Eigen::Vector3d _posDes)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), axes(_axes)
{
    _init(_stiffness, _damping);
    setState(_posDes);
}

/*
gOcraCoMTaskManager::gOcraCoMTaskManager(GHCJTController& ctrl, const Model& model, const std::string& taskName, ocra::ECartesianDof axes, double stiffness, double damping, double weight, Eigen::Vector3d posDes, Eigen::Vector3d velDes, Eigen::Vector3d accDes)
    : _ctrl(ctrl), _model(model), _name(taskName), axes(_axes)
{
    _init(stiffness, damping, weight);
}
*/

void gOcraCoMTaskManager::_init(double stiffness, double damping)
{
    featFrame = new ocra::CoMFrame(name + ".CoMFrame", model);
    featDesFrame = new ocra::TargetFrame(name + ".TargetFrame", model);
    feat = new ocra::PositionFeature(name + ".PositionFeature", *featFrame, axes);
    featDes = new ocra::PositionFeature(name + ".PositionFeature_Des", *featDesFrame, axes);
    task = &(ctrl.createGHCJTTask(name, *feat, *featDes));
    ctrl.addTask(task);
    task->setStiffness(stiffness);
    task->setDamping(damping);
    task->setWeight(1);
    task->activateAsObjective();

    // Set the desired state to the current position of the segment with 0 vel or acc
    setState(model.getCoMPosition());
}

void gOcraCoMTaskManager::setState(const Eigen::Vector3d& position)
{
    setState(position, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
}

void gOcraCoMTaskManager::setState(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration)
{
    featDesFrame->setPosition(Eigen::Displacementd(position));
    featDesFrame->setVelocity(Eigen::Twistd(0.0, 0.0, 0.0, velocity(0), velocity(1), velocity(2)) );
    featDesFrame->setAcceleration(Eigen::Twistd(0.0, 0.0, 0.0, acceleration(0), acceleration(1), acceleration(2)) );
}



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

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

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

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

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

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

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

}