TaskSpaceVMP.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    RobotAPI::ArmarXObjects::DMPController
 * @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 "TaskSpaceVMP.h"
 
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/core/Trajectory.h>
 
#include <dmp/representation/dmp/umitsmp.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::tsvmp
{
 
    void
    TaskSpaceDMPController::flow(double deltaT,
                                 const Eigen::Matrix4f& currentPose,
                                 const Eigen::VectorXf& twist)
    {
        canVal = flow(canVal, deltaT, currentPose, twist);
    }
 
    double
    TaskSpaceDMPController::flow(double canVal,
                                 double deltaT,
                                 const Eigen::Matrix4f& currentPose,
                                 const Eigen::VectorXf& twist)
    {
        if (paused)
        {
            targetVel.setZero();
            return canVal;
        }
        if (canVal < 0.1 && config.DMPStyle == "Periodic")
        {
            canVal = config.motionTimeDuration;
        }
        if (canVal < 1e-8 && config.DMPStyle == "Discrete")
        {
            targetVel.setZero();
            return canVal;
        }
 
        Eigen::Vector3f currentPosition;
        Eigen::Quaterniond currentOrientation;
        double posiError = 0;
        double oriError = 0;
 
        getError(currentPose, currentPosition, currentOrientation, posiError, oriError);
 
        double poseError = posiError + config.phaseStopParas.mm2radi * oriError;
 
        double phaseDist;
        if (isDisturbance)
        {
            phaseDist = config.phaseStopParas.backDist;
        }
        else
        {
            phaseDist = config.phaseStopParas.goDist;
        }
        double phaseL = config.phaseStopParas.maxValue;
        double phaseK = config.phaseStopParas.slop;
 
        double phaseStop = phaseL / (1 + exp(-phaseK * (poseError - phaseDist)));
        double mpcFactor = 1 - (phaseStop / phaseL);
 
        if (mpcFactor < 0.1)
        {
            isDisturbance = true;
        }
 
        if (mpcFactor > 0.9)
        {
            isDisturbance = false;
        }
 
        double timeDuration = config.motionTimeDuration;
        canVal -= tau * deltaT * 1; // / (1 + phaseStop) ;
 
 
        DMP::Vec<DMP::DMPState> temporalState = dmpPtr->calculateDirectlyVelocity(
            currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
 
        // scale translation velocity
        for (size_t i = 0; i < 3; ++i)
        {
            currentState[i].vel = tau * temporalState[i].vel * config.DMPAmplitude / timeDuration;
            currentState[i].pos += deltaT * currentState[i].vel;
        }
 
        // define the translation velocity
        if (isPhaseStopControl)
        {
            float vel0, vel1;
 
            Eigen::Vector3f linearVel;
            linearVel << twist(0), twist(1), twist(2);
            for (size_t i = 0; i < 3; i++)
            {
                vel0 = currentState[i].vel;
                vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) -
                       config.phaseStopParas.Dpos * linearVel(i);
                targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
            }
        }
        else
        {
            for (size_t i = 0; i < 3; i++)
            {
                targetVel(i) = currentState[i].vel;
            }
        }
 
 
        // define the rotation velocity
        Eigen::Quaterniond dmpQuaternionVel;
        dmpQuaternionVel.w() = temporalState[3].vel;
        dmpQuaternionVel.x() = temporalState[4].vel;
        dmpQuaternionVel.y() = temporalState[5].vel;
        dmpQuaternionVel.z() = temporalState[6].vel;
 
        Eigen::Quaterniond dmpQuaternionPosi;
        dmpQuaternionPosi.w() = currentState[3].pos;
        dmpQuaternionPosi.x() = currentState[4].pos;
        dmpQuaternionPosi.y() = currentState[5].pos;
        dmpQuaternionPosi.z() = currentState[6].pos;
 
 
        Eigen::Quaterniond angularVel0 = dmpQuaternionVel * dmpQuaternionPosi.inverse();
        angularVel0.w() *= 2;
        angularVel0.x() *= 2;
        angularVel0.y() *= 2;
        angularVel0.z() *= 2;
 
 
        double angularChange = angularVel0.angularDistance(oldDMPAngularVelocity);
        if (angularVel0.w() * oldDMPAngularVelocity.w() < 0 &&
            angularVel0.x() * oldDMPAngularVelocity.x() < 0 &&
            angularVel0.y() * oldDMPAngularVelocity.y() < 0 &&
            angularVel0.z() * oldDMPAngularVelocity.z() < 0 && angularChange < 1e-2)
        {
            angularVel0.w() = -angularVel0.w();
            angularVel0.x() = -angularVel0.x();
            angularVel0.y() = -angularVel0.y();
            angularVel0.z() = -angularVel0.z();
        }
        oldDMPAngularVelocity = angularVel0;
 
        // scale orientation velocity
        angularVel0.w() = 0;
        angularVel0.x() = tau * angularVel0.x() * config.oriAmplitude / timeDuration;
        angularVel0.y() = tau * angularVel0.y() * config.oriAmplitude / timeDuration;
        angularVel0.z() = tau * angularVel0.z() * config.oriAmplitude / timeDuration;
 
        //    Eigen::Quaterniond scaledQuat = (angularVel0 * dmpQuaternionPosi);
        //    currentState[3].vel = 0.5 * scaledQuat.w();
        //    currentState[4].vel = 0.5 * scaledQuat.x();
        //    currentState[6].vel = 0.5 * scaledQuat.z();
        //    currentState[5].vel = 0.5 * scaledQuat.y();
 
        for (size_t i = 3; i < 7; ++i)
        {
            currentState[i].vel = tau * temporalState[i].vel * config.oriAmplitude / timeDuration;
            currentState[i].pos += currentState[i].vel * deltaT;
        }
 
        if (isPhaseStopControl)
        {
            Eigen::Vector3f currentAngularVel;
            currentAngularVel << twist(3), twist(4), twist(5);
 
            Eigen::Quaternionf targetQuaternionf;
            targetQuaternionf.w() = targetPoseVec[3];
            targetQuaternionf.x() = targetPoseVec[4];
            targetQuaternionf.y() = targetPoseVec[5];
            targetQuaternionf.z() = targetPoseVec[6];
 
            Eigen::Matrix3f desiredMat(targetQuaternionf);
            Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
            Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
            Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
            Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY -
                                          config.phaseStopParas.Dori * currentAngularVel;
 
            targetVel(3) =
                mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
            targetVel(4) =
                mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
            targetVel(5) =
                mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
        }
        else
        {
            targetVel(3) = angularVel0.x();
            targetVel(4) = angularVel0.y();
            targetVel(5) = angularVel0.z();
        }
 
        debugData.canVal = canVal;
        debugData.oriError = oriError;
        debugData.posiError = posiError;
        debugData.mpcFactor = mpcFactor;
        debugData.poseError = poseError;
 
        return canVal;
    }
 
    void
    TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames,
                                              const std::vector<double>& ratios)
    {
        if (ratios.size() != fileNames.size())
        {
            ARMARX_ERROR << "ratios should have the same size with files";
            return;
        }
 
 
        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
 
        double ratioSum = 0;
        for (size_t i = 0; i < fileNames.size(); ++i)
        {
            DMP::SampledTrajectoryV2 traj;
            std::string absPath;
            ArmarXDataPath::getAbsolutePath(fileNames.at(i), absPath);
            traj.readFromCSVFile(absPath);
            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
            trajs.push_back(traj);
 
            ratioSum += ratios.at(i);
        }
 
        if (ratioSum == 0)
        {
            ARMARX_ERROR << "ratios are invalid. The sum is equal to 0";
            return;
        }
 
        DMP::DVec ratiosVec;
        ratiosVec.resize(ratios.size());
        for (size_t i = 0; i < ratios.size(); ++i)
        {
            ratiosVec.at(i) = ratios.at(i) / ratioSum;
        }
 
        dmpPtr->learnFromTrajectories(trajs);
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
    }
 
    void
    TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames)
    {
        std::vector<double> ratios;
        for (size_t i = 0; i < fileNames.size(); ++i)
        {
            if (i == 0)
            {
                ratios.push_back(1.0);
            }
            else
            {
                ratios.push_back(0.0);
            }
        }
 
        learnDMPFromFiles(fileNames, ratios);
    }
 
    void
    TaskSpaceDMPController::learnDMPFromSampledTrajectory(
        const DMP::Vec<DMP::SampledTrajectoryV2>& trajs,
        const std::vector<double>& ratios)
    {
        dmpPtr->learnFromTrajectories(trajs);
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
    }
 
    void
    TaskSpaceDMPController::setRatios(const std::vector<double>& ratios)
    {
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
    }
 
    void
    TaskSpaceDMPController::learnDMPFromTrajectory(const TrajectoryPtr& traj)
    {
        ARMARX_CHECK_EQUAL(traj->dim(), 7);
        DMP::SampledTrajectoryV2 dmpTraj;
 
        DMP::DVec timestamps(traj->getTimestamps());
        for (size_t i = 0; i < traj->dim(); ++i)
        {
            DMP::DVec dimData(traj->getDimensionData(i, 0));
            dmpTraj.addDimension(timestamps, dimData);
        }
 
        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
 
        dmpTraj = DMP::SampledTrajectoryV2::normalizeTimestamps(dmpTraj, 0, 1);
        trajs.push_back(dmpTraj);
        DMP::DVec ratiosVec;
        ratiosVec.push_back(1.0);
        dmpPtr->learnFromTrajectories(trajs);
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
    }
 
    void
    TaskSpaceDMPController::loadDMPFromString(const std::string& dmpStr)
    {
        std::stringstream ss;
        ss.str(dmpStr);
        boost::archive::text_iarchive ia{ss};
        DMP::UMITSMP* dmpPointer;
        ia >> dmpPointer;
        dmpPtr.reset(dmpPointer);
    }
 
    std::string
    TaskSpaceDMPController::saveDMPToString()
    {
        std::stringstream ss;
        boost::archive::text_oarchive oa{ss};
        oa << dmpPtr.get();
        return ss.str();
    }
 
    void
    TaskSpaceDMPController::loadDMPFromXML(const std::string& dmpXML)
    {
        std::string absPath;
        ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
        DMP::UMITSMP* dmpPointer;
        std::ifstream ifs(absPath);
        boost::archive::xml_iarchive ai(ifs);
        ai >> boost::serialization::make_nvp("dmp", dmpPointer);
        dmpPtr.reset(dmpPointer);
    }
 
    void
    TaskSpaceDMPController::saveDMPToXML(const std::string& dmpXML)
    {
        std::string absPath;
        ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
        std::ofstream ofs(absPath);
        boost::archive::xml_oarchive ar(ofs);
        DMP::UMITSMP* dmpPointer = dmpPtr.get();
        ar << boost::serialization::make_nvp("dmp", dmpPointer);
        dmpPtr.reset(dmpPointer);
    }
 
    void
    TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
    {
 
        setViaPose(canVal, eigen4f2vec(viaPose));
    }
 
    void
    TaskSpaceDMPController::setViaPose(double canVal,
                                       const std::vector<double>& viaPoseWithQuaternion)
    {
        if (canVal <= dmpPtr->getUMin())
        {
            goalPoseVec = viaPoseWithQuaternion;
        }
        dmpPtr->setViaPoint(canVal, viaPoseWithQuaternion);
    }
 
    void
    TaskSpaceDMPController::removeAllViaPoints()
    {
        dmpPtr->removeViaPoints();
    }
 
    void
    TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose,
                                             const Eigen::Matrix4f& goalPose)
    {
        std::vector<double> currentPoseVec = eigen4f2vec(currentPose);
        std::vector<double> goalPoseVec = eigen4f2vec(goalPose);
 
        prepareExecution(currentPoseVec, goalPoseVec);
    }
 
    void
    TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec,
                                             const std::vector<double>& goalPoseVec)
    {
        ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
        ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
 
        ARMARX_IMPORTANT << "prepareExecution: currentPoseVec: " << currentPoseVec;
        for (size_t i = 0; i < currentPoseVec.size(); ++i)
        {
            currentState[i].pos = currentPoseVec.at(i);
            currentState[i].vel = 0;
            targetPoseVec.at(i) = currentPoseVec.at(i);
        }
 
        dmpPtr->prepareExecution(goalPoseVec, currentState, 1, 1);
        this->goalPoseVec = goalPoseVec;
        isDisturbance = false;
        canVal = config.motionTimeDuration;
        oldDMPAngularVelocity.setIdentity();
    }
 
    void
    TaskSpaceDMPController::setSpeed(double times)
    {
        if (times <= 0)
        {
            ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive speed times";
        }
 
        tau = times;
    }
 
    void
    TaskSpaceDMPController::setAmplitude(double amp)
    {
        if (amp <= 0)
        {
            ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive amplitude";
        }
        config.DMPAmplitude = amp;
    }
 
    std::vector<double>
    TaskSpaceDMPController::eigen4f2vec(const Eigen::Matrix4f& pose)
    {
        std::vector<double> viaPoseVec;
        viaPoseVec.resize(7);
 
        for (size_t i = 0; i < 3; ++i)
        {
            viaPoseVec.at(i) = pose(i, 3);
        }
 
        VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(pose);
 
        viaPoseVec.at(3) = quat.w;
        viaPoseVec.at(4) = quat.x;
        viaPoseVec.at(5) = quat.y;
        viaPoseVec.at(6) = quat.z;
 
        return viaPoseVec;
    }
 
    void
    TaskSpaceDMPController::getError(const Eigen::Matrix4f& currentPose,
                                     Eigen::Vector3f& currentPosition,
                                     Eigen::Quaterniond& currentOrientation,
                                     double& posiError,
                                     double& oriError)
    {
        currentPosition.setZero();
        currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
 
        VirtualRobot::MathTools::Quaternion quat =
            VirtualRobot::MathTools::eigen4f2quat(currentPose);
        currentOrientation.w() = quat.w;
        currentOrientation.x() = quat.x;
        currentOrientation.y() = quat.y;
        currentOrientation.z() = quat.z;
 
        posiError = 0;
        for (size_t i = 0; i < 3; ++i)
        {
            posiError += pow(currentPosition(i) - targetPoseVec[i], 2);
        }
        posiError = sqrt(posiError);
 
        Eigen::Quaterniond targetQuaternion;
        targetQuaternion.w() = targetPoseVec[3];
        targetQuaternion.x() = targetPoseVec[4];
        targetQuaternion.y() = targetPoseVec[5];
        targetQuaternion.z() = targetPoseVec[6];
 
        oriError = targetQuaternion.angularDistance(currentOrientation);
        if (oriError > M_PI)
        {
            oriError = 2 * M_PI - oriError;
        }
    }
 
    TaskSpaceDMPController::TaskSpaceDMPController(std::string name,
                                                   const TaskSpaceDMPControllerConfig& config,
                                                   bool isPhaseStopControl)
    {
        this->config = config;
 
        int ModeInd;
        if (config.DMPMode == "MinimumJerk")
        {
            ModeInd = 2;
        }
        else
        {
            ModeInd = 1;
        }
 
 
        dmpPtr.reset(new DMP::UMITSMP(config.DMPKernelSize, ModeInd));
        canVal = config.motionTimeDuration;
 
        targetPoseVec.resize(7);
        targetVel.resize(6);
        targetVel.setZero();
        currentState.resize(7);
 
        this->isPhaseStopControl = isPhaseStopControl;
        dmpName = name;
        this->paused = false;
        tau = 1;
    }
 
    std::string
    TaskSpaceDMPController::getName()
    {
        return dmpName;
    }
 
    Eigen::VectorXf
    TaskSpaceDMPController::getTargetVelocity()
    {
        return targetVel;
    }
 
    std::vector<double>
    TaskSpaceDMPController::getTargetPose()
    {
        return targetPoseVec;
    }
 
    Eigen::Matrix4f
    TaskSpaceDMPController::getTargetPoseMat()
    {
        Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(
            targetPoseVec.at(4), targetPoseVec.at(5), targetPoseVec.at(6), targetPoseVec.at(3));
        res(0, 3) = targetPoseVec.at(0);
        res(1, 3) = targetPoseVec.at(1);
        res(2, 3) = targetPoseVec.at(2);
 
        return res;
    }
 
    Eigen::Matrix4f
    TaskSpaceDMPController::getIntegratedPoseMat()
    {
        Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(currentState.at(4).pos,
                                                                    currentState.at(5).pos,
                                                                    currentState.at(6).pos,
                                                                    currentState.at(3).pos);
        res(0, 3) = currentState.at(0).pos;
        res(1, 3) = currentState.at(1).pos;
        res(2, 3) = currentState.at(2).pos;
 
        return res;
    }
 
    void
    TaskSpaceDMPController::setGoalPose(const Eigen::Matrix4f& goalPose)
    {
        setViaPose(dmpPtr->getUMin(), goalPose);
    }
 
    void
    TaskSpaceDMPController::setGoalPoseVec(const std::vector<double> goalPoseVec)
    {
        setViaPose(dmpPtr->getUMin(), goalPoseVec);
    }
 
    DMP::UMITSMPPtr
    TaskSpaceDMPController::getDMP()
    {
        return dmpPtr;
    }
 
    void
    TaskSpaceDMPController::pauseController()
    {
        this->paused = true;
    }
 
    void
    TaskSpaceDMPController::resumeController()
    {
        this->paused = false;
    }
 
    void
    TaskSpaceDMPController::setWeights(const std::vector<std::vector<double>>& weights)
    {
        dmpPtr->setWeights(weights);
    }
 
    void
    TaskSpaceDMPController::setTranslWeights(const std::vector<std::vector<double>>& weights)
    {
        ARMARX_CHECK_EQUAL(weights.size(), 3);
 
        for (size_t i = 0; i < 3; ++i)
        {
            dmpPtr->setWeights(i, weights[i]);
        }
    }
 
    void
    TaskSpaceDMPController::setRotWeights(const std::vector<std::vector<double>>& weights)
    {
        ARMARX_CHECK_EQUAL(weights.size(), 4);
 
        for (size_t i = 0; i < 4; ++i)
        {
            dmpPtr->setWeights(3 + i, weights[i]);
        }
    }
 
    DMP::DVec2d
    TaskSpaceDMPController::getWeights()
    {
        return dmpPtr->getWeights();
    }
 
    DMP::DVec2d
    TaskSpaceDMPController::getTranslWeights()
    {
        DMP::DVec2d res;
        DMP::DVec2d weights = getWeights();
        for (size_t i = 0; i < 3; ++i)
        {
            res.push_back(weights[i]);
        }
        return res;
    }
 
    DMP::DVec2d
    TaskSpaceDMPController::getRotWeights()
    {
        DMP::DVec2d res;
        DMP::DVec2d weights = getWeights();
        for (size_t i = 3; i < 7; ++i)
        {
            res.push_back(weights[i]);
        }
        return res;
    }
} // namespace armarx::control::deprecated_njoint_mp_controller::tsvmp