KeypointsMPController.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 "KeypointsMPController.h"
 
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
namespace armarx::control::njoint_mp_controller::task_space
{
    NJointControllerRegistration<KeypointMPController>
        registrationControllerKeypointMPController("KeypointMPController");
 
    KeypointMPController::KeypointMPController(const RobotUnitPtr& robotUnit,
                                               const NJointControllerConfigPtr& config,
                                               const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "creating task-space admittance controller";
        KeypointMPControllerConfigPtr cfg = KeypointMPControllerConfigPtr::dynamicCast(config);
 
        ARMARX_CHECK_EXPRESSION(cfg);
        ARMARX_CHECK_EXPRESSION(robotUnit);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        useSynchronizedRtRobot();
        kinematicChainName = cfg->nodeSetName;
 
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
 
        jointNames.clear();
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
            jointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            ARMARX_CHECK_EXPRESSION(ct);
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
            ARMARX_CHECK_EXPRESSION(casted_ct);
            targets.push_back(casted_ct);
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            torqueSensors.push_back(torqueSensor);
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
        controller.initialize(rns);
 
        ftsensor.initialize(rns, robotUnit, cfg->controlParamJsonFile);
 
        /// mp related
        //    pointVMP.reset(new mplib::representation::vmp::PrincipalComponentVMP());
 
        mplib::factories::VMPFactory mpfactory;
        mpfactory.addConfig("kernelSize", 100);
        mplib::representation::VMPType vmp_type =
            mplib::representation::VMPType::PrincipalComponent;
        std::shared_ptr<mplib::representation::AbstractMovementPrimitive> vmp =
            mpfactory.createMP(vmp_type);
        pointVMP =
            std::dynamic_pointer_cast<mplib::representation::vmp::PrincipalComponentVMP>(vmp);
 
        /// buffer related
        common::FTSensor::FTBufferData ftData;
        ftData.forceBaseline.setZero();
        ftData.torqueBaseline.setZero();
        ftData.enableTCPGravityCompensation = cfg->enableTCPGravityCompensation;
        ftData.tcpMass = cfg->tcpMass;
        ftData.tcpCoMInFTSensorFrame = cfg->tcpCoMInForceSensorFrame;
        ftSensorBuffer.reinitAllBuffers(ftData);
 
        law::TaskspaceKeypointsAdmittanceController::Config configData;
        configData.kpImpedance = cfg->kpImpedance;
        configData.kdImpedance = cfg->kdImpedance;
        configData.kpAdmittance = cfg->kpAdmittance;
        configData.kdAdmittance = cfg->kdAdmittance;
        configData.kmAdmittance = cfg->kmAdmittance;
 
        configData.kpNullspaceTorque = cfg->kpNullspaceTorque;
        configData.kdNullspaceTorque = cfg->kdNullspaceTorque;
 
        configData.currentForceTorque.setZero();
        configData.desiredTCPPose = Eigen::Matrix4f::Identity();
        configData.desiredTCPTwist.setZero();
        configData.desiredNullspaceJointAngles = cfg->desiredNullspaceJointAngles;
 
        configData.torqueLimit = cfg->torqueLimit;
        configData.qvelFilter = cfg->qvelFilter;
 
        ARMARX_IMPORTANT << VAROUT(cfg->numKeypoints);
        ARMARX_CHECK_EQUAL(cfg->initialKeypointPosition.size(), cfg->numKeypoints * 3);
        ARMARX_CHECK_EQUAL(cfg->pointCtrlMask.size(), cfg->numKeypoints * 3);
        configData.numPoints = cfg->numKeypoints;
        //    configData.keypointStiffness = cfg->keypointStiffness;
        configData.keypointKp = cfg->keypointKp;
        configData.keypointKi = cfg->keypointKi;
        configData.keypointKd = cfg->keypointKd;
        configData.maxControlValue = cfg->maxControlValue;
        configData.maxDerivation = cfg->maxDerivation;
 
        configData.pointCtrlMask = cfg->pointCtrlMask;
        configData.currentKeypointPosition = cfg->initialKeypointPosition;
        configData.filteredKeypointPosition = cfg->initialKeypointPosition;
        configData.desiredKeypointPosition = cfg->initialKeypointPosition;
        configData.isRigid = cfg->isRigid;
        configData.fixedTranslation.setZero(cfg->numKeypoints * 3);
        reinitTripleBuffer(configData);
 
        numPoints = cfg->numKeypoints;
        initialStateEigen = cfg->initialKeypointPosition;
        controller.filterCoeff = cfg->filterCoeff;
        controller.s.filteredKeypointPosition = cfg->initialKeypointPosition;
        controller.s.isRigid = cfg->isRigid;
        controller.s.fixedTranslation.setZero(cfg->numKeypoints * 3);
        controller.s.currentKeypointPosition = cfg->initialKeypointPosition;
 
        controller.initPID(cfg->keypointKp,
                           cfg->keypointKi,
                           cfg->keypointKd,
                           cfg->maxControlValue,
                           cfg->maxDerivation);
 
        //    controller.s.numPoints = cfg->numKeypoints;
        //    controller.s.desiredKeypointPosition = cfg->initialKeypointPosition;
 
 
        rt2mpInfo rt2mp;
        for (int i = 0; i < cfg->numKeypoints * 3; i++)
        {
            mplib::core::DVec state;
            state.push_back(cfg->initialKeypointPosition[i]);
            state.push_back(0.0);
            rt2mp.currentState.push_back(state);
        }
        rt2mp.deltaT = 0.0;
        rt2mpBuffer.reinitAllBuffers(rt2mp);
    }
 
