NJointJointSpaceDMPController.cpp

Go to the documentation of this file.

#include "NJointJointSpaceDMPController.h"
 
 
 
namespace armarx::control::deprecated_njoint_mp_controller::joint_space
{
 
    NJointControllerRegistration<NJointJointSpaceDMPController> registrationControllerNJointJointSpaceDMPController("NJointJointSpaceDMPController");
 
    std::string NJointJointSpaceDMPController::getClassName(const Ice::Current&) const
    {
        return "NJointJointSpaceDMPController";
    }
 
    NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::RobotUnitPtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
    {
        NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION_W_HINT(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>()));
            torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
            gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
            velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
        }
        if (cfg->jointNames.size() == 0)
        {
            ARMARX_ERROR << "cfg->jointNames.size() == 0";
        }
 
        dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
        timeDuration = cfg->timeDuration;
        canVal = timeDuration;
        finished = false;
        phaseL = cfg->phaseL;
        phaseK = cfg->phaseK;
        phaseDist0 = cfg->phaseDist0;
        phaseDist1 = cfg->phaseDist1;
        phaseKp = cfg->phaseKp;
 
        isDisturbance = false;
 
        NJointJointSpaceDMPControllerControlData initData;
        initData.tau = 1.0;
        initData.isStart = false;
        reinitTripleBuffer(initData);
    }
 
    void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
    {
        if (rtGetControlStruct().isStart && !finished)
        {
            currentState.clear();
            double phaseStop = 0;
            double error = 0;
            std::vector<double> currentPosition;
            std::vector<double> currentVelocity;
            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;
                currentPos.vel *= timeDuration;
                currentState.push_back(currentPos);
                currentPosition.push_back(currentPos.pos);
                currentVelocity.push_back(currentPos.vel);
 
                error += pow(currentPos.pos - targetState[i], 2);
            }
 
            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;
            }
 
            double tau = rtGetControlStruct().tau;
            double deltaT = timeSinceLastIteration.toSecondsDouble();
            canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
            double dmpDeltaT = deltaT / timeDuration;
            dmpPtr->setTemporalFactor(tau);
 
            currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
 
            if (canVal < 1e-8)
            {
                finished = true;
            }
 
            for (size_t i = 0; i < dimNames.size(); ++i)
            {
                const auto& jointName = dimNames.at(i);
                if (targets.count(jointName) == 1)
                {
                    double vel0 = currentState[i].vel / timeDuration;
                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
                    double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                    targets[jointName]->velocity = finished ? 0.0f : vel;
 
                    std::string targetVelstr = jointName + "_targetvel";
                    std::string targetStatestr = jointName + "_dmpTarget";
                    debugOutputData.getWriteBuffer().latestTargetVelocities[jointName] = vel;
                    debugOutputData.getWriteBuffer().dmpTargetState[jointName] = targetState[i];
 
                }
            }
 
            debugOutputData.getWriteBuffer().currentCanVal = canVal;
            debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
            debugOutputData.commitWrite();
        }
        else
        {
            for (size_t i = 0; i < dimNames.size(); ++i)
            {
                const auto& jointName = dimNames.at(i);
                if (targets.count(jointName) == 1)
                {
                    targets[jointName]->velocity =  0.0f;
                }
            }
        }
    }
 
    void NJointJointSpaceDMPController::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));
            dimNames = traj.getDimensionNames();
            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->setOneStepMPC(true);
        dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
 
        ARMARX_INFO << "Learned DMP ... ";
    }
 
    void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
    {
        currentState.clear();
        targetState.clear();
        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;
            currentState.push_back(currentPos);
            targetState.push_back(currentPos.pos);
        }
        dmpPtr->prepareExecution(goals, currentState, 1,  tau);
        finished = false;
 
        this->goals = goals;
        this->tau = tau;
 
        LockGuardType guard {controlDataMutex};
        getWriterControlStruct().tau = tau;
        getWriterControlStruct().isStart = true;
        writeControlStruct();
 
    }
 
    void NJointJointSpaceDMPController::showMessages(const Ice::Current&)
    {
    }
 
    void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
    {
        this->tau = tau;
        LockGuardType guard {controlDataMutex};
        getWriterControlStruct().tau = tau;
        getWriterControlStruct().isStart = true;
        writeControlStruct();
    }
 
    void NJointJointSpaceDMPController::rtPreActivateController()
    {
    }
 
    void NJointJointSpaceDMPController::rtPostDeactivateController()
    {
 
    }
 
    void NJointJointSpaceDMPController::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);
        }
 
        auto valuesst = debugOutputData.getUpToDateReadBuffer().dmpTargetState;
        for (auto& pair : valuesst)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
        debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
    }
 
 
}