gOcraSegPoseTaskManager.cpp

Go to the documentation of this file.

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

namespace gocra
{

gOcraSegPoseTaskManager::gOcraSegPoseTaskManager(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::Displacementd::Identity(), _stiffness, _damping);
}

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

void gOcraSegPoseTaskManager::_init(const Eigen::Displacementd& _ref_LocalFrame, double _stiffness, double _damping)
{
    featFrame = new ocra::SegmentFrame(name + ".SegmentFrame", model, model.SegmentName(segmentName), _ref_LocalFrame);
    featDesFrame = new ocra::TargetFrame(name + ".TargetFrame", model);
    feat = new ocra::DisplacementFeature(name + ".DisplacementFeature", *featFrame, axes);
    featDes = new ocra::DisplacementFeature(name + ".DisplacementFeature_Des", *featDesFrame, axes);

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

    ctrl.addTask(task);

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

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


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

void gOcraSegPoseTaskManager::setState(const Eigen::Displacementd& pose)
{
    setState(pose, Eigen::Twistd::Zero(), Eigen::Twistd::Zero());
}

void gOcraSegPoseTaskManager::setState(const Eigen::Displacementd& pose, const Eigen::Twistd& velocity, const Eigen::Twistd& acceleration)
{
    featDesFrame->setPosition(pose);
    featDesFrame->setVelocity(velocity);
    featDesFrame->setAcceleration(acceleration);
}


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

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

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

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

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


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

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

}