gOcraSegCartesianTaskManager.cpp

Go to the documentation of this file.

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

namespace gocra
{

gOcraSegCartesianTaskManager::gOcraSegCartesianTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, const std::string& _segmentName, ocra::ECartesianDof _axes, double _stiffness, double _damping)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), segmentName(_segmentName), axes(_axes)
{
    _init(Eigen::Vector3d::Zero(), _stiffness, _damping);
}

gOcraSegCartesianTaskManager::gOcraSegCartesianTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, const std::string& _segmentName, const Eigen::Vector3d& _segPoint_Local, ocra::ECartesianDof _axes, double _stiffness, double _damping)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), segmentName(_segmentName), axes(_axes)
{
    _init(_segPoint_Local, _stiffness, _damping);
}

gOcraSegCartesianTaskManager::gOcraSegCartesianTaskManager(GHCJTController& _ctrl, const ocra::Model& _model, const std::string& _taskName, const std::string& _segmentName, ocra::ECartesianDof _axes, double _stiffness, double _damping, const Eigen::Vector3d& _poseDes)
    : gOcraTaskManagerBase(_ctrl, _model, _taskName), segmentName(_segmentName), axes(_axes)
{
    _init(Eigen::Vector3d::Zero(), _stiffness, _damping);
    // Have no idea this wrapper needs to be done
    setState(_poseDes);
}

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


void gOcraSegCartesianTaskManager::_init(const Eigen::Vector3d& _taskPoint_LocalFrame, double _stiffness, double _damping)
{
    featFrame = new ocra::SegmentFrame(name + ".SegmentFrame", model, model.SegmentName(segmentName), Eigen::Displacementd(_taskPoint_LocalFrame));
    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->activateAsObjective();
    task->setStiffness(_stiffness);
    task->setDamping(_damping);

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

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

void gOcraSegCartesianTaskManager::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 gOcraSegCartesianTaskManager::setStiffness(double stiffness)
{
    task->setStiffness(stiffness);
}

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

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

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

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

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

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


}