BimanualImpedanceController.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       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImpedanceController.h"
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h>
#include <armarx/control/common/utils.h>
 
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointTaskspaceBimanualImpedanceController>
            registrationControllerNJointTaskspaceBimanualImpedanceController(
            "NJointTaskspaceBimanualImpedanceController");
 
    NJointTaskspaceBimanualImpedanceController::NJointTaskspaceBimanualImpedanceController(
            const RobotUnitPtr& robotUnit,
            const NJointControllerConfigPtr& config,
            const VirtualRobot::RobotPtr&) :
            robotUnit(robotUnit)
    {
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
 
        ARMARX_CHECK_EXPRESSION(cfg);
        ARMARX_CHECK_EXPRESSION(robotUnit);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        useSynchronizedRtRobot();
        kinematicChainName = cfg->nodeSetName;
        VirtualRobot::RobotNodeSetPtr rtRns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
 
        nonRtRobot = robotUnit->cloneRobot();
        VirtualRobot::RobotNodeSetPtr nonRtRns = nonRtRobot->getRobotNodeSet(cfg->nodeSetName);
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        ARMARX_CHECK_EXPRESSION(rtRns) << cfg->nodeSetName;
        ARMARX_CHECK_EXPRESSION(nonRtRns) << cfg->nodeSetName;
 
        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 auto* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
            const auto* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
            const auto* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (torqueSensor == nullptr)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (velocitySensor == nullptr)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (positionSensor == nullptr)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            sensorDevices.torqueSensors.push_back(torqueSensor);
            sensorDevices.velocitySensors.push_back(velocitySensor);
            sensorDevices.positionSensors.push_back(positionSensor);
        };
 
        {
            auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config);
 
            validateConfigData(configData);
 
            ARMARX_INFO << VAROUT(configData.kpImpedance);
            ARMARX_INFO << VAROUT(configData.kdImpedance);
            ARMARX_INFO << VAROUT(configData.kpNullspace);
            ARMARX_INFO << VAROUT(configData.kdNullspace);
            ARMARX_INFO << VAROUT(configData.desiredPose);
            ARMARX_INFO << VAROUT(configData.desiredTwist);
            if (configData.desiredNullspaceJointAngles.has_value())
                ARMARX_INFO << VAROUT(configData.desiredNullspaceJointAngles.value());
            ARMARX_INFO << VAROUT(configData.torqueLimit);
            ARMARX_INFO << VAROUT(configData.qvelFilter);
 
            bufferUserToAdditionalTask.reinitAllBuffers(configData);
            bufferUserToRt.reinitAllBuffers(configData);
        }
 
        controlTargets.targets.resize(rtRns->getSize(), 0.);
    }
 
    std::string
    NJointTaskspaceBimanualImpedanceController::getClassName(const Ice::Current&) const
    {
        return "NJointTaskspaceBimanualImpedanceController";
    }
 
    // std::string NJointTaskspaceBimanualImpedanceController::getKinematicChainName(const Ice::Current&)
    // {
    //     return kinematicChainName;
    // }
 
    void
    NJointTaskspaceBimanualImpedanceController::onInitNJointController()
    {
        runTask("NJointTaskspaceAdmittanceAdditionalTask",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT
                            << "Create a new thread alone NJointTaskspaceAdmittanceController";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointTaskspaceBimanualImpedanceController::additionalTask()
    {
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        controller.updateControlStatus(
                bufferUserToAdditionalTask.getUpToDateReadBuffer(),
                bufferRtToAdditionalTask.getUpToDateReadBuffer());
    }
 
    void
    NJointTaskspaceBimanualImpedanceController::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);
        }
 
        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
    NJointTaskspaceBimanualImpedanceController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                                     const Ice::Current& iceCurrent)
    {
        auto configData = ::armarx::fromAron<AronDTO, BO>(dto);
 
        validateConfigData(configData);
 
        bufferUserToAdditionalTask.getWriteBuffer() = configData;
        bufferUserToAdditionalTask.commitWrite();
 
        bufferUserToRt.getWriteBuffer() = configData;
        bufferUserToRt.commitWrite();
    }
 
    void
    NJointTaskspaceBimanualImpedanceController::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.kdNullspace);
        checkSize(configData.kpNullspace);
 
        checkNonNegative(configData.kdNullspace);
        checkNonNegative(configData.kpNullspace);
        checkNonNegative(configData.kdImpedance);
        checkNonNegative(configData.kpImpedance);
    }
 
    void
    NJointTaskspaceBimanualImpedanceController::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::debugEigenPose(datafields, "current_pose", nonRtData.currentPose);
        common::debugEigenPose(datafields, "desired_pose", configData.desiredPose);
        common::debugEigenVec(datafields, "kpImpedance", configData.kpImpedance);
        common::debugEigenVec(datafields, "kdImpedance", configData.kdImpedance);
        common::debugEigenVec(datafields, "forceImpedance", rtData.forceImpedance);
        common::debugEigenVec(datafields, "desiredJointTorques", rtData.desiredJointTorques);
 
        debugObs->setDebugChannel("NJointTaskspaceBimanualImpedanceController", datafields);
    }
 
 
    void
    NJointTaskspaceBimanualImpedanceController::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);
        if (reInitPreActivate.load())
        {
            auto& c = bufferUserToAdditionalTask.getWriteBuffer();
            c.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
            c.desiredPose = currentPose;
 
            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
    NJointTaskspaceBimanualImpedanceController::rtPostDeactivateController()
    {
        controller.isInitialized.store(false);
    }
} /// namespace armarx::control::njoint_controller::task_space