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    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 "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::fromAron<AronDTO, BO>(cfg->config);
        validateConfigData(configData);
        bufferUserToAdditionalTask.reinitAllBuffers(configData);
        bufferUserToRt.reinitAllBuffers(configData);
        ARMARX_IMPORTANT << VAROUT(configData.mpConfig.mpList.size());
        createMPs(configData.mpConfig);
 
//        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()){
        const auto& cfg = bufferUserToAdditionalTask.getUpToDateReadBuffer();
        reconfigureMPs(cfg.mpConfig);
//        configureConstraints(cfg);
    }
}
 
//void NJointKVILImpedanceMPController::configureConstraints(const std::vector<Constraint>& c)
//{
//    n_constraints = c.numPoints;
//    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