AdmittanceController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
 * @author     zhou ( you dot zhou at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "AdmittanceController.h"
 
#include <armarx/control/common/utils.h>
 
 
namespace armarx::control::njoint_mp_controller::task_space
{
NJointControllerRegistration<NJointTSAdmittanceMPController> registrationControllerNJointTSAdmittanceMPController("NJointTSAdmittanceMPController");
 
 
NJointTSAdmittanceMPController::NJointTSAdmittanceMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot):
    NJointTaskspaceAdmittanceController{robotUnit, config, robot},
    mp::MPPool{}
{
    const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
    createMPs(cfg.mpConfig);
    ARMARX_IMPORTANT << "NJointTSImpedanceMPController construction done";
}
 
 
std::string NJointTSAdmittanceMPController::getClassName(const Ice::Current &) const
{
    return "NJointTSAdmittanceMPController";
}
 
 
void NJointTSAdmittanceMPController::updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent)
{
    NJointTaskspaceAdmittanceController::updateConfig(dto);
    const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
    reconfigureMPs(cfg.mpConfig);
}
 
 
void NJointTSAdmittanceMPController::additionalTask()
{
    NJointTaskspaceAdmittanceController::additionalTask();
    /// parse arguments and run mp
    for (auto& _mp: mps)
    {
        if (_mp.second.mp->getClassName() == "TSMP")
        {
            mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
            in->pose    = controller.tcp->getPoseInRootFrame();
            in->vel     = controller.bufferNonRtToOnPublish.getWriteBuffer().currentTwist;
            in->deltaT  = bufferRtToAdditionalTask.getUpToDateReadBuffer().deltaT;
        }
    }
    runMPs();
    /// set mp target to rt buffer
    auto& rtConfig = bufferUserToRt.getWriteBuffer();
    for (auto& _mp: mps)
    {
        if (_mp.second.mp->getClassName() == "TSMP")
        {
            mp::TSMPOutputPtr out = std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
            rtConfig.desiredPose = out->pose;
            rtConfig.desiredTwist = out->vel;
            bufferUserToRt.commitWrite();
        }
    }
}
 
void NJointTSAdmittanceMPController::rtPreActivateController()
{
    ARMARX_IMPORTANT << "rt pre activate: reinitialize the mp input output, as well as the rt related buffer values";
    VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
    Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
    NJointTaskspaceAdmittanceController::rtPreActivateController();
    for (auto& _mp: mps)
    {
        if (_mp.second.mp->getClassName() == "TSMP")
        {
            ARMARX_IMPORTANT << "init input output data to/from MPs";
 
            std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
            std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
        }
    }
}
} /// namespace armarx::control::njoint_mp_controller::task_space