KeypointsImpedanceController.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    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KeypointsImpedanceController.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.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 <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/control_law/aron/KeypointControllerConfig.aron.generated.h>
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointKeypointsImpedanceController>
        registrationControllerNJointKeypointsImpedanceController(
            "NJointKeypointsImpedanceController");
 
    NJointKeypointsImpedanceController::NJointKeypointsImpedanceController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
 
        ARMARX_CHECK_EXPRESSION(cfg);
        ARMARX_CHECK_EXPRESSION(robotUnit);
        {
            auto configData = ::armarx::fromAronDict<AronDTO, BO>(cfg->config);
 
            kinematicChainName = configData.nodeSetName;
            ARMARX_CHECK_EXPRESSION(!kinematicChainName.empty());
            validateConfigData(configData);
 
            bufferUserToAdditionalTask.reinitAllBuffers(configData);
            bufferAdditionalTaskToUser.reinitAllBuffers(configData);
            bufferUserToRt.reinitAllBuffers(configData);
        }
 
        useSynchronizedRtRobot();
        VirtualRobot::RobotNodeSetPtr rtRns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
 
        nonRtRobot = robotUnit->cloneRobot();
        VirtualRobot::RobotNodeSetPtr nonRtRns = nonRtRobot->getRobotNodeSet(kinematicChainName);
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        ARMARX_CHECK_EXPRESSION(nonRtRns) << kinematicChainName;
        ARMARX_CHECK_EXPRESSION(rtRns) << kinematicChainName;
 
        controller.initialize(nonRtRns);
 
        jointNames.clear();
        for (size_t i = 0; i < rtRns->getSize(); ++i)
        {
            std::string jointName = rtRns->getNode(i)->getName();
            jointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            ARMARX_CHECK_EXPRESSION(ct);
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
            ARMARX_CHECK_EXPRESSION(casted_ct);
            targets.push_back(casted_ct);
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            sensorDevices.torqueSensors.push_back(torqueSensor);
            sensorDevices.velocitySensors.push_back(velocitySensor);
            sensorDevices.positionSensors.push_back(positionSensor);
        };
 
        controlTargets.targets.resize(rtRns->getSize(), 0.);
    }
 
    std::string
    NJointKeypointsImpedanceController::getClassName(const Ice::Current&) const
    {
        return "NJointKeypointsImpedanceController";
    }
 
    //std::string NJointKeypointsImpedanceController::getKinematicChainName(const Ice::Current &)
    //{
    //    return kinematicChainName;
    //}
 
    void
    NJointKeypointsImpedanceController::onInitNJointController()
    {
        runTask("NJointTaskspaceAdmittanceAdditionalTask",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT
                        << "Create a new thread alone NJointKeypointsImpedanceController";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointKeypointsImpedanceController::additionalTask()
    {
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        controller.updateControlStatus(bufferUserToAdditionalTask.getUpToDateReadBuffer(),
                                       bufferRtToAdditionalTask.getUpToDateReadBuffer());
    }
 
    void
    NJointKeypointsImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                                              const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        auto& userConfig = bufferUserToRt.getUpToDateReadBuffer();
 
        if (rtFirstRun.load())
        {
            rtFirstRun.store(false);
            rtReady.store(true);
            controller.firstRun();
        }
 
        auto& s = bufferRtToAdditionalTask.getWriteBuffer();
        size_t nDoF = sensorDevices.positionSensors.size();
        for (size_t i = 0; i < nDoF; ++i)
        {
            s.jointPosition[i] = sensorDevices.positionSensors[i]->position;
            s.jointVelocity[i] = sensorDevices.velocitySensors[i]->velocity;
            s.jointTorque[i] = sensorDevices.torqueSensors[i]->torque;
        }
        s.deltaT = deltaT;
        bufferRtToAdditionalTask.commitWrite();
 
        controller.run(userConfig, controlTargets);
 
        for (unsigned int i = 0; i < controlTargets.targets.size(); i++)
        {
            targets.at(i)->torque = controlTargets.targets.at(i);
            if (!targets.at(i)->isValid())
            {
                targets.at(i)->torque = 0;
            }
        }
    }
 
    void
    NJointKeypointsImpedanceController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                                     const Ice::Current& iceCurrent)
    {
        auto configData = ::armarx::fromAronDict<AronDTO, BO>(dto);
 
        validateConfigData(configData);
 
        bufferUserToAdditionalTask.getWriteBuffer() = configData;
        bufferUserToAdditionalTask.commitWrite();
 
        bufferUserToRt.getWriteBuffer() = configData;
        bufferUserToRt.commitWrite();
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointKeypointsImpedanceController::getConfig(const Ice::Current& iceCurrent)
    {
        auto configData = bufferAdditionalTaskToUser.getUpToDateReadBuffer();
        return armarx::toAronDict<AronDTO, BO>(configData);
    }
 
    void
    NJointKeypointsImpedanceController::validateConfigData(BO& configData)
    {
        const auto nDoF = jointNames.size();
 
        const auto checkSize = [nDoF](const auto& v)
        { ARMARX_CHECK_EQUAL((unsigned)v.rows(), nDoF); };
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
 
        if (!configData.desiredNullspaceJointAngles.has_value() and !isControllerActive())
        {
            ARMARX_INFO << "No user defined nullspace target, reset to zero with " << VAROUT(nDoF);
            configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
            reInitPreActivate.store(true);
        }
        checkSize(configData.desiredNullspaceJointAngles.value());
        checkSize(configData.kdNullspaceTorque);
        checkSize(configData.kpNullspaceTorque);
 
        checkNonNegative(configData.kdNullspaceTorque);
        checkNonNegative(configData.kpNullspaceTorque);
        checkNonNegative(configData.kdImpedance);
        checkNonNegative(configData.kpImpedance);
 
        ARMARX_CHECK_GREATER_EQUAL(configData.numPoints, 0);
        ARMARX_CHECK_GREATER_EQUAL(configData.qvelFilter, 0);
        ARMARX_CHECK_GREATER_EQUAL(configData.numPoints, 0);
        ARMARX_CHECK_GREATER_EQUAL(configData.keypointPositionFilter, 0);
        ARMARX_CHECK_GREATER_EQUAL(configData.keypointVelocityFilter, 0);
 
        ARMARX_CHECK_EQUAL(configData.keypointKp.size(), configData.numPoints * 3);
        ARMARX_CHECK_EQUAL(configData.keypointKd.size(), configData.numPoints * 3);
        ARMARX_CHECK_EQUAL(configData.fixedTranslation.size(), configData.numPoints * 3);
    }
 
    void
    NJointKeypointsImpedanceController::onPublish(const SensorAndControl&,
                                                  const DebugDrawerInterfacePrx&,
                                                  const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
 
        auto nonRtData = controller.bufferNonRtToOnPublish.getUpToDateReadBuffer();
        auto rtData = controller.bufferRtToOnPublish.getUpToDateReadBuffer();
        auto configData = bufferUserToAdditionalTask.getUpToDateReadBuffer();
 
        common::debugEigenVec(datafields, "kpImpedance", configData.kpImpedance);
        common::debugEigenVec(datafields, "kdImpedance", configData.kdImpedance);
 
        common::debugEigenVec(datafields, "forceImpedance", rtData.forceImpedance);
        common::debugEigenVec(datafields, "desiredJointTorques", rtData.desiredJointTorques);
        common::debugEigenVec(datafields, "f_Impedance", rtData.forceImpedance);
        common::debugEigenVec(datafields, "tau_d_joint", rtData.desiredJointTorques);
 
        common::debugEigenPose(datafields, "current_pose", nonRtData.currentPose);
        common::debugEigenPose(datafields, "desired_pose", nonRtData.desiredPose);
        common::debugEigenVec(datafields, "v_d", nonRtData.desiredVel);
        common::debugEigenVec(datafields, "a_d", nonRtData.desiredAcc);
        common::debugEigenVec(datafields, "f_density", nonRtData.desiredDensityForce);
        common::debugEigenVec(datafields, "pointTrackingForce", nonRtData.pointTrackingForce);
        common::debugEigenVec(
            datafields, "desiredKeypointPosition", nonRtData.desiredKeypointPosition);
        common::debugEigenVec(
            datafields, "currentKeypointPosition", nonRtData.currentKeypointPosition);
        common::debugEigenVec(
            datafields, "currentKeypointVelocity", nonRtData.currentKeypointVelocity);
        common::debugEigenVec(
            datafields, "filteredKeypointPosition", nonRtData.filteredKeypointPosition);
 
        debugObs->setDebugChannel("NJointKeypointsImpedanceController", datafields);
    }
 
    void
    NJointKeypointsImpedanceController::rtPreActivateController()
    {
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose;
        controller.preactivateInit(rns, bufferUserToRt.getUpToDateReadBuffer());
 
        if (reInitPreActivate.load())
        {
            auto& c = bufferUserToAdditionalTask.getWriteBuffer();
            c.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
 
            bufferUserToAdditionalTask.reinitAllBuffers(c);
            bufferUserToRt.reinitAllBuffers(c);
 
            reInitPreActivate.store(false);
        }
 
        const auto nDoF = rns->getSize();
 
        {
            law::RobotStatus rtToNonRtStatus;
            rtToNonRtStatus.jointPosition.resize(nDoF, 0.);
            rtToNonRtStatus.jointVelocity.resize(nDoF, 0.);
            rtToNonRtStatus.jointTorque.resize(nDoF, 0.);
            for (size_t i = 0; i < nDoF; ++i)
            {
                rtToNonRtStatus.jointPosition[i] = sensorDevices.positionSensors[i]->position;
                rtToNonRtStatus.jointVelocity[i] = sensorDevices.velocitySensors[i]->velocity;
                rtToNonRtStatus.jointTorque[i] = sensorDevices.torqueSensors[i]->torque;
            }
            bufferRtToAdditionalTask.reinitAllBuffers(rtToNonRtStatus);
        }
    }
 
    void
    NJointKeypointsImpedanceController::rtPostDeactivateController()
    {
        controller.isInitialized.store(false);
    }
} // namespace armarx::control::njoint_controller::task_space