SharedMemoryController.cpp

Go to the documentation of this file.

#include "SharedMemoryController.h"
 
#include <boost/date_time/posix_time/posix_time.hpp>
 
#include <Eigen/Dense>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include <armarx/control/common/type.h>
#include <armarx/control/njoint_controller/task_space/Base.h>
#include <armarx/control/njoint_controller/task_space/ColAvoid.h>
#include <armarx/control/njoint_controller/task_space/ColAvoidVel.h>
 
namespace armarx::control::njoint_controller::task_space
{
    namespace base = armarx::control::njoint_controller::task_space;
 
    armarx::NJointControllerRegistration<
        NJointSharedMemoryTaskspaceController<base::NJointTSVelController>>
        registrationControllerNJointTSVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::SharedMemoryTSVel));
 
    armarx::NJointControllerRegistration<
        NJointSharedMemoryTaskspaceController<base::NJointTSVelColController>>
        registrationControllerNJointSharedMemoryTSVelColController(
        armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSVelCol));
 
    armarx::NJointControllerRegistration<
        NJointSharedMemoryTaskspaceController<base::NJointTSVeloColController>>
        registrationControllerNJointSharedMemoryTSVeloColController(
        armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSVeloCol));
 
    armarx::NJointControllerRegistration<
        NJointSharedMemoryTaskspaceController<base::NJointTSImpController>>
        registrationControllerNJointTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::SharedMemoryTSImp));
 
    armarx::NJointControllerRegistration<NJointSharedMemoryTaskspaceController<
        base::NJointTSMixImpVelController>>
        registrationControllerNJointTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::SharedMemoryTSMixImpVel));
 
    armarx::NJointControllerRegistration<NJointSharedMemoryTaskspaceController<
        base::NJointTSImpColController>>
        registrationControllerNJointCollisionAvoidanceTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::SharedMemoryTSImpCol));
 
    armarx::NJointControllerRegistration<NJointSharedMemoryTaskspaceController<
        base::NJointTSMixImpVelColController>>
        registrationControllerNJointCollisionAvoidanceTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::SharedMemoryTSMixImpVelCol));
 
    // Implement getClassName() method for templated NJointSharedMemoryTaskspaceController
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSVelController>::getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSVel);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSVelColController>::getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSVelCol);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSVeloColController>::getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSVeloCol);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSImpController>::getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSImp);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSMixImpVelController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSMixImpVel);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSImpColController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSImpCol);
    }
 
    template <>
    std::string
    NJointSharedMemoryTaskspaceController<
        class base::NJointTSMixImpVelColController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::SharedMemoryTSMixImpVelCol);
    }
 
    template <typename NJointTaskspaceController>
    NJointSharedMemoryTaskspaceController<NJointTaskspaceController>::
        NJointSharedMemoryTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                              const armarx::NJointControllerConfigPtr& config,
                                              const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController{robotUnit, config, robot}
    {
        init();
    }
 
    // Method(s) of NJointSharedMemoryTaskspaceController in general
    template <typename NJointTaskspaceController>
    void
    NJointSharedMemoryTaskspaceController<NJointTaskspaceController>::init()
    {
        writer = std::make_unique<SharedMemoryWriter<ToSharedMemory>>(
            "NJointSharedMemoryTaskspaceController_rt2shared_memory",
            "NJointSharedMemoryTaskspaceController_rt2shared_memory_mutex");
 
        reader = std::make_unique<SharedMemoryReader<FromSharedMemory>>(
            "NJointSharedMemoryTaskspaceController_shared_memory2rt",
            "NJointSharedMemoryTaskspaceController_shared_memory2rt_mutex");
 
        ARMARX_CHECK(this->limb.size() <= MAX_N)
            << "NJointSharedMemoryTaskspaceController currently supports maximum " << MAX_N
            << " limbs!";
 
        if (this->hands)
        {
            ARMARX_CHECK(this->hands->hands.size() == 0 or
                         this->hands->hands.size() == this->limb.size())
                << "NJointSharedMemoryTaskspaceController currently supports either zero hands or "
                   "n_hands = n_limbs!";
        }
 
        writeConfig();
 
        ARMARX_INFO << "-- NJointSharedMemoryTaskspaceController initialized --";
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointSharedMemoryTaskspaceController<NJointTaskspaceController>::additionalTask()
    {
        const auto [rtTargetSafe, ftSafe] = this->additionalTaskUpdateStatus();
 
        if (not writer->sem.timed_wait(boost::posix_time::microsec_clock::universal_time() +
                                       boost::posix_time::milliseconds(5)))
        {
            return;
        }
 
        // Compute time
        for (auto& pair : this->limb)
        {
            pair.second->nonRTDeltaT =
                pair.second->rtStatusInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
            pair.second->nonRTAccumulateTime = pair.second->rtStatusInNonRT.accumulateTime;
        }
        if (this->hands)
        {
            for (auto& pair : this->hands->hands)
            {
                pair.second->nonRTDeltaT =
                    pair.second->rtsInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
                pair.second->nonRTAccumulateTime = pair.second->rtsInNonRT.accumulateTime;
            }
        }
 
 
        ToSharedMemory writeData;
        {
            unsigned int index = 0;
            unsigned int indexJSC = 0;
            for (auto& pair : this->limb)
            {
                auto& arm = pair.second;
 
                // Taskspace (Current actual pose and twist)
                Eigen::Matrix<float, 7, 1> currentPose;
                currentPose.head(3) = simox::math::mat4f_to_pos(arm->rtStatusInNonRT.currentPose);
                currentPose.tail(4) = simox::math::mat4f_to_quat(arm->rtStatusInNonRT.currentPose)
                                          .coeffs(); // x,y,z,w
                std::memcpy(
                    writeData.currentPose + (index * 7), currentPose.data(), sizeof(float) * 7);
                std::memcpy(writeData.currentTwist + (index * 6),
                            arm->rtStatusInNonRT.currentTwist.data(),
                            sizeof(float) * 6);
 
                // Taskspace (Current desired pose)
                Eigen::Matrix<float, 7, 1> currentDesiredPose;
                currentDesiredPose.head(3) =
                    simox::math::mat4f_to_pos(arm->rtStatusInNonRT.desiredPose);
                currentDesiredPose.tail(4) =
                    simox::math::mat4f_to_quat(arm->rtStatusInNonRT.desiredPose)
                        .coeffs(); // x,y,z,w
                std::memcpy(writeData.currentDesiredPose + index * 7,
                            currentDesiredPose.data(),
                            sizeof(float) * 7);
 
                // Forces
                std::memcpy(writeData.currentForceTorque + (index * 6),
                            arm->rtStatusInNonRT.currentForceTorque.data(),
                            sizeof(float) * 6);
 
                {
                    // Joint Space
                    const unsigned int n = arm->rtStatusInNonRT.jointPosition.size();
                    ARMARX_CHECK_LESS_EQUAL(indexJSC + n, MAX_JS_DIM_N);
                    std::memcpy(writeData.currentJointAngles + indexJSC,
                                arm->rtStatusInNonRT.jointPosition.data(),
                                sizeof(float) * n);
                    std::memcpy(writeData.currentJointVelocities + indexJSC,
                                arm->rtStatusInNonRT.qvelFiltered.data(),
                                sizeof(float) * n);
                    indexJSC += n;
                }
 
                //writeData.deltaTinSeconds = arm->rtStatusInNonRT.deltaT;
                writeData.deltaTinSeconds = arm->nonRTDeltaT;
 
                writeAdditonalDataToSM(writeData, arm->nonRtConfig, index);
 
                index++;
            }
        }
 
        if (this->hands)
        {
            unsigned int index = 0;
            for (auto& [_, hand] : this->hands->hands)
            {
                const unsigned int n = hand->targetNonRT.jointPosition.value().size();
                ARMARX_CHECK_LESS_EQUAL(index + n, MAX_HC_DIM_N);
                std::memcpy(writeData.currentHandConfig + index,
                            hand->targetNonRT.jointPosition->data(),
                            sizeof(float) * n);
                index += n;
            }
        }
 
        writeCoordinatorDataToSM(writeData);
 
        writeData.rtSafe = rtTargetSafe;
        writeData.n_limbs = this->limb.size();
 
        writer->updateNonLocking(writeData);
 
        reader->sem.post(); // notify shared_memory that writing is done
 
        if (not rtTargetSafe)
        {
            this->handleRTNotSafeInNonRT();
        }
 
        // wait for shared_memory to have read and written but maximum 5ms
        // (e.g., when shared_memory code was quit)
        if (not writer->sem.timed_wait(boost::posix_time::microsec_clock::universal_time() +
                                       boost::posix_time::milliseconds(5)))
        {
            ARMARX_RT_LOGF_WARN("Waiting for semaphore timed out");
            return;
        }
 
        FromSharedMemory readData;
        reader->updateNonLocking(readData);
 
        if (readData.initialized)
        {
            unsigned int index = 0;
            unsigned int indexJSC = 0;
            for (auto& pair : this->limb)
            {
                auto& arm = pair.second;
 
                // Task space
                Eigen::Matrix<float, 7, 1> desiredPose;
                std::memcpy(
                    desiredPose.data(), readData.desiredPose + (index * 7), sizeof(float) * 7);
                const Eigen::Vector3f& pos = desiredPose.head(3);
                const Eigen::Vector4f& orientation = desiredPose.tail(4);
                const Eigen::Matrix4f pose =
                    simox::math::pos_quat_to_mat4f(pos, Eigen::Quaternionf(orientation));
                arm->nonRtConfig.desiredPose = pose;
                std::memcpy(arm->nonRtConfig.desiredTwist.data(),
                            readData.desiredTwist + (index * 6),
                            sizeof(float) * 6);
 
                if (readData.useJointAngles)
                {
                    // Joint space
                    const unsigned int n = arm->nonRtConfig.desiredNullspaceJointAngles->size();
                    ARMARX_CHECK_LESS_EQUAL(indexJSC + n, MAX_JS_DIM_N);
                    std::memcpy(arm->nonRtConfig.desiredNullspaceJointAngles->data(),
                                readData.desiredJointAngles + indexJSC,
                                sizeof(float) * n);
                    indexJSC += n;
                }
 
                readAdditonalDataFromSM(readData, arm->nonRtConfig, index);
 
                // ARMARX_RT_LOGF_INFO(
                //     "new target \n\n [%.2f]\n\n", pos(2)).deactivateSpam(1);
 
                index++;
            }
        }
        if (readData.initialized and this->hands)
        {
            unsigned int index = 0;
            for (auto& [_, hand] : this->hands->hands)
            {
                const unsigned int n = hand->targetNonRT.jointPosition.value().size();
                ARMARX_CHECK_LESS_EQUAL(index + n, MAX_HC_DIM_N);
                std::memcpy(hand->targetNonRT.jointPosition->data(),
                            readData.desiredHandConfig + index,
                            sizeof(float) * n);
                index += n;
            }
        }
 
        if (readData.initialized)
        {
            readCoordinatorDataFromSM(readData);
        }
 
        this->additionalTaskSetTarget();
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointSharedMemoryTaskspaceController<NJointTaskspaceController>::writeConfig()
    {
        // TODO optional write config (joint names, etc.) to shared memory
    }
 
} // namespace armarx::control::njoint_controller::task_space