    std::string
    KeypointMPController::getClassName(const Ice::Current&) const
    {
        return "KeypointMPController";
    }
 
    std::string
    KeypointMPController::getKinematicChainName(const Ice::Current&)
    {
        return kinematicChainName;
    }
 
    void
    KeypointMPController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                                const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
 
        bool valid = controller.updateControlStatus(rtGetControlStruct(),
                                                    timeSinceLastIteration,
                                                    torqueSensors,
                                                    velocitySensors,
                                                    positionSensors);
 
        /// for mp controllers you can commit the status buffer now
        {
            for (size_t i = 0; i < (unsigned)controller.s.numPoints * 3; i++)
            {
                mplib::core::DVec state;
                state.push_back(controller.s.currentKeypointPosition[i]);
                state.push_back(0.0);
                rt2mpBuffer.getWriteBuffer().currentState[i] = state;
            }
            rt2mpBuffer.getWriteBuffer().deltaT = controller.s.deltaT;
            rt2mpBuffer.commitWrite();
 
            controlStatusBuffer.getWriteBuffer().currentPose = controller.s.currentPose;
            controlStatusBuffer.getWriteBuffer().currentTwist = controller.s.currentTwist * 1000.0f;
            controlStatusBuffer.getWriteBuffer().deltaT = controller.s.deltaT;
            controlStatusBuffer.getWriteBuffer().currentKeypointPosition =
                controller.s.currentKeypointPosition;
            controlStatusBuffer.getWriteBuffer().desiredKeypointPosition =
                controller.s.desiredKeypointPosition;
            controlStatusBuffer.commitWrite();
        }
 
        if (rtFirstRun.load())
        {
            rtFirstRun.store(false);
            rtReady.store(false);
 
            ftsensor.reset();
            controller.firstRun();
            //        ARMARX_IMPORTANT << "admittance control first run with\n" << VAROUT(controller.s.desiredTCPPose);
        }
        else
        {
            ftsensor.compensateTCPGravity(ftSensorBuffer.getReadBuffer());
            if (!rtReady.load())
            {
                if (ftsensor.calibrate(controller.s.deltaT))
                {
                    rtReady.store(true);
                }
            }
            else
            {
                if (valid)
                {
                    controller.s.currentForceTorque =
                        ftsensor.getFilteredForceTorque(ftSensorBuffer.getReadBuffer());
                }
            }
        }
 
