gOcraSegOrientationTaskManager.cpp

Go to the documentation of this file.

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

namespace gocra
{

gOcraSegOrientationTaskManager::gOcraSegOrientationTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, const std::string& _segmentName, double _stiffness, double _damping)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), segmentName(_segmentName)
{
    _init(Eigen::Rotation3d::Identity(), _stiffness, _damping);
}

gOcraSegOrientationTaskManager::gOcraSegOrientationTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, const std::string& _segmentName, double _stiffness, double _damping, const Eigen::Rotation3d& _orientationDes)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), segmentName(_segmentName)
{
    _init(Eigen::Rotation3d::Identity(), _stiffness, _damping);
    setOrientation(_orientationDes);
}

void gOcraSegOrientationTaskManager::_init(Eigen::Rotation3d _refOrientation_LocalFrame, double _stiffness, double _damping)
{
    featFrame = new ocra::SegmentFrame(name + ".SegmentFrame", model, model.SegmentName(segmentName), Eigen::Displacementd(Eigen::Vector3d::Zero(), _refOrientation_LocalFrame));
    featDesFrame = new ocra::TargetFrame(name + ".TargetFrame", model);
    feat = new ocra::OrientationFeature(name + ".OrientationFeature", *featFrame);
    featDes = new ocra::OrientationFeature(name + ".OrientationFeature_Des", *featDesFrame);

    featDesFrame->setPosition(Eigen::Displacementd::Identity());
    featDesFrame->setVelocity(Eigen::Twistd::Zero());
    featDesFrame->setAcceleration(Eigen::Twistd::Zero());

    task = &(ctrl.createGHCJTTask(name, *feat, *featDes));

    ctrl.addTask(task);

    task->activateAsObjective();
    task->setStiffness(_stiffness);
    task->setDamping(_damping);
}

void gOcraSegOrientationTaskManager::setOrientation(const Eigen::Rotation3d& orientation)
{
    featDesFrame->setPosition(Eigen::Displacementd(Eigen::Vector3d::Zero(), orientation));
    featDesFrame->setVelocity(Eigen::Twistd::Zero());
    featDesFrame->setAcceleration(Eigen::Twistd::Zero());
}



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

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

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

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

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

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

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


}