KVILImpedanceController.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    ...
 * @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 "KVILImpedanceController.h"
 
#include <armarx/control/common/math.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<NJointKVILImpedanceMPController>
        registrationControllerNJointKVILImpedanceMPController("NJointKVILImpedanceMPController");
 
    NJointKVILImpedanceMPController::NJointKVILImpedanceMPController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointKeypointsImpedanceController{robotUnit, config, robot}, mp::MPPool{}
    {
        {
            ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
            auto configData = ::armarx::fromAronDict<AronDTO, BO>(cfg->config);
            validateConfigData(configData);
            bufferUserToAdditionalTask.reinitAllBuffers(configData);
            bufferAdditionalTaskToUser.reinitAllBuffers(configData);
            bufferUserToRt.reinitAllBuffers(configData);
            ARMARX_IMPORTANT << VAROUT(configData.mpList.size());
            createMPs(configData.mpList);
 
            //        configureConstraints(configData.constraintList);
        }
        ARMARX_IMPORTANT << "NJointTSImpedanceMPController construction done";
        //    const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
        //    createMPs(cfg.mpConfig);
        //    configureConstraints(cfg);
    }
 
    std::string
    NJointKVILImpedanceMPController::getClassName(const Ice::Current&) const
    {
        return "NJointKVILImpedanceMPController";
    }
 
    //void NJointKVILImpedanceMPController::reconfigureController(const std::string &filename, const Ice::Current &)
    //{
    //    NJointKeypointsImpedanceController::reconfigureController(filename);
    //    reconfigureMPs(filename);
    //}
 
    void
    NJointKVILImpedanceMPController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                                  const Ice::Current& iceCurrent)
    {
        NJointKeypointsImpedanceController::updateConfig(dto);
        if (isFinished("all"))
        {
            isMPReady.store(false);
            const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
            reconfigureMPs(cfg.mpList);
            //        configureConstraints(cfg);
            isMPReady.store(true);
        }
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointKVILImpedanceMPController::getConfig(const Ice::Current& iceCurrent)
    {
        return ::armarx::toAronDict<AronDTO, BO>(
            bufferAdditionalTaskToUser.getUpToDateReadBuffer());
    }
 
    void
    NJointKVILImpedanceMPController::configureConstraints(const ConstraintList& c)
    {
        //    n_constraints = c.size();
        //    for (int i = 0; i < n_constraints; i ++)
        //    {
        //        std::string name = "mp_"+std::to_string(i);
        //        auto c = mp_cfg[name];
        //        ConstraintPtr constraint(new Constraint());
        //        constraint->type = c["constraint_type"];
        //        ARMARX_INFO << VAROUT(constraint->type);
        //
        //        constraint->localFrameOrigin = c["local_frame_origin_obs"];
        //        ARMARX_CHECK_EQUAL(constraint->localFrameOrigin.size(), 3);
        //        ARMARX_INFO << VAROUT(constraint->localFrameOrigin);
        //
        //        constraint->localFrameRotMat = c["local_frame_rot_mat_obs"];
        //        ARMARX_CHECK_EQUAL(constraint->localFrameRotMat.cols(), 3);
        //        ARMARX_CHECK_EQUAL(constraint->localFrameRotMat.rows(), 3);
        //        ARMARX_INFO << VAROUT(constraint->localFrameRotMat);
        //
        //        constraint->vmpStart = common::getEigenVec(c, c, "vmp_start");
        //        ARMARX_INFO << VAROUT(constraint->vmpStart);
        //        if (c["manifold"] == "linear")
        //        {
        //            constraint->pcaMean = c["pca_mean"];
        //            constraint->pcaComponents = c["pca_components"];
        //            ARMARX_CHECK_EQUAL(constraint->pcaMean.size(), 3);
        //            ARMARX_CHECK_EQUAL(constraint->pcaComponents.cols(), 3);
        //            ARMARX_CHECK_EQUAL(constraint->pcaComponents.rows(), 3);
        //            ARMARX_IMPORTANT << VAROUT(constraint->pcaMean) << VAROUT(constraint->pcaComponents);
        //        }
        //        else
        //        {
        //            constraint->pcaMean.setZero();
        //            constraint->pcaComponents.setZero();
        //        }
        //        if (constraint->type == "p2c")
        //        {
        //            //            constraint->pcaMean = c["pca_mean"];
        //            constraint->pm_scale = c["pm_scale"];
        //            constraint->pm_init_proj_point  = common::getEigenVec(c, c, "pm_init_proj_point");
        //            constraint->pm_init_tangent_vec = common::getEigenVec(c, c, "pm_init_tangent_vec");
        //            constraint->pm_init_proj_value  = c["pm_init_proj_value"];
        //
        //            ARMARX_INFO << VAROUT(constraint->pm_init_proj_point)
        //                        << VAROUT(constraint->pm_init_tangent_vec)
        //                        << VAROUT(constraint->pm_init_proj_value);
        //
        //            NonlinearManifoldInfoStruct curve_info;
        //            curve_info.pm_proj_point = constraint->pm_init_proj_point;
        //            curve_info.pm_proj_point_to_mean_distance = constraint->pm_init_proj_value;
        //            curve_info.pm_tangent_vec = constraint->pm_init_tangent_vec;
        //            nl_mani_buffer.reinitAllBuffers(curve_info);
        //
        //            p2CStruct p2c_data;
        //            p2c_data.curve_kpt_position = common::getEigenVec(c, c, "pm_imit_kpts_position_local");
        //            kpt_status_buffer.reinitAllBuffers(p2c_data);
        //
        //            //            ARMARX_CHECK_EQUAL(constraint->pcaMean.size(), 3);
        //            //            ARMARX_IMPORTANT << VAROUT(constraint->pcaMean);
        //        }
        //        if (constraint->type == "p2l" or constraint->type == "p2P" or constraint->type == "p2c")
        //        {
        //            constraint->priority = c["priority"];
        //            constraint->density_std_dev = c["density_std_dev"];
        //            constraint->density_weights = getEigenVec(c, c, "density_weights");
        //            ARMARX_IMPORTANT << VAROUT(constraint->priority) << VAROUT(constraint->density_std_dev) << VAROUT(constraint->density_weights);
        //            getEigenMatrix(c, "density_mu", constraint->density_mu);
        //            ARMARX_IMPORTANT << VAROUT(constraint->density_mu);
        //            ARMARX_CHECK_EQUAL(constraint->density_mu.rows(), constraint->density_weights.size());
        //            //            ARMARX_CHECK_EQUAL(constraint->density_mu.cols(), 1);
        //        }
        //        else
        //        {
        //            constraint->density_mu = Eigen::Matrix1f::Zero();
        //            constraint->density_weights.setZero(0);
        //        }
        //        constraint->spatial_scale = c["spatial_scale"];
        //        constraints.insert({name, constraint});
        //        ARMARX_INFO << " -- added constraint: " << name;
        //    }
        //    ARMARX_IMPORTANT << "--------------------------------------- Done! ---------------------------------------";
    }
 
    void
    NJointKVILImpedanceMPController::setCurveProjection(const Eigen::Vector3f& curve_proj_point_,
                                                        float proj_value_,
                                                        const Eigen::Vector3f& curve_vec_,
                                                        const Ice::Current&)
    {
        nl_mani_buffer.getWriteBuffer().pm_proj_point = curve_proj_point_;
        nl_mani_buffer.getWriteBuffer().pm_proj_point_to_mean_distance = proj_value_;
        nl_mani_buffer.getWriteBuffer().pm_tangent_vec = curve_vec_;
        nl_mani_buffer.commitWrite();
        //    curve_proj_point = curve_proj_point_;
        //    curve_proj_point_to_mean_distance = proj_value_;
        //    vec_to_min_on_curve = curve_vec_;
        //    curve_set.store(true);
    }
 
    Eigen::Vector3f
    NJointKVILImpedanceMPController::getCurveKeypointPosition(const Ice::Current&)
    {
        return kpt_status_buffer.getUpToDateReadBuffer().curve_kpt_position;
    }
 
    void
    NJointKVILImpedanceMPController::additionalTask()
    {
        //    Eigen::VectorXf kptsPosition = controlStatusBuffer.getReadBuffer().filteredKeypointPosition;
        //    Eigen::VectorXf kptsVelocity = controlStatusBuffer.getReadBuffer().currentKeypointVelocity;
        //    double deltaT = controlStatusBuffer.getReadBuffer().deltaT;
        //    ARMARX_CHECK_EQUAL(kptsPosition.size(), n_constraints * 3);
        //    ARMARX_CHECK_EQUAL(kptsVelocity.size(), n_constraints * 3);
        //
        //    /// parse arguments and run mp
        ////    ARMARX_INFO << VAROUT(controller.s.numPoints) << VAROUT(kptsPosition) << VAROUT(kptsVelocity) << VAROUT(deltaT);
        //    for (int i = 0; i < n_constraints; i++)
        //    {
        //        std::string name = "mp_"+std::to_string(i);
        //        mp::KeypointsMPInputPtr in = std::dynamic_pointer_cast<mp::KeypointsMPInput>(mps[name].input);
        //
        //        auto c = constraints[name];
        //        Eigen::Vector3f kptPositionLocal = c->localFrameRotMat.transpose() * (kptsPosition.segment(3*i, 3) - c->localFrameOrigin);
        //        Eigen::Vector3f kptVelocityLocal = c->localFrameRotMat.transpose() * kptsVelocity.segment(3*i, 3);
        //        if (c->type == "p2p"/* or c->type == "p2c"*/)
        //        {
        //            in->keypointPosition = kptPositionLocal;
        //            in->keypointVelocity = kptVelocityLocal;
        //        }
        //
        //
        //        else if (c->type == "p2c")
        //        {
        //            kpt_status_buffer.getWriteBuffer().curve_kpt_position = kptPositionLocal;
        //            kpt_status_buffer.commitWrite();
        //
        //            Eigen::Vector3f devi_vec = kptPositionLocal - nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point;
        //            float dist = devi_vec.norm();
        //            devi_vec /= dist;
        //            float vel_proj = kptVelocityLocal.dot(devi_vec);
        //            in->keypointPosition(0) = dist; // c->pcaComponents.row(0).cross(kptPositionLocal - c->pcaMean).norm();
        //            in->keypointVelocity(0) = vel_proj;
        //        }
        //
        //
        //
        //        else if (c->type == "p2l")
        //        {
        //            Eigen::Vector3f line_vec = c->pcaComponents.row(0);
        //            float proj_value = line_vec.dot(kptPositionLocal - c->pcaMean);
        //            Eigen::Vector3f proj_point = c->pcaMean + proj_value * line_vec;
        //            Eigen::Vector3f devi_vec = kptPositionLocal - proj_point;
        //            float dist = devi_vec.norm();
        //            devi_vec /= dist;
        //            float vel_proj = kptVelocityLocal.dot(devi_vec);
        //            in->keypointPosition(0) = dist; // c->pcaComponents.row(0).cross(kptPositionLocal - c->pcaMean).norm();
        //            in->keypointVelocity(0) = vel_proj;
        //        }
        //        else if (c->type == "p2P")
        //        {
        //            Eigen::Vector3f plane_norm_vec = c->pcaComponents.row(2);
        //            in->keypointPosition(0) = plane_norm_vec.dot(kptPositionLocal - c->pcaMean);
        //            in->keypointVelocity(0) = plane_norm_vec.dot(kptVelocityLocal);
        //        }
        //        in->deltaT = deltaT;
        //    }
        //    {
        //        // null space mp
        //        mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(mps["null"].input);
        //        in->angleRadian = controlStatusBuffer.getReadBuffer().qpos;
        //        in->angularVel  = controlStatusBuffer.getReadBuffer().qvel;
        //        in->deltaT      = controlStatusBuffer.getReadBuffer().deltaT;
        //    }
        //
        //    runMPs();
        //
        //    Eigen::VectorXf targetKeypointPosition;
        //    Eigen::VectorXf targetKeypointVelocity;
        //    Eigen::VectorXf targetDensityForce;
        //    targetKeypointPosition.setZero(kptsPosition.size());
        //    targetKeypointVelocity.setZero(kptsPosition.size());
        //    targetDensityForce.setZero(kptsPosition.size());
        //    /// parse mp target
        //    for (int i = 0; i < n_constraints; i++)
        //    {
        //        std::string name = "mp_"+std::to_string(i);
        //        mp::KeypointsMPOutputPtr out = std::dynamic_pointer_cast<mp::KeypointsMPOutput>(mps[name].output);
        //
        //        auto c = constraints[name];
        //        Eigen::Vector3f kptPositionLocal = c->localFrameRotMat.transpose() * (kptsPosition.segment(3*i, 3) - c->localFrameOrigin);
        //
        //        Eigen::Vector3f targetPosition;
        //        Eigen::Vector3f targetVelocity;
        //        Eigen::Vector3f density_force = Eigen::Vector3f::Zero();
        //
        //        if (c->type == "p2p"/* or c->type == "p2c"*/)
        //        {
        //            targetPosition = out->keypointPosition;
        //            targetVelocity = out->keypointVelocity;
        //        }
        //
        //
        //        else if (c->type == "p2c")
        //        {
        ////            LockGuardType guard {curveDataMutex};
        ////            if (curve_set.load())
        ////            {
        //
        //            Eigen::Vector3f curve_proj_point = nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point;
        //            Eigen::Vector3f curve_tangent_vec = nl_mani_buffer.getUpToDateReadBuffer().pm_tangent_vec;
        //            float curve_proj_point_to_mean_distance = nl_mani_buffer.getUpToDateReadBuffer().pm_proj_point_to_mean_distance;
        //
        //            Eigen::Vector3f devi_vec = kptPositionLocal - curve_proj_point;
        //            float dist = devi_vec.norm();
        //            devi_vec /= dist;
        //
        //            /// density force
        //            //            float kp = 0.0f;
        //            //            if (curve_proj_point_to_mean_distance > 100.0f)
        //            //            {
        //            //                kp = std::signbit(curve_proj_point_to_mean_distance) ? -1 : 1 * 100.0f;
        //            //            }
        //            //            else
        //            //            {
        //            //                kp = curve_proj_point_to_mean_distance;
        //            //            }
        //            Eigen::Vector3f base_force = - curve_tangent_vec * curve_proj_point_to_mean_distance * 0.0000015f;
        //            base_force = c->localFrameRotMat * base_force;
        //
        //            Eigen::Vector1f prob_derivative = Eigen::Vector1f::Zero();
        //            Eigen::Vector1f data {curve_proj_point_to_mean_distance};
        //            for (int i = 0; i < c->density_mu.rows(); i++)
        //            {
        //                prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
        //            }
        //            density_force = prob_derivative(0) * curve_tangent_vec * 10.0f;
        //            density_force = c->localFrameRotMat * density_force;
        //
        //            if (base_force.norm() > density_force.norm())
        //            {
        //                ARMARX_RT_LOGF_INFO("use base force");
        //                density_force = base_force;
        //            }
        //
        //            /// priority
        //            if (c->priority > 0)
        //            {
        //                if (constraints["mp_0"]->type == "p2p")
        //                {
        //                    /// already in root frame
        //                    Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
        //                    Eigen::Vector3f line_vec_root = c->localFrameRotMat * curve_tangent_vec;
        //                    Eigen::Vector3f tang_vec = devi.cross(line_vec_root).cross(devi);
        //                    tang_vec.normalize();
        //                    float force_proj = density_force.dot(tang_vec);
        //                    density_force = force_proj * tang_vec;
        //                }
        //            }
        //
        //            targetPosition = curve_proj_point + devi_vec * out->keypointPosition(0);
        //            targetVelocity = devi_vec * out->keypointVelocity(0);
        //
        //            //            }
        //            //            else
        //            //            {
        //            //                targetPosition = kptsPosition.segment(3*i, 3);
        //            //                targetVelocity.setZero();
        //            //            }
        //        }
        //
        //
        //        else if (c->type == "p2l")
        //        {
        //            Eigen::Vector3f line_vec = c->pcaComponents.row(0);
        //            float proj_value = line_vec.dot(kptPositionLocal - c->pcaMean);
        //            Eigen::Vector3f proj_point = c->pcaMean + proj_value * line_vec;
        //            Eigen::Vector3f devi_vec = kptPositionLocal - proj_point;
        //            float dist = devi_vec.norm();
        //            devi_vec /= dist;
        //
        //            /// density force
        //            float kp = 0.0f;
        //            if (abs(proj_value) > 100.0f)
        //            {
        //                kp = std::signbit(proj_value) ? -1 : 1 * 100.0f;
        //            }
        //            else
        //            {
        //                kp = proj_value;
        //            }
        //            Eigen::Vector3f base_force = - c->pcaComponents.row(0) * proj_value * 0.0000015f;
        //            base_force = c->localFrameRotMat * base_force;
        //
        //            Eigen::Vector1f prob_derivative = Eigen::Vector1f::Zero();
        //            Eigen::Vector1f data {proj_value};
        //            for (int i = 0; i < c->density_mu.rows(); i++)
        //            {
        //                prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
        //            }
        //            density_force = prob_derivative(0) * c->pcaComponents.row(0) * 10.0f;
        //            density_force = c->localFrameRotMat * density_force;
        //
        //            if (base_force.norm() > density_force.norm())
        //            {
        //                ARMARX_RT_LOGF_INFO("use base force");
        //                density_force = base_force;
        //            }
        //
        //            /// priority
        //            if (c->priority > 0)
        //            {
        //                if (constraints["mp_0"]->type == "p2p")
        //                {
        //                    /// already in root frame
        //                    Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
        //                    Eigen::Vector3f line_vec_local = c->pcaComponents.row(0);
        //                    Eigen::Vector3f line_vec_root = c->localFrameRotMat * line_vec_local;
        //                    Eigen::Vector3f tang_vec = devi.cross(line_vec_root).cross(devi);
        //                    tang_vec.normalize();
        //                    float force_proj = density_force.dot(tang_vec);
        //                    density_force = force_proj * tang_vec;
        //                }
        //            }
        //
        //            targetPosition = proj_point + devi_vec * out->keypointPosition(0);
        //            targetVelocity = devi_vec * out->keypointVelocity(0);
        //        }
        //        else if (c->type == "p2P")
        //        {
        //            Eigen::Vector3f proj_value_vec = c->pcaComponents * (kptPositionLocal - c->pcaMean);
        //            Eigen::Vector3f plane_norm_vec = c->pcaComponents.row(2);
        //            Eigen::Vector3f proj_point = kptPositionLocal - plane_norm_vec * proj_value_vec(2);
        //
        //            /// density force
        //            Eigen::Vector2f proj_value_on_plane = proj_value_vec.head<2>();
        //            proj_value_on_plane.normalize();
        //
        //            Eigen::Vector3f base_force = - c->pcaComponents.row(0) * proj_value_on_plane(0) - c->pcaComponents.row(1) * proj_value_on_plane(1);
        //            base_force = c->localFrameRotMat * base_force;
        //
        //            Eigen::Vector2f prob_derivative = Eigen::Vector2f::Zero();
        //            Eigen::Vector2f data = proj_value_vec.head<2>();
        //            for (int i = 0; i < c->density_mu.rows(); i++)
        //            {
        //                prob_derivative = prob_derivative + pdf_gradient(c->density_mu.row(i), c->density_std_dev, data);
        //            }
        //            density_force = (prob_derivative(0) * c->pcaComponents.row(0) + prob_derivative(1) * c->pcaComponents.row(1)) * 10.0f;
        //            density_force = c->localFrameRotMat * density_force;
        //
        //            if (base_force.norm() > density_force.norm())
        //            {
        //                ARMARX_RT_LOGF_INFO("use base force");
        //                density_force = base_force;
        //            }
        //
        //            /// handle priority
        //            if (c->priority > 0)
        //            {
        //                if (constraints["mp_0"]->type == "p2p")
        //                {
        //                    Eigen::Vector3f devi = kptsPosition.segment(3*i, 3) - kptsPosition.segment(0, 3);
        //                    Eigen::Vector3f plane_norm_vec_root = c->localFrameRotMat * plane_norm_vec;
        //                    Eigen::Vector3f tang_vec = devi.cross(plane_norm_vec_root);
        //                    tang_vec.normalize();
        //                    float force_proj = density_force.dot(tang_vec);
        //                    density_force = force_proj * tang_vec;
        //                }
        //            }
        //
        //
        //            targetPosition = proj_point + plane_norm_vec * out->keypointPosition(0);
        //            targetVelocity = out->keypointVelocity(0) * plane_norm_vec;
        //        }
        //        targetKeypointPosition.segment(3*i, 3) = c->localFrameRotMat * targetPosition + c->localFrameOrigin;
        //        targetKeypointVelocity.segment(3*i, 3) = c->localFrameRotMat * targetVelocity;
        //        targetDensityForce.segment(3*i, 3) = density_force;
        //    }
        //    ARMARX_CHECK_EQUAL(targetKeypointPosition.size(), n_constraints * 3);
        //    ARMARX_CHECK_EQUAL(targetKeypointVelocity.size(), n_constraints * 3);
        //    ARMARX_CHECK_EQUAL(targetDensityForce.size(), n_constraints * 3);
        //    getWriterControlStruct().desiredKeypointPosition = targetKeypointPosition;
        //    getWriterControlStruct().desiredKeypointVelocity = targetKeypointVelocity;
        //    getWriterControlStruct().desiredDensityForce = targetDensityForce;
        //    {
        //        mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(mps["null"].output);
        //        getWriterControlStruct().desiredNullspaceJointAngles = out->angleRadian;
        //    }
        //    writeControlStruct();
    }
 
    void
    NJointKVILImpedanceMPController::rtPreActivateController()
    {
        //    ARMARX_IMPORTANT << "rt pre activate: reinitialize the mp input output, as well as the rt related buffer values";
        //    NJointKeypointsImpedanceController::rtPreActivateController();
        //    for (int i = 0; i < controller.s.numPoints; i++)
        //    {
        //        std::string name = "mp_"+std::to_string(i);
        //
        //        ARMARX_INFO << "initialize " << name;
        //
        //        auto in = std::dynamic_pointer_cast<mp::KeypointsMPInput>(mps[name].input);
        //        auto out = std::dynamic_pointer_cast<mp::KeypointsMPOutput>(mps[name].output);
        //        auto c = constraints[name];
        //
        //        in->keypointPosition = c->vmpStart;
        //        out->keypointPosition = c->vmpStart;
        //
        //        if (c->type == "p2p")
        //        {
        //            in->keypointVelocity = controller.s.currentKeypointVelocity;
        //            out->keypointVelocity = controller.s.currentKeypointVelocity;
        //        }
        //        else if (c->type == "p2l")
        //        {
        //            in->keypointVelocity = Eigen::Vector1f::Zero();
        //            out->keypointVelocity = Eigen::Vector1f::Zero();
        //        }
        //        else if (c->type == "p2P")
        //        {
        //            in->keypointVelocity = Eigen::Vector1f::Zero();
        //            out->keypointVelocity = Eigen::Vector1f::Zero();
        //        }
        //        else if (c->type == "p2c")
        //        {
        //            in->keypointVelocity = Eigen::Vector1f::Zero();
        //            out->keypointVelocity = Eigen::Vector1f::Zero();
        //        }
        //    }
        //    {
        //        ARMARX_IMPORTANT << "initialize input output buffers between nullspace mp";
        //        auto in  = std::dynamic_pointer_cast<mp::JSMPInput>(mps["null"].input);
        //        auto out = std::dynamic_pointer_cast<mp::JSMPOutput>(mps["null"].output);
        //        in->angleRadian  = controller.s.qpos;
        //        in->angularVel   = controller.s.qvel;
        //        in->deltaT = 0.0;
        //        out->angleRadian = controller.s.qpos;
        //        out->angularVel  = controller.s.qvel;
        //    }
    }
} // namespace armarx::control::njoint_mp_controller::task_space