BiKAC.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/>.
 *
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "BiKAC.h"
 
#include <VirtualRobot/MathTools.h>
// #include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/utils.h>
 
// #include <armarx/control/njoint_mp_controller/task_space/aron/aron_conversions.h>
 
 
namespace armarx::control::njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointBiKAC> registrationControllerNJointBiKAC("NJointBiKAC");
 
    NJointBiKAC::NJointBiKAC(const RobotUnitPtr& robotUnit,
                             const NJointControllerConfigPtr& config,
                             const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceImpedanceController{robotUnit, config, robot}, mp::MPPool{}
    {
        mpConfig = MPListConfig();
        ARMARX_IMPORTANT << getClassName() + " construction done";
    }
 
    std::string
    NJointBiKAC::getClassName(const Ice::Current&) const
    {
        return "NJointBiKAC";
    }
 
    void
    NJointBiKAC::updateMPConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                const Ice::Current& iceCurrent)
    {
        ARMARX_IMPORTANT << "Updating MP Config";
        if (isFinished("all"))
        {
            auto configData = MPListConfig::FromAron(dto);
            isMPReady.store(false);
            while (additionalTaskRunning.load())
            {
                usleep(1000);
            }
            for (const auto& _mp : configData.mpList)
            {
                if (limb.find(_mp.nodeSetName) == limb.end())
                {
                    ARMARX_WARNING << "You have to make sure the RobotNodeSet " << _mp.nodeSetName
                                   << " in your MP config "
                                   << "corresponds to one of the RobotNodeSet in the controllers";
                    return;
                }
            }
            mpConfig = configData;
            reconfigureMPs(mpConfig);
            //            isMPReady.store(true);
 
            reInitMPInputOutputData();
            ARMARX_INFO << "-- successfully updated MP config.";
        }
        else
        {
            ARMARX_WARNING << "MP is not finished yet, cannot reset MP\n"
                              "If you want to force reset, please call \n\n"
                              "      ctrl->stop('all')    \n\n"
                              "before you continue";
        }
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointBiKAC::getMPConfig(const Ice::Current& iceCurrent)
    {
        return mpConfig.toAronDTO();
    }
 
    void
    NJointBiKAC::updateBiKACConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                   const Ice::Current& iceCurrent)
    {
        ARMARX_IMPORTANT << "---------------------------------- Updating BiKAC Config "
                            "-------------------------------------------";
        if (isFinished("all"))
        {
            auto configData = ::armarx::fromAronDict<arondto::BiKACConfig, BiKACConfig>(dto);
            isConstraintReady.store(false);
            for (const auto& _c : configData.cList)
            {
                ARMARX_INFO << "reinit mp in/output data, and checking " << _c.idx
                            << "-th constraint with type " << _c.type;
                if (limb.find(_c.rnsKpt) == limb.end())
                {
                    ARMARX_WARNING << "You have to make sure the RobotNodeSet " << _c.rnsFrame
                                   << " in your " << _c.idx
                                   << "-th constraint config for a keypoint "
                                   << "corresponds to one of the RobotNodeSet in the controllers";
                    return;
                }
 
                auto& _mp = mps.at(_c.mpName);
                if (_c.type == "pose")
                {
                    ARMARX_IMPORTANT << "constraint " << _c.idx << " is of type " << _c.type;
 
                    if (_mp.mp->getClassName() != "TSMP")
                    {
                        ARMARX_WARNING << "the pose constraint must have a TSMP";
                        return;
                    }
                    std::dynamic_pointer_cast<mp::TSMPInput>(_mp.input)->pose =
                        _c.currentKptPoseLocal;
                    std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.output)->pose =
                        _c.currentKptPoseLocal;
                    continue;
                }
                else
                {
                    ARMARX_IMPORTANT << "constraint " << _c.idx << " is of type " << _c.type;
 
                    std::dynamic_pointer_cast<mp::JSMPInput>(_mp.input)->angleRadian =
                        _c.currentKptPoseLocal.block<3, 1>(0, 3);
                    std::dynamic_pointer_cast<mp::JSMPInput>(_mp.input)->angularVel =
                        Eigen::Vector3f::Zero();
                    std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.output)->angleRadian =
                        _c.currentKptPoseLocal.block<3, 1>(0, 3);
                    std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.output)->angularVel =
                        Eigen::Vector3f::Zero();
                }
 
                if (_c.rnsFrame != "static" and limb.find(_c.rnsFrame) == limb.end())
                {
                    ARMARX_WARNING << "You have to make sure the RobotNodeSet " << _c.rnsFrame
                                   << " in your " << _c.idx << "-th constraint config for a frame "
                                   << "corresponds to one of the RobotNodeSet in the controllers";
                    return;
                }
            }
            ARMARX_INFO << "Some debug output";
            for (const auto& c : configData.cList)
            {
                ARMARX_IMPORTANT << "----------------------- c -----------------------------";
                ARMARX_INFO << VAROUT(c.idx) << VAROUT(c.type) << VAROUT(c.rnsFrame)
                            << VAROUT(c.rnsKpt);
                ARMARX_INFO << VAROUT(c.currentFrameRoot);
                ARMARX_INFO << VAROUT(c.currentKptPoseRoot);
                ARMARX_INFO << VAROUT(c.currentKptVelocityRoot);
                ARMARX_INFO << VAROUT(c.trackingForceRoot);
                ARMARX_IMPORTANT << "-------------------------------------------------------";
            }
            biKacConfig = configData;
            bufferBiKACConfig.reinitAllBuffers(biKacConfig);
            biKacConfigForDebug = biKacConfig;
            isConstraintReady.store(true);
            isMPReady.store(true);
        }
        else
        {
            ARMARX_WARNING << "MP is not finished yet, cannot reset constraints \n"
                              "If you want to force reset, please call \n\n"
                              "      ctrl->stop('all')    \n\n"
                              "before you continue";
        }
        ARMARX_IMPORTANT << "---------------------------------- BiKAC Config Updated "
                            "-------------------------------------------";
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointBiKAC::getBiKACConfig(const Ice::Current& iceCurrent)
    {
        return ::armarx::toAronDict<arondto::BiKACConfig, BiKACConfig>(biKacConfig);
    }
 
    void
    NJointBiKAC::updateConstraintStatus(const bool rtSafe)
    {
        double deltaT = 0;
        for (auto& c : biKacConfig.cList)
        {
            auto& _mp = mps.at(c.mpName);
            auto& slaveArm = limb.at(c.rnsKpt);
            slaveArm->rtStatusInNonRT = slaveArm->bufferRtToNonRt.getUpToDateReadBuffer();
            const auto currentSlaveTCPPose = slaveArm->controller.tcp->getPoseInRootFrame();
            c.currentKptPoseRoot = currentSlaveTCPPose * c.poseDiffKptLocal;
            deltaT = slaveArm->rtStatusInNonRT.deltaT;
 
            if (c.type == "pose")
            {
                // c.currentFrameRoot = itself, should not be updated
                const auto masterFrameInv = c.currentFrameRoot.inverse();
                c.currentKptPoseLocal = masterFrameInv * c.currentKptPoseRoot;
                c.currentKptVelocityRoot =
                    (1 - c.keypointVelocityFilter) * c.currentKptVelocityRoot +
                    c.keypointVelocityFilter * slaveArm->rtStatusInNonRT.currentTwist.head<3>();
                if (rtSafe)
                {
                    c.currentKptVelocityLocal.head<3>() =
                        c.currentFrameRoot.block<3, 3>(0, 0).transpose() *
                        c.currentKptVelocityRoot.head<3>();
                    c.currentKptVelocityLocal.tail<3>() =
                        c.currentFrameRoot.block<3, 3>(0, 0).transpose() *
                        c.currentKptVelocityRoot.tail<3>();
 
                    mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.input);
                    in->pose = c.currentKptPoseLocal;
                    in->vel = c.currentKptVelocityLocal;
                    in->deltaT = deltaT;
 
                    _mp.mp->run(_mp.input, _mp.output);
 
                    mp::TSMPOutputPtr out = std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.output);
                    c.targetKptPoseLocal = out->pose;
                    c.targetKptPoseRoot = c.currentFrameRoot * out->pose;
 
                    Eigen::Vector6f vel = Eigen::Vector6f::Zero();
                    vel.head<3>() = c.currentFrameRoot.block<3, 3>(0, 0) * out->vel.head<3>();
                    vel.tail<3>() = c.currentFrameRoot.block<3, 3>(0, 0) * out->vel.tail<3>();
 
                    slaveArm->nonRtData.c.desiredPose =
                        c.targetKptPoseRoot * c.poseDiffKptLocal.inverse();
                    slaveArm->nonRtData.c.desiredTwist = vel;
                    ARMARX_TRACE;
                }
            }
            else
            {
                // differently from c.type==pose, here we need to explicitly update the master's local frame
                if (c.rnsFrame != "static")
                {
                    auto& masterArm = limb.at(c.rnsFrame);
                    const auto currentMasterTCPPose =
                        masterArm->controller.tcp->getPoseInRootFrame();
                    c.currentFrameRoot = currentMasterTCPPose * c.poseDiffFrameLocal;
                }
                const auto masterFrameInv = c.currentFrameRoot.inverse();
                c.currentKptPoseLocal = masterFrameInv * c.currentKptPoseRoot;
 
                Eigen::Vector3f angular_vel = slaveArm->rtStatusInNonRT.currentTwist.tail<3>();
                Eigen::Vector3f vecRoot =
                    currentSlaveTCPPose.block<3, 3>(0, 0) * c.poseDiffKptLocal.block<3, 1>(0, 3);
                c.currentKptVelocityRoot.head<3>() =
                    (1 - c.keypointVelocityFilter) * c.currentKptVelocityRoot.head<3>() +
                    c.keypointVelocityFilter * (angular_vel.cross(vecRoot) +
                                                slaveArm->rtStatusInNonRT.currentTwist.head<3>());
                /// if the local frame is moving, this is not accurate anymore
                c.currentKptVelocityLocal.head<3>() =
                    c.currentFrameRoot.block<3, 3>(0, 0).transpose() *
                    c.currentKptVelocityRoot.head<3>();
                ARMARX_TRACE;
 
                if (rtSafe)
                {
                    mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.input);
                    in->angleRadian = c.currentKptPoseLocal.block<3, 1>(0, 3);
                    in->angularVel = c.currentKptVelocityLocal.head<3>();
                    in->deltaT = deltaT;
 
                    _mp.mp->run(_mp.input, _mp.output);
 
                    mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.output);
                    c.targetKptPoseLocal.block<3, 1>(0, 3) = out->angleRadian;
                }
 
                ARMARX_TRACE;
                Eigen::Vector3f modified_target = c.targetKptPoseLocal.block<3, 1>(0, 3);
                if (c.p2pIdx >= 0 and c.type != "p2p")
                {
                    const auto& p2p = biKacConfig.cList.at(c.p2pIdx);
                    Eigen::Vector3f p2p_local =
                        (masterFrameInv * p2p.currentKptPoseRoot).block<3, 1>(0, 3);
                    const auto x_ = c.targetKptPoseLocal.block<3, 1>(0, 3);
                    float radius = (c.currentKptPoseLocal.block<3, 1>(0, 3) - p2p_local).norm();
                    // modified_target = (x_ - p2p_local).normalized() * radius + p2p_local;
                    if (c.type == "p2l")
                    {
                        Eigen::Vector3f line_vec = c.pcaComponentsLocal.row(0);
 
                        auto vec_mean_to_p2p = p2p_local - c.pcaMeanLocal;
                        float proj_p2p = vec_mean_to_p2p.dot(line_vec);
                        auto proj_point_p2p = c.pcaMeanLocal + line_vec * proj_p2p;
 
                        Eigen::Vector3f line_norm_to_p2p =
                            (p2p_local - proj_point_p2p).normalized();
                        float proj_x_ = (x_ - p2p_local).dot(line_norm_to_p2p);
                        Eigen::Vector3f proj_point = p2p_local + proj_x_ * line_norm_to_p2p;
 
                        Eigen::Vector3f radial_vec = (x_ - proj_point).normalized();
                        proj_x_ = std::min(std::max(proj_x_, -radius), radius);
                        proj_point = p2p_local + proj_x_ * line_norm_to_p2p;
                        modified_target =
                            sqrt(radius * radius - proj_x_ * proj_x_) * radial_vec + proj_point;
                    }
                    else if (c.type == "p2P")
                    {
                        Eigen::Vector3f plane_norm_vec = c.pcaComponentsLocal.row(2);
                        float proj_x_ = (x_ - p2p_local).dot(plane_norm_vec);
                        Eigen::Vector3f proj_point = p2p_local + proj_x_ * plane_norm_vec;
 
                        Eigen::Vector3f radial_vec = (x_ - proj_point).normalized();
                        proj_x_ = std::min(std::max(proj_x_, -radius), radius);
                        proj_point = p2p_local + proj_x_ * plane_norm_vec;
                        modified_target =
                            sqrt(radius * radius - proj_x_ * proj_x_) * radial_vec + proj_point;
                    }
                }
                c.targetKptPoseLocal.block<3, 1>(0, 3) = modified_target;
                c.targetKptPoseRoot = c.currentFrameRoot * c.targetKptPoseLocal;
                ARMARX_TRACE;
                Eigen::Vector3f kpt_error =
                    modified_target - c.currentKptPoseLocal.block<3, 1>(0, 3);
                /// the force is in Newton since the error is in mm and the kpt_kp is scaled 1000 times smaller
                c.trackingForceLocal =
                    (1 - c.keypointForceFilter) * c.trackingForceLocal +
                    c.keypointForceFilter *
                        (c.kptKp.cwiseProduct(kpt_error) -
                         c.kptKd.cwiseProduct(c.currentKptVelocityLocal.head<3>()));
                c.trackingForceRoot = c.currentFrameRoot.block<3, 3>(0, 0) * c.trackingForceLocal;
                ARMARX_TRACE;
            }
        }
 
        for (auto& pair : biKacConfig.admittanceData)
        {
            auto& force_torque = pair.second.forceTorque;
            Eigen::Vector6f force_torque_current;
            force_torque_current.setZero();
            Eigen::Vector3f meanKeypointPositionRoot = Eigen::Vector3f::Zero();
            for (auto i : pair.second.constraintIdxList)
            {
                meanKeypointPositionRoot =
                    meanKeypointPositionRoot +
                    biKacConfig.cList.at(i).currentKptPoseRoot.block<3, 1>(0, 3);
            }
            meanKeypointPositionRoot =
                meanKeypointPositionRoot / pair.second.constraintIdxList.size();
 
            for (auto i : pair.second.constraintIdxList)
            {
                Eigen::Vector3f devi =
                    biKacConfig.cList.at(i).currentKptPoseRoot.block<3, 1>(0, 3) -
                    meanKeypointPositionRoot;
                devi = devi * 0.001;
                auto& force = biKacConfig.cList.at(i).trackingForceRoot;
                if (force.norm() > 100.0)
                {
                    ARMARX_RT_LOGF_WARN("force too large, set to zero");
                    force.setZero();
                }
                force_torque_current.head<3>() += force;
                force_torque_current.tail<3>() += devi.cross(force);
            }
            force_torque = 0.1 * force_torque + 0.9 * force_torque_current;
            ARMARX_TRACE;
 
            Eigen::Vector6f acc = biKacConfig.kmAdmittance.cwiseProduct(force_torque) -
                                  biKacConfig.kdAdmittance.cwiseProduct(pair.second.desiredVel);
 
            Eigen::Vector6f vel = pair.second.desiredVel +
                                  0.5 * static_cast<float>(deltaT) * (acc + pair.second.desiredAcc);
            Eigen::VectorXf deltaPose =
                0.5 * static_cast<float>(deltaT) * (vel + pair.second.desiredVel);
            pair.second.desiredAcc = acc;
            pair.second.desiredVel = vel;
 
            Eigen::Matrix3f deltaPoseMat =
                VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
            pair.second.desiredPose.block<3, 1>(0, 3) += deltaPose.head<3>();
            pair.second.desiredPose.block<3, 3>(0, 0) =
                deltaPoseMat * pair.second.desiredPose.block<3, 3>(0, 0);
 
            ARMARX_TRACE;
            auto& slaveArm = limb.at(pair.first);
            slaveArm->nonRtData.c.desiredPose = pair.second.desiredPose;
            slaveArm->nonRtData.c.desiredTwist = pair.second.desiredVel;
        }
        bufferBiKACConfig.getWriteBuffer() = biKacConfig;
        bufferBiKACConfig.commitWrite();
    }
 
    void
    NJointBiKAC::additionalTask()
    {
        bool rtSafe = true;
        for (auto& pair : limb)
        {
            rtSafe = rtSafe and pair.second->bufferRtToNonRt.getUpToDateReadBuffer().rtSafe;
        }
 
        robotUnit->updateVirtualRobot(nonRtRobot);
        if (isConstraintReady.load() and isMPReady.load())
        {
            updateConstraintStatus(rtSafe);
        }
        for (auto& pair : limb)
        {
            limbNonRT(pair.second);
        }
    }
 
    void
    NJointBiKAC::limbNonRT(ArmPtr& arm)
    {
        arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
        arm->bufferConfigNonRtToRt.commitWrite();
    }
 
    // void
    // NJointBiKAC::reInitMPInputOutputData()
    // {
    //     ARMARX_IMPORTANT << "reconfigure MP: reinitialize the mp input output, as well as the rt related buffer values";
    //     for (auto& _mp: mps)
    //     {
    //         const auto& nodeSetName = _mp.second.mp->getNodeSetName();
    //         ARMARX_CHECK_EQUAL(nodeSetName, limb.at(nodeSetName)->kinematicChainName);
    //         VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(nodeSetName);
    //         Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
    //
    //         if (_mp.second.mp->getClassName() == "TSMP")
    //         {
    //             ARMARX_IMPORTANT << "init input output data for " << _mp.second.mp->getMPName();
    //             std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
    //             std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
    //         }
    //         else if (_mp.second.mp->getClassName() == "JSMP")
    //         {
    //             ARMARX_IMPORTANT << "init input output data for " << _mp.second.mp->getMPName();
    //             std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian = rns->getJointValuesEigen();
    //             std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel = Eigen::VectorXf::Zero(rns->getJointValues().size());
    //             std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian = rns->getJointValuesEigen();
    //             std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel = Eigen::VectorXf::Zero(rns->getJointValues().size());
    //         }
    //     }
    // }
 
    void
    NJointBiKAC::onPublish(const SensorAndControl&,
                           const DebugDrawerInterfacePrx&,
                           const DebugObserverInterfacePrx& debugObs)
    {
        for (auto& pair : limb)
        {
            limbPublish(pair.second, debugObs);
        }
 
        StringVariantBaseMap datafields;
        biKacConfigForDebug = bufferBiKACConfig.getUpToDateReadBuffer();
        for (const auto& c : biKacConfigForDebug.cList)
        {
            const std::string idx = std::to_string(c.idx);
            common::debugEigenPose(datafields, "c_frame_root_" + idx, c.currentFrameRoot);
            common::debugEigenVec(
                datafields, "c_kpt_posi_root_" + idx, c.currentKptPoseRoot.block<3, 1>(0, 3));
            common::debugEigenVec(
                datafields, "c_kpt_posi_local_" + idx, c.currentKptPoseLocal.block<3, 1>(0, 3));
            common::debugEigenVec(
                datafields, "t_kpt_posi_root_" + idx, c.targetKptPoseRoot.block<3, 1>(0, 3));
            common::debugEigenVec(
                datafields, "t_kpt_posi_local_" + idx, c.targetKptPoseLocal.block<3, 1>(0, 3));
            common::debugEigenVec(datafields, "c_force_root" + idx, c.trackingForceRoot);
            common::debugEigenVec(datafields, "c_force_local" + idx, c.trackingForceLocal);
            // ARMARX_IMPORTANT << "----------------------- c -----------------------------";
            // ARMARX_INFO << VAROUT(c.idx) << VAROUT(c.type) << VAROUT(c.rnsFrame) << VAROUT(c.rnsKpt);
            // ARMARX_INFO << VAROUT(c.currentFrameRoot);
            // ARMARX_INFO << VAROUT(c.currentKptPoseRoot);
            // ARMARX_INFO << VAROUT(c.currentKptVelocityRoot);
            // ARMARX_INFO << VAROUT(c.trackingForce);
            // ARMARX_IMPORTANT << "-------------------------------------------------------";
        }
        for (const auto& pair : biKacConfigForDebug.admittanceData)
        {
            common::debugEigenVec(datafields, "ft_" + pair.first, pair.second.forceTorque);
            common::debugEigenPose(datafields, "virt_pose_" + pair.first, pair.second.desiredPose);
            common::debugEigenVec(datafields, "virt_vel_" + pair.first, pair.second.desiredVel);
        }
 
        debugObs->setDebugChannel("BiKAC", datafields);
    }
} // namespace armarx::control::njoint_mp_controller::task_space