NJointPeriodicTSDMPForwardVelController.cpp

Go to the documentation of this file.

#include "NJointPeriodicTSDMPForwardVelController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController>
        registrationControllerNJointPeriodicTSDMPForwardVelController(
            "NJointPeriodicTSDMPForwardVelController");
 
    NJointPeriodicTSDMPForwardVelController::NJointPeriodicTSDMPForwardVelController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg = NJointPeriodicTSDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
 
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
 
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
 
        tcp = rns->getTCP();
        // set tcp controller
        tcpController.reset(new CartesianVelocityController(rns, tcp));
        nodeSetName = cfg->nodeSetName;
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
 
        taskSpaceDMPConfig.DMPMode = "Linear";
        taskSpaceDMPConfig.DMPStyle = "Periodic";
        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
 
 
        dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
 
        NJointPeriodicTSDMPForwardVelControllerControlData initData;
        initData.targetPose = tcp->getPoseInRootFrame();
        initData.targetTSVel.resize(6);
        for (size_t i = 0; i < 6; ++i)
        {
            initData.targetTSVel(i) = 0;
        }
        reinitTripleBuffer(initData);
 
        finished = false;
        firstRun = true;
 
 
        const SensorValueBase* svlf =
            robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
        forceOffset.setZero();
        filteredForce.setZero();
 
        UserToRTData initUserData;
        initUserData.targetForce = 0;
        user2rtData.reinitAllBuffers(initUserData);
 
        oriToolDir << 0, 0, 1;
 
        kpf = cfg->Kpf;
    }
 
    std::string
    NJointPeriodicTSDMPForwardVelController::getClassName(const Ice::Current&) const
    {
        return "NJointPeriodicTSDMPForwardVelController";
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::controllerRun()
    {
        if (!started)
        {
            return;
        }
 
        if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
        {
            return;
        }
 
        double deltaT = rt2CtrlData.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
        Eigen::VectorXf targetVels = dmpCtrl->getTargetVelocity();
        Eigen::Matrix4f targetPose = dmpCtrl->getIntegratedPoseMat();
 
        debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
        debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
        debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
        debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
        debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
        debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
        debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
        debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
        debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
        VirtualRobot::MathTools::Quaternion currentQ =
            VirtualRobot::MathTools::eigen4f2quat(currentPose);
        debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
        debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
        debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
        debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
        debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
        debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
        debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
        debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
        debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
        debugOutputData.getWriteBuffer().deltaT = deltaT;
        debugOutputData.commitWrite();
 
        getWriterControlStruct().targetTSVel = targetVels;
        getWriterControlStruct().targetPose = targetPose;
        writeControlStruct();
 
        dmpRunning = true;
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                                   const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
        rt2UserData.commitWrite();
 
        if (firstRun || !dmpRunning)
        {
            targetPose = currentPose;
            for (size_t i = 0; i < targets.size(); ++i)
            {
                targets.at(i)->velocity = 0.0f;
            }
            firstRun = false;
            filteredForce.setZero();
 
            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
            toolTransform = currentHandOri.transpose();
            // force calibration
            if (!dmpRunning)
            {
                forceOffset =
                    (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
            }
        }
        else
        {
 
            Eigen::MatrixXf jacobi =
                ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
            Eigen::VectorXf qvel;
            qvel.resize(velocitySensors.size());
            for (size_t i = 0; i < velocitySensors.size(); ++i)
            {
                qvel(i) = velocitySensors[i]->velocity;
            }
 
            Eigen::VectorXf tcptwist = jacobi * qvel;
 
            rt2CtrlData.getWriteBuffer().currentPose = currentPose;
            rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
            rt2CtrlData.getWriteBuffer().deltaT = deltaT;
            rt2CtrlData.getWriteBuffer().currentTime += deltaT;
            rt2CtrlData.commitWrite();
 
 
            // forward controller
            rtUpdateControlStruct();
            Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
            //        Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
 
            // force detection
            //            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
            //            Eigen::Vector3f filteredForceInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->toGlobalCoordinateSystemVec(filteredForce);
            //            filteredForceInRoot = rtGetRobot()->getRootNode()->toLocalCoordinateSystemVec(filteredForceInRoot);
            //            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
 
            //            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
            //            Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
            //            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
            //            Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
 
            //            ARMARX_IMPORTANT << "original force: " << forceSensor->force;
            //            ARMARX_IMPORTANT << "filteredForce: " << filteredForce;
            //            ARMARX_IMPORTANT << "filteredForceInRoot: " << filteredForceInRoot;
            //            ARMARX_IMPORTANT << "forceOffset: " << forceOffset;
            //            ARMARX_IMPORTANT << "currentToolOri: " << currentToolOri;
 
            for (size_t i = 3; i < 6; ++i)
            {
                targetVel(i) = 0;
            }
 
            //            float forceCtrl = kpf * (targetForce - filteredForceInRoot.norm());
            //            Eigen::Vector3f desiredZVel = - forceCtrl * (currentToolDir / currentToolDir.norm());
            //            targetVel.block<3, 1>(0, 0) += desiredZVel;
 
            // dead zone for force
            //        if (filteredForceInRoot.norm() > cfg->minimumReactForce)
            //        {
            //            // rotation changes
            //            Eigen::Vector3f axis = oriToolDir.cross(filteredForceInRoot);
            //            float angle = oriToolDir.dot(filteredForceInRoot);
            //            Eigen::AngleAxisf desiredToolOri(angle, axis);
            //            Eigen::Matrix3f desiredHandOri = toolTransform.transpose() * desiredToolOri;
            //            Eigen::Matrix3f desiredRotMat = desiredHandOri * currentHandOri.transpose();
            //            Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
            //            for (size_t i = 3; i < 6; ++i)
            //            {
            //                targetVel(i) = desiredRPY(i - 3);
            //            }
            //        }}
 
            //            ARMARX_IMPORTANT << "targetVel: " << targetVel;
            //            ARMARX_IMPORTANT << "targetPose: " << targetPose;
 
            //            targetPose.block<3, 1>(0, 3) += deltaT * targetVel.block<3, 1>(0, 0);
            //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * deltaT, targetVel(4) * deltaT, targetVel(5) * deltaT);
            //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
 
            float dTf = (float)deltaT;
            targetPose.block<3, 1>(0, 3) =
                targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(
                targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
 
            ARMARX_IMPORTANT << "targetVel: " << targetVel.block<3, 1>(0, 0);
            ARMARX_IMPORTANT << "targetPose: " << targetPose;
            ARMARX_IMPORTANT << "deltaT: " << deltaT;
 
            Eigen::Matrix3f diffMat =
                targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
 
            Eigen::VectorXf rtTargetVel = targetVel;
            rtTargetVel.block<3, 1>(0, 0) +=
                cfg->Kpos * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
            rtTargetVel.block<3, 1>(3, 0) += cfg->Kori * errorRPY;
 
            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
            if (normLinearVelocity > fabs(cfg->maxLinearVel))
            {
                rtTargetVel.block<3, 1>(0, 0) =
                    fabs(cfg->maxLinearVel) * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
            }
 
            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
            if (normAngularVelocity > fabs(cfg->maxAngularVel))
            {
                rtTargetVel.block<3, 1>(3, 0) =
                    fabs(cfg->maxAngularVel) * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
            }
 
 
            // cartesian vel controller
 
            Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
            if (cfg->avoidJointLimitsKp > 0)
            {
                jnv += cfg->avoidJointLimitsKp * tcpController->calculateJointLimitAvoidance();
            }
 
            Eigen::VectorXf jointTargetVelocities = tcpController->calculate(
                rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
            for (size_t i = 0; i < targets.size(); ++i)
            {
                targets.at(i)->velocity = jointTargetVelocities(i);
                if (!targets.at(i)->isValid())
                {
                    targets.at(i)->velocity = 0.0f;
                }
                else
                {
                    if (fabs(targets.at(i)->velocity) > fabs(cfg->maxJointVel))
                    {
                        targets.at(i)->velocity =
                            fabs(cfg->maxJointVel) *
                            (targets.at(i)->velocity / fabs(targets.at(i)->velocity));
                    }
                }
            }
        }
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
                                                               const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::setGoals(const Ice::DoubleSeq& goals,
                                                      const Ice::Current& ice)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::setAmplitude(Ice::Double amp, const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setAmplitude(amp);
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::runDMP(const Ice::DoubleSeq& goals,
                                                    double tau,
                                                    const Ice::Current&)
    {
        firstRun = true;
        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration <
                               cfg->waitTimeForCalibration)
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
        LockGuardType guard{controllerMutex};
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
        dmpCtrl->setSpeed(tau);
        finished = false;
 
        ARMARX_INFO << "run DMP";
        started = true;
        dmpRunning = false;
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::onPublish(const SensorAndControl&,
                                                       const DebugDrawerInterfacePrx&,
                                                       const DebugObserverInterfacePrx& debugObs)
    {
        std::string datafieldName;
        std::string debugName = "Periodic";
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
        for (auto& pair : values)
        {
            datafieldName = pair.first + "_" + debugName;
            datafields[datafieldName] = new Variant(pair.second);
        }
 
        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
        for (auto& pair : currentPose)
        {
            datafieldName = pair.first + "_" + debugName;
            datafields[datafieldName] = new Variant(pair.second);
        }
 
        datafieldName = "canVal_" + debugName;
        datafields[datafieldName] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        datafieldName = "mpcFactor_" + debugName;
        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
        datafieldName = "error_" + debugName;
        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
        datafieldName = "posError_" + debugName;
        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
        datafieldName = "oriError_" + debugName;
        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
        datafieldName = "deltaT_" + debugName;
        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
        datafieldName = "DMPController_" + debugName;
 
        debugObs->setDebugChannel(datafieldName, datafields);
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
 
        RTToControllerData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose = tcp->getPoseInRootFrame();
        initSensorData.currentTwist.setZero();
        rt2CtrlData.reinitAllBuffers(initSensorData);
 
        RTToUserData initInterfaceData;
        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
        rt2UserData.reinitAllBuffers(initInterfaceData);
 
 
        started = false;
        runTask("NJointPeriodicTSDMPForwardVelController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointPeriodicTSDMPForwardVelController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space