NJointJSDMPController.cpp

Go to the documentation of this file.

#include "NJointJSDMPController.h"
 
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::joint_space
{
 
    NJointControllerRegistration<NJointJSDMPController>
        registrationControllerNJointJSDMPController("NJointJSDMPController");
 
    std::string
    NJointJSDMPController::getClassName(const Ice::Current&) const
    {
        return "NJointJSDMPController";
    }
 
    NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&,
                                                 const armarx::NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "creating joint space dmp controller ... ";
        useSynchronizedRtRobot();
 
        cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointJointSpaceDMPControllerConfigPtr";
 
        for (std::string jointName : cfg->jointNames)
        {
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
            positionSensors.insert(
                std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
            velocitySensors.insert(
                std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
        }
        if (cfg->jointNames.size() == 0)
        {
            ARMARX_ERROR << "cfg->jointNames.size() == 0";
        }
        ARMARX_INFO << "start creating dmpPtr ... "
                    << " baseMode: " << cfg->baseMode;
 
        dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, 100, cfg->baseMode, 1));
        timeDuration = cfg->timeDuration;
        phaseL = cfg->phaseL;
        phaseK = cfg->phaseK;
        phaseDist0 = cfg->phaseDist0;
        phaseDist1 = cfg->phaseDist1;
        phaseKp = cfg->phaseKp;
        dimNames = cfg->jointNames;
        ARMARX_INFO << "created dmpPtr ... ";
 
        targetVels.resize(cfg->jointNames.size());
        NJointJSDMPControllerControlData initData;
        initData.targetJointVels.resize(cfg->jointNames.size());
        for (size_t i = 0; i < cfg->jointNames.size(); ++i)
        {
            initData.targetJointVels[i] = 0;
            targetVels[i] = 0;
        }
 
        reinitTripleBuffer(initData);
 
 
        NJointJSDMPControllerSensorData initSensorData;
        initSensorData.currentTime = 0;
        initSensorData.deltaT = 0;
        initSensorData.currentState.resize(cfg->jointNames.size());
        controllerSensorData.reinitAllBuffers(initSensorData);
 
        deltaT = 0;
 
        qpos.resize(dimNames.size());
        qvel.resize(dimNames.size());
    }
 
    void
    NJointJSDMPController::controllerRun()
    {
        if (!started || finished)
        {
            for (size_t i = 0; i < dimNames.size(); ++i)
            {
                targetVels[i] = 0;
            }
        }
        else
        {
            currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
            double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
 
            if (canVal > 1e-8)
            {
                double phaseStop = 0;
                double mpcFactor = 1;
 
                std::vector<double> currentPosition;
                double error = 0;
                currentPosition.resize(dimNames.size());
 
                for (size_t i = 0; i < currentState.size(); i++)
                {
                    DMP::DMPState currentPos = currentState[i];
                    currentPosition[i] = currentPos.pos;
                    error += pow(currentPos.pos - targetState[i], 2);
                }
 
                if (cfg->isPhaseStop)
                {
                    double phaseDist;
 
                    if (isDisturbance)
                    {
                        phaseDist = phaseDist1;
                    }
                    else
                    {
                        phaseDist = phaseDist0;
                    }
 
                    error = sqrt(error);
                    phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
                    mpcFactor = 1 - (phaseStop / phaseL);
 
                    if (mpcFactor < 0.1)
                    {
                        isDisturbance = true;
                    }
 
                    if (mpcFactor > 0.9)
                    {
                        isDisturbance = false;
                    }
                }
 
                canVal -= tau * deltaT * 1 / (1 + phaseStop);
                double dmpDeltaT = deltaT / timeDuration;
 
                currentDMPState = dmpPtr->calculateDirectlyVelocity(
                    currentDMPState, canVal / timeDuration, dmpDeltaT, targetState);
 
                for (size_t i = 0; i < currentDMPState.size(); ++i)
                {
                    double vel0 = tau * currentDMPState[i].vel / timeDuration;
                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
                    //                    double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                    double vel = vel1 + vel0;
                    targetVels[i] = vel;
                    debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] =
                        (float)vel;
                    debugOutputData.getWriteBuffer().latestTargets[dimNames[i]] =
                        (float)currentDMPState[i].pos;
                }
 
                debugOutputData.getWriteBuffer().currentCanVal = canVal;
                debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
                debugOutputData.commitWrite();
            }
            else
            {
                finished = true;
                for (size_t i = 0; i < dimNames.size(); ++i)
                {
                    targetVels[i] = 0;
                }
            }
        }
 
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().targetJointVels = targetVels;
        writeControlStruct();
    }
 
    void
    NJointJSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                 const IceUtil::Time& timeSinceLastIteration)
    {
        for (size_t i = 0; i < dimNames.size(); i++)
        {
            const auto& jointName = dimNames.at(i);
            DMP::DMPState currentPos;
            currentPos.pos = (positionSensors.count(jointName) == 1)
                                 ? positionSensors[jointName]->position
                                 : 0.0f;
            currentPos.vel = (velocitySensors.count(jointName) == 1)
                                 ? velocitySensors[jointName]->velocity
                                 : 0.0f;
            qpos[i] = currentPos.pos;
            qvel[i] = currentPos.vel;
            controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
        }
        controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
        controllerSensorData.getWriteBuffer().currentTime +=
            timeSinceLastIteration.toSecondsDouble();
        controllerSensorData.commitWrite();
 
 
        rt2UserData.getWriteBuffer().qpos = qpos;
        rt2UserData.getWriteBuffer().qvel = qvel;
        rt2UserData.commitWrite();
 
        Eigen::VectorXf targetJointVels = rtGetControlStruct().targetJointVels;
        //        ARMARX_INFO << targetJointVels;
 
        for (size_t i = 0; i < dimNames.size(); ++i)
        {
 
            if (fabs(targetJointVels[i]) > cfg->maxJointVel)
            {
                targets[dimNames[i]]->velocity =
                    targetJointVels[i] < 0 ? -cfg->maxJointVel : cfg->maxJointVel;
            }
            else
            {
                targets[dimNames[i]]->velocity = targetJointVels[i];
            }
        }
    }
 
    void
    NJointJSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
    {
        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
 
        DMP::DVec ratios;
        for (size_t i = 0; i < fileNames.size(); ++i)
        {
            DMP::SampledTrajectoryV2 traj;
            traj.readFromCSVFile(fileNames.at(i));
            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
            trajs.push_back(traj);
 
            if (i == 0)
            {
                ratios.push_back(1.0);
            }
            else
            {
                ratios.push_back(0.0);
            }
        }
        dmpPtr->learnFromTrajectories(trajs);
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
 
        ARMARX_INFO << "Learned DMP ... ";
    }
 
    void
    NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&)
    {
        while (!rt2UserData.updateReadBuffer())
        {
            usleep(100);
        }
 
        targetState.clear();
        targetState.resize(dimNames.size());
        currentState.clear();
        currentState.resize(dimNames.size());
        currentDMPState.clear();
        currentDMPState.resize(dimNames.size());
 
        std::vector<double> goalVec = goals;
        for (size_t i = 0; i < dimNames.size(); i++)
        {
            DMP::DMPState currentPos;
            currentPos.pos = rt2UserData.getReadBuffer().qpos[i];
            currentPos.vel = rt2UserData.getReadBuffer().qvel[i];
 
            currentState[i] = currentPos;
            currentDMPState[i] = currentPos;
 
            targetState.push_back(currentPos.pos);
 
            if (rtGetRobot()->getRobotNode(dimNames[i])->isLimitless())
            {
                double tjv = goalVec[i];
                double cjv = currentPos.pos;
                double diff = std::fmod(tjv - cjv, 2 * M_PI);
                if (fabs(diff) > M_PI)
                {
                    if (signbit(diff))
                    {
                        diff = -2 * M_PI - diff;
                    }
                    else
                    {
                        diff = 2 * M_PI - diff;
                    }
                    tjv = cjv - diff;
                }
                else
                {
                    tjv = cjv + diff;
                }
 
                goalVec[i] = tjv;
                ARMARX_INFO << "dim name: " << dimNames[i]
                            << " current state: qpos: " << currentPos.pos
                            << " orig target: " << goals[i] << " current goal: " << tjv;
            }
        }
 
        dmpPtr->prepareExecution(goalVec, currentDMPState, 1, 1);
        canVal = timeDuration;
        finished = false;
        isDisturbance = false;
 
        tau = times;
        ARMARX_INFO << "run DMP";
        started = true;
    }
 
    void
    NJointJSDMPController::showMessages(const Ice::Current&)
    {
    }
 
    std::string
    NJointJSDMPController::getDMPAsString(const Ice::Current&)
    {
        std::stringstream ss;
        boost::archive::text_oarchive oa{ss};
        oa << dmpPtr.get();
        return ss.str();
    }
 
    std::vector<double>
    NJointJSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
    {
        std::stringstream ss;
        ss.str(dmpString);
        boost::archive::text_iarchive ia{ss};
        DMP::UMIDMP* newDmpPtr;
        ia >> newDmpPtr;
        dmpPtr.reset(newDmpPtr);
        return dmpPtr->defaultGoal;
    }
 
    void
    NJointJSDMPController::setViaPoints(Ice::Double u, double viapoint, const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpPtr->setViaPoint(u, viapoint);
    }
 
    void
    NJointJSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
    {
        dmpPtr->setWeights(weights);
    }
 
    DoubleSeqSeq
    NJointJSDMPController::getMPWeights(const Ice::Current&)
    {
        DMP::DVec2d res = dmpPtr->getWeights();
        DoubleSeqSeq resvec;
        for (size_t i = 0; i < res.size(); ++i)
        {
            std::vector<double> cvec;
            for (size_t j = 0; j < res[i].size(); ++j)
            {
                cvec.push_back(res[i][j]);
            }
            resvec.push_back(cvec);
        }
 
        return resvec;
    }
 
    void
    NJointJSDMPController::setSpeed(double times, const Ice::Current&)
    {
        LockGuardType guard(controllerMutex);
        tau = times;
    }
 
    void
    NJointJSDMPController::rtPreActivateController()
    {
    }
 
    void
    NJointJSDMPController::rtPostDeactivateController()
    {
    }
 
    void
    NJointJSDMPController::onPublish(const SensorAndControl&,
                                     const DebugDrawerInterfacePrx&,
                                     const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        values = debugOutputData.getUpToDateReadBuffer().latestTargets;
        for (auto& pair : values)
        {
            datafields[pair.first + "_pos"] = new Variant(pair.second);
        }
 
        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
        debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
    }
 
 
    void
    NJointJSDMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        started = false;
        runTask("NJointJSDMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointJSDMPController::onDisconnectNJointController()
    {
    }
 
 
} // namespace armarx::ctrl::njoint_ctrl::dmp