        const auto& desiredJointTorques = controller.run(rtReady.load());
        ARMARX_CHECK_EQUAL(int(targets.size()), int(desiredJointTorques.size()));
 
        /// write torque target to joint device
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->torque = desiredJointTorques(i);
            if (!targets.at(i)->isValid())
            {
                targets.at(i)->torque = 0;
            }
        }
 
        /// for debug output
        {
            controlStatusBuffer.getWriteBuffer().kpImpedance = controller.s.kpImpedance;
            controlStatusBuffer.getWriteBuffer().kdImpedance = controller.s.kdImpedance;
            controlStatusBuffer.getWriteBuffer().forceImpedance = controller.s.forceImpedance;
            controlStatusBuffer.getWriteBuffer().currentForceTorque =
                controller.s.currentForceTorque;
 
            controlStatusBuffer.getWriteBuffer().virtualPose = controller.s.virtualPose;
            controlStatusBuffer.getWriteBuffer().desiredTCPPose = controller.s.desiredTCPPose;
            controlStatusBuffer.getWriteBuffer().virtualVel = controller.s.virtualVel;
            controlStatusBuffer.getWriteBuffer().virtualAcc = controller.s.virtualAcc;
            controlStatusBuffer.getWriteBuffer().currentKeypointPosition =
                controller.s.currentKeypointPosition;
            controlStatusBuffer.getWriteBuffer().desiredKeypointPosition =
                controller.s.desiredKeypointPosition;
            controlStatusBuffer.getWriteBuffer().filteredKeypointPosition =
                controller.s.filteredKeypointPosition;
            controlStatusBuffer.getWriteBuffer().pointTrackingForce =
                controller.s.pointTrackingForce;
            controlStatusBuffer.commitWrite();
        }
    }
 
    void
    KeypointMPController::toggleMP(const bool enableMP, const Ice::Current&)
    {
        mpEnabled.store(enableMP);
        ARMARX_IMPORTANT << "Toggle MP: " << enableMP;
    }
 
    bool
    KeypointMPController::getMPEnabled(const Ice::Current&)
    {
        return mpEnabled.load();
    }
 
    void
    KeypointMPController::runMP()
    {
        if (!rtReady.load() && !mpEnabled.load())
        {
            // do something
            usleep(1000);
        }
        else
        {
            mplib::core::DVec2d currentState = rt2mpBuffer.getUpToDateReadBuffer().currentState;
            double deltaT = rt2mpBuffer.getUpToDateReadBuffer().deltaT;
 
            if (canVal > 1e-8 && mpRunning.load())
            {
                double phaseStop = 0;
                double tau = 1.0;
 
                canVal -= tau * deltaT * 1 / ((1 + phaseStop) * timeDuration);
                currentState = pointVMP->calculateDesiredState(canVal, currentState);
 
                Eigen::VectorXf desiredPosition;
                desiredPosition.setZero(currentState.size());
                for (size_t i = 0; i < currentState.size(); i++)
                {
                    desiredPosition[i] = currentState[i][0];
                }
                getWriterControlStruct().desiredKeypointPosition = desiredPosition;
                writeControlStruct();
            }
            else
            {
                // normally set velocity to zero. for keypoint velocity?
                usleep(1000);
            }
        }
    }
 
    void
    KeypointMPController::reconfigureController(const std::string&, const Ice::Current&)
    {
    }
 
    void
    KeypointMPController::publishPose(StringVariantBaseMap& datafields,
                                      const std::string& name,
                                      const Eigen::Matrix4f& pose)
    {
        Eigen::Vector6f vec;
        vec.head<3>() = pose.block<3, 1>(0, 3);
        vec.tail<3>() = VirtualRobot::MathTools::eigen4f2rpy(pose);
        publishVec6f(datafields, name, vec);
    }
 
    void
    KeypointMPController::publishVec6f(StringVariantBaseMap& datafields,
                                       const std::string& name,
                                       const Eigen::Vector6f& vec)
    {
        datafields[name + "_x"] = new Variant(vec(0));
        datafields[name + "_y"] = new Variant(vec(1));
        datafields[name + "_z"] = new Variant(vec(2));
        datafields[name + "_rx"] = new Variant(vec(3));
        datafields[name + "_ry"] = new Variant(vec(4));
        datafields[name + "_rz"] = new Variant(vec(5));
    }
 
    void
    KeypointMPController::publishVecXf(StringVariantBaseMap& datafields,
                                       const std::string& name,
                                       const Eigen::Vector6f& vec)
    {
        for (int i = 0; i < vec.size(); i++)
        {
            datafields[name + "_" + std::to_string(i)] = new Variant(vec(i));
        }
    }
 
    void
    KeypointMPController::onPublish(const SensorAndControl&,
                                    const DebugDrawerInterfacePrx&,
                                    const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = debugRTBuffer.getUpToDateReadBuffer().desired_torques;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        //    publishPose(datafields, "virtual_pose", controlStatusBuffer.getUpToDateReadBuffer().virtualPose);
        //    publishPose(datafields, "desired_pose", controlStatusBuffer.getUpToDateReadBuffer().desiredTCPPose);
        //    publishVec6f(datafields, "virtual_vel", controlStatusBuffer.getUpToDateReadBuffer().virtualVel);
        //    publishVec6f(datafields, "virtual_acc", controlStatusBuffer.getUpToDateReadBuffer().virtualAcc);
        //    publishVec6f(datafields, "kp_Impedance", controlStatusBuffer.getUpToDateReadBuffer().kpImpedance);
        //    publishVec6f(datafields, "kd_Impedance", controlStatusBuffer.getUpToDateReadBuffer().kdImpedance);
        //    publishVec6f(datafields, "force_Impedance", controlStatusBuffer.getUpToDateReadBuffer().forceImpedance);
        //    publishVec6f(datafields, "current_ForceTorque", controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque);
        //    publishVecXf(datafields, "current_KeypointPosition", controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition);
        //    publishVecXf(datafields, "desired_KeypointPosition", controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition);
 
        //    Eigen::Matrix4f virtualPose = controlStatusBuffer.getUpToDateReadBuffer().virtualPose;
        //    Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(virtualPose);
        //    datafields["virtualPose_x"]  = new Variant(virtualPose(0, 3));
        //    datafields["virtualPose_y"]  = new Variant(virtualPose(1, 3));
        //    datafields["virtualPose_z"]  = new Variant(virtualPose(2, 3));
        //    datafields["virtualPose_rx"] = new Variant(rpy(0));
        //    datafields["virtualPose_ry"] = new Variant(rpy(1));
        //    datafields["virtualPose_rz"] = new Variant(rpy(2));
 
        //    Eigen::Matrix4f desiredPose = controlStatusBuffer.getUpToDateReadBuffer().desiredTCPPose;
        //    Eigen::Vector3f rpyDesired = VirtualRobot::MathTools::eigen4f2rpy(desiredPose);
        //    datafields["desiredPose_x"]  = new Variant(desiredPose(0, 3));
        //    datafields["desiredPose_y"]  = new Variant(desiredPose(1, 3));
        //    datafields["desiredPose_z"]  = new Variant(desiredPose(2, 3));
        //    datafields["desiredPose_rx"] = new Variant(rpyDesired(0));
        //    datafields["desiredPose_ry"] = new Variant(rpyDesired(1));
        //    datafields["desiredPose_rz"] = new Variant(rpyDesired(2));
 
        //    datafields["virtualVel_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(0));
        //    datafields["virtualVel_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(1));
        //    datafields["virtualVel_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(2));
        //    datafields["virtualVel_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(3));
        //    datafields["virtualVel_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(4));
        //    datafields["virtualVel_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(5));
 
        //    datafields["virtualAcc_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(0));
        //    datafields["virtualAcc_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(1));
        //    datafields["virtualAcc_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(2));
        //    datafields["virtualAcc_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(3));
        //    datafields["virtualAcc_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(4));
        //    datafields["virtualAcc_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(5));
 
        //    datafields["kpImpedance_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(0));
        //    datafields["kpImpedance_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(1));
        //    datafields["kpImpedance_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(2));
        //    datafields["kpImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(3));
        //    datafields["kpImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(4));
        //    datafields["kpImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(5));
 
        //    datafields["kdImpedance_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(0));
        //    datafields["kdImpedance_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(1));
        //    datafields["kdImpedance_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(2));
        //    datafields["kdImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(3));
        //    datafields["kdImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(4));
        //    datafields["kdImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(5));
 
        //    datafields["forceImpedance_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(0));
        //    datafields["forceImpedance_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(1));
        //    datafields["forceImpedance_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(2));
        //    datafields["forceImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(3));
        //    datafields["forceImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(4));
        //    datafields["forceImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(5));
 
        //    datafields["currentForceTorque_x"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(0));
        //    datafields["currentForceTorque_y"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(1));
        //    datafields["currentForceTorque_z"]  = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(2));
        //    datafields["currentForceTorque_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(3));
        //    datafields["currentForceTorque_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(4));
        //    datafields["currentForceTorque_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(5));
 
 
        datafields["currentKeypointPosition_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(0));
        datafields["currentKeypointPosition_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(1));
        datafields["currentKeypointPosition_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(2));
        //    datafields["currentKeypointPosition_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(3));
        //    datafields["currentKeypointPosition_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(4));
        //    datafields["currentKeypointPosition_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(5));
 
        datafields["desiredKeypointPosition_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(0));
        datafields["desiredKeypointPosition_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(1));
        datafields["desiredKeypointPosition_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(2));
        //    datafields["desiredKeypointPosition_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(3));
        //    datafields["desiredKeypointPosition_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(4));
        //    datafields["desiredKeypointPosition_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(5));
 
        datafields["filteredKeypointPosition_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(0));
        datafields["filteredKeypointPosition_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(1));
        datafields["filteredKeypointPosition_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(2));
 
        datafields["track_force_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(0));
        datafields["track_force_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(1));
        datafields["track_force_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(2));
        datafields["track_force_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(3));
        datafields["track_force_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(4));
        datafields["track_force_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(5));
 
        debugObs->setDebugChannel("KeypointMPController", datafields);
    }
 
    void
    KeypointMPController::setTCPPose(const Eigen::Matrix4f& pose, const Ice::Current&)
    {
        //    LockGuardType guard {controlDataMutex};
        //    getWriterControlStruct().desiredTCPPose = pose;
        //    writeControlStruct();
    }
 
    //void KeypointMPController::setNullspaceJointAngles(const Eigen::VectorXf& jointAngles, const Ice::Current &)
    //{
    //    LockGuardType guard {controlDataMutex};
    //    getWriterControlStruct().desiredNullspaceJointAngles = jointAngles;
    //    writeControlStruct();
    //}
 
    void
    KeypointMPController::setControlParameters(const std::string& name,
                                               const Eigen::VectorXf& value,
                                               const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        if (name == "kpImpedance")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kpImpedance = value;
        }
        else if (name == "kdImpedance")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kdImpedance = value;
        }
        else if (name == "kpAdmittance")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kpAdmittance = value;
        }
        else if (name == "kdAdmittance")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kdAdmittance = value;
        }
        else if (name == "kmAdmittance")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kmAdmittance = value;
        }
        else if (name == "kpNullspaceTorque")
        {
            ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
            getWriterControlStruct().kpNullspaceTorque = value;
        }
        else if (name == "kdNullspaceTorque")
        {
            ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
            getWriterControlStruct().kdNullspaceTorque = value;
        }
        else
        {
            ARMARX_ERROR << name << " is not supported by TaskSpaceAdmittanceController";
        }
        writeControlStruct();
    }
 
    void
    KeypointMPController::setForceTorqueBaseline(const Eigen::Vector3f& forceBaseline,
                                                 const Eigen::Vector3f& torqueBaseline,
                                                 const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().forceBaseline = forceBaseline;
        ftSensorBuffer.getWriteBuffer().torqueBaseline = torqueBaseline;
        ftSensorBuffer.commitWrite();
    }
 
    void
    KeypointMPController::setTCPMass(Ice::Float mass, const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
        ftSensorBuffer.getWriteBuffer().tcpMass = mass;
        ftSensorBuffer.commitWrite();
    }
 
    void
    KeypointMPController::setTCPCoMInFTFrame(const Eigen::Vector3f& tcpCoMInFTSensorFrame,
                                             const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
        ftSensorBuffer.getWriteBuffer().tcpCoMInFTSensorFrame = tcpCoMInFTSensorFrame;
        ftSensorBuffer.commitWrite();
    }
 
    void
    KeypointMPController::calibrateFTSensor(const Ice::Current&)
    {
        rtReady.store(false);
    }
 
    std::string
    KeypointMPController::getNames(const Ice::Current&)
    {
        return "KeypointMP";
    }
 
    void
    KeypointMPController::start(const Ice::DoubleSeq& goal, const Ice::Current&)
    {
        pointVMP->prepareExecution(goal, initialState, 1.0);
        mplib::representation::MPStateVec initState =
            mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(
                initialState);
    }
 
    void
    KeypointMPController::startWithTime(const Ice::DoubleSeq& goal,
                                        Ice::Double duration,
                                        const Ice::Current&)
    {
        timeDuration = duration;
        pointVMP->prepareExecution(goal, initialState, 1.0);
        mplib::representation::MPStateVec initState =
            mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(
                initialState);
        mpRunning.store(true);
    }
 
    void
    KeypointMPController::startAsTraj(const Ice::Current&)
    {
        pointVMP->prepareExecution(goal, initialState, 1.0);
        mplib::representation::MPStateVec initState =
            mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(
                initialState);
    }
 
    void
    KeypointMPController::startAsTrajWithTime(Ice::Double duration, const Ice::Current&)
    {
    }
 
    void
    KeypointMPController::stop(const Ice::Current&)
    {
        mpRunning.store(false);
    }
 
    void
    KeypointMPController::pause(const Ice::Current&)
    {
        mpRunning.store(false);
    }
 
    void
    KeypointMPController::resume(const Ice::Current&)
    {
        mpRunning.store(true);
    }
 
    void
    KeypointMPController::reset(const Ice::Current&)
    {
        if (mpRunning.load())
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        rtFirstRun = true;
    }
 
    bool
    KeypointMPController::isFinished(const Ice::Current&)
    {
        return !mpRunning.load();
    }
 
    void
    KeypointMPController::learnFromCSV(const Ice::StringSeq& fileList, const Ice::Current&)
    {
        std::vector<mplib::core::SampledTrajectory> trajs;
        for (auto& file : fileList)
        {
            mplib::core::SampledTrajectory traj;
            traj.readFromCSVFile(file);
            trajs.push_back(traj);
        }
        pointVMP->learnFromTrajectories(trajs);
 
        ARMARX_CHECK_EQUAL(trajs[0].dim(), (unsigned)numPoints * 3);
        initialState.clear();
        goal.clear();
 
        for (size_t i = 0; i < trajs[0].dim(); i++)
        {
            mplib::core::DVec state;
            //            state.push_back(trajs[0].begin()->getPosition(i));
            //            state.push_back(trajs[0].begin()->getDeriv(i, 1));
            state.push_back(initialStateEigen[i]);
            state.push_back(0.0);
            initialState.push_back(state);
            goal.push_back(trajs[0].rbegin()->getPosition(i));
        }
    }
 
    void
    KeypointMPController::setGoal(const Ice::DoubleSeq&, const Ice::Current&)
    {
    }
 
    void
    KeypointMPController::setStartAndGoal(const Ice::DoubleSeq&,
                                          const Ice::DoubleSeq&,
                                          const Ice::Current&)
    {
    }
 
    void
    KeypointMPController::setViaPoint(Ice::Double, const Ice::DoubleSeq&, const Ice::Current&)
    {
    }
 
    void
    KeypointMPController::removeAllViaPoint(const Ice::Current&)
    {
    }
 
    std::string
    KeypointMPController::serialize(const Ice::Current&)
    {
        //    std::stringstream ss;
        //    boost::archive::text_oarchive oa{ss};
        //    oa << pointVMP.get();
        //    return ss.str();
        return "notImplemented";
    }
 
    Ice::DoubleSeq
    KeypointMPController::deserialize(const std::string& mpAsString, const Ice::Current&)
    {
        //    std::stringstream ss;
        //    ss.str(mpAsString);
        //    boost::archive::text_iarchive ia{ss};
 
        //    VMPPtr mpPointer;
        //    ia >> mpPointer;
        //    pointVMP.reset(mpPointer);
        //    return pointVMP->defaultGoal;
        std::vector<double> dummy;
        dummy.push_back(0.0);
        return dummy;
    }
 
    Ice::Double
    KeypointMPController::getCanVal(const Ice::Current&)
    {
        return canVal;
    }
 
    void
    KeypointMPController::toggleGravityCompensation(const bool toggle, const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = toggle;
        ftSensorBuffer.commitWrite();
        rtReady.store(false); /// you also need to re-calibrate ft sensor
    }
 
    void
    KeypointMPController::rtPreActivateController()
    {
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose;
        controller.s.currentPose = currentPose;
        controller.s.desiredTCPPose = currentPose;
        controller.s.previousTargetPose = currentPose;
        controller.s.virtualPose = currentPose;
        controller.desiredPose = currentPose;
        controller.s.virtualVel.setZero();
        controller.s.virtualAcc.setZero();
        Eigen::VectorXf fixedTranslation;
        fixedTranslation.setZero(controller.s.currentKeypointPosition.size());
        ARMARX_IMPORTANT << VAROUT(controller.s.isRigid)
                         << VAROUT(controller.s.currentKeypointPosition);
        if (controller.s.isRigid)
        {
            for (int i = 0; i < numPoints; i++)
            {
                fixedTranslation.segment(3 * i, 3) =
                    controller.s.currentKeypointPosition.segment(3 * i, 3) -
                    currentPose.block<3, 1>(0, 3);
            }
            ARMARX_IMPORTANT << VAROUT(fixedTranslation);
        }
        controller.s.fixedTranslation = fixedTranslation;
        controller.pid->reset();
        getWriterControlStruct().fixedTranslation = controller.s.fixedTranslation;
        getWriterControlStruct().desiredTCPPose = currentPose;
        writeControlStruct();
    }
 
    void
    KeypointMPController::onInitNJointController()
    {
        runTask("KeypointMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT << "Try to run mp";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            runMP();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    KeypointMPController::setKeypoints(const Eigen::VectorXf& keypointPosition, const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        ARMARX_CHECK_EQUAL(getWriterControlStruct().currentKeypointPosition.size(),
                           keypointPosition.size());
        getWriterControlStruct().currentKeypointPosition = keypointPosition;
        writeControlStruct();
    }
 
    void
    KeypointMPController::resetKeypoints(const int n_points,
                                         const float keypoint_stiffness,
                                         const bool is_rigid,
                                         const Eigen::VectorXf& ctrl_mask,
                                         const Eigen::VectorXf& init_keypoints_position,
                                         const Ice::Current&)
    {
        numPoints = n_points;
        getWriterControlStruct().numPoints = n_points;
        getWriterControlStruct().keypointKp = keypoint_stiffness;
        getWriterControlStruct().isRigid = is_rigid;
        getWriterControlStruct().pointCtrlMask = ctrl_mask;
        getWriterControlStruct().currentKeypointPosition = init_keypoints_position;
        writeControlStruct();
    }
} // namespace armarx::control::njoint_mp_controller::task_space