SharedMemoryController.h

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     Andre Meixner ( andre dot meixner at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
 
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h>
 
#include "SharedMemory.h"
 
 
#define MAX_N 2 // Maximum number of nodesets
#define TS6_DIM_N (MAX_N * 6)
#define TS7_DIM_N (MAX_N * 7)
#define MAX_JS_DIM 8 // Maximum allowed number of joint space size
#define MAX_JS_DIM_N (MAX_N * MAX_JS_DIM)
#define MAX_HC_DIM 4 // Maximum allowed number of joints for hand configuration
#define MAX_HC_DIM_N (MAX_N * MAX_HC_DIM)
 
namespace armarx::control::njoint_controller::task_space
{
    struct ToSharedMemory
    {
        int32_t n_limbs;
        // x1, y1, z1, qx1, qy1, qz1, qw1, x2, ... (with position in mm)
        float currentPose[TS7_DIM_N];
        // x1, y1, z1, roll1, pitch1, yaw1, x2, ...
        float currentTwist[TS6_DIM_N];
        // x1, y1, z1, qx1, qy1, qz1, qw1 (with position in mm)
        float currentCoordinatedPose[7];
        // x1, y1, z1, qx1, qy1, qz1, qw1, x2, ... (with position in mm)
        float currentDesiredPose[TS7_DIM_N];
 
        float kpImpedance[TS6_DIM_N];
        float kdImpedance[TS6_DIM_N];
 
        float currentForceTorque[TS6_DIM_N];
 
         // In radian or mm
        float currentJointAngles[MAX_JS_DIM_N];
        float currentJointVelocities[MAX_JS_DIM_N];
 
         // In Radian
        float currentHandConfig[MAX_HC_DIM_N];
 
        float deltaTinSeconds;
        bool rtSafe;
 
        inline static size_t
        size()
        {
            return sizeof(int32_t) + (sizeof(float) * TS7_DIM_N) + (sizeof(float) * TS6_DIM_N) +
                   (sizeof(float) * 7) + (sizeof(float) * TS7_DIM_N) + (sizeof(float) * TS6_DIM_N) +
                   (sizeof(float) * TS6_DIM_N) + (sizeof(float) * TS6_DIM_N) +
                   (sizeof(float) * MAX_JS_DIM_N) + (sizeof(float) * MAX_JS_DIM_N) +
                   (sizeof(float) * MAX_HC_DIM_N) + sizeof(float) + sizeof(bool);
        }
    };
 
    struct FromSharedMemory
    {
        // x1, y1, z1, qx1, qy1, qz1, qw1, x2, ... (with position in mm)
        float desiredPose[TS7_DIM_N];
        // x1, y1, z1, roll1, pitch1, yaw1, x2, ... (currently has no effect)
        float desiredTwist[TS6_DIM_N];
        // x1, y1, z1, qx1, qy1, qz1, qw1 (with position in mm)
        float desiredCoordinatedPose[7];
 
        float kpImpedance[TS6_DIM_N];
        float kdImpedance[TS6_DIM_N];
 
         // In radian or mm
        float desiredJointAngles[MAX_JS_DIM_N];
 
         // In radian
        float desiredHandConfig[MAX_HC_DIM_N];
 
        bool useJointAngles = false;
        bool initialized = false;
 
        inline static size_t
        size()
        {
            return (sizeof(float) * TS7_DIM_N) + (sizeof(float) * TS6_DIM_N) + (sizeof(float) * 7) +
                   (sizeof(float) * TS6_DIM_N) + (sizeof(float) * TS6_DIM_N) +
                   (sizeof(float) * MAX_JS_DIM_N) + (sizeof(float) * MAX_HC_DIM_N) + sizeof(bool) +
                   sizeof(bool);
        }
    };
 
    template <typename NJointTaskspaceController>
    concept ContainsStiffness = requires {
        typename NJointTaskspaceController::Config;
        { std::declval<typename NJointTaskspaceController::Config>().kpImpedance };
    };
 
    template <typename NJointTaskspaceController>
    class NJointSharedMemoryTaskspaceController : virtual public NJointTaskspaceController
    {
    public:
        NJointSharedMemoryTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                              const armarx::NJointControllerConfigPtr& config,
                                              const VirtualRobot::RobotPtr& robot);
 
        void init();
 
        void writeConfig();
 
        //overwritten for specific templates, see .cpp
        std::string
        getClassName(const Ice::Current&) const final
        {
            return "NJointSharedMemoryTaskspaceUnknownController";
        }
 
        void
        writeAdditonalDataToSM(ToSharedMemory& writeData,
                               const typename NJointTaskspaceController::Config& config,
                               unsigned int index) const
        {
            // Default do nothing
        }
 
        void
        readAdditonalDataFromSM(const FromSharedMemory& readData,
                                typename NJointTaskspaceController::Config& config,
                                unsigned int index) const
        {
            // Default do nothing
        }
 
        void
        writeCoordinatorDataToSM(ToSharedMemory& writeData) const
        {
            // Default do nothing
        }
 
        void
        readCoordinatorDataFromSM(const FromSharedMemory& readData)
        {
            // Default do nothing
        }
 
        void
        writeAdditonalDataToSM(ToSharedMemory& writeData,
                               const typename NJointTaskspaceController::Config& config,
                               unsigned int index) const
            requires ContainsStiffness<NJointTaskspaceController>
        {
            std::memcpy(
                writeData.kpImpedance + (index * 6), config.kpImpedance.data(), sizeof(float) * 6);
            std::memcpy(
                writeData.kdImpedance + (index * 6), config.kdImpedance.data(), sizeof(float) * 6);
        }
 
        void
        readAdditonalDataFromSM(const FromSharedMemory& readData,
                                typename NJointTaskspaceController::Config& config,
                                unsigned int index) const
            requires ContainsStiffness<NJointTaskspaceController>
        {
            std::memcpy(
                config.kpImpedance.data(), readData.kpImpedance + (index * 6), sizeof(float) * 6);
 
            std::memcpy(
                config.kdImpedance.data(), readData.kdImpedance + (index * 6), sizeof(float) * 6);
        }
 
        void
        writeCoordinatorDataToSM(ToSharedMemory& writeData) const
            requires ContainsStiffness<NJointTaskspaceController>
        {
            // TODO: Add concept that checks if it contains coordinator
 
            if (this->coordinator)
            {
                this->coordinator->updateNonRt();
 
                Eigen::Matrix<float, 7, 1> currentCoordinatedPose;
                currentCoordinatedPose.head(3) =
                    simox::math::mat4f_to_pos(this->coordinator->dataNonRt.currentPose);
                currentCoordinatedPose.tail(4) =
                    simox::math::mat4f_to_quat(this->coordinator->dataNonRt.currentPose)
                        .coeffs(); // x,y,z,w
                std::memcpy(writeData.currentCoordinatedPose,
                            currentCoordinatedPose.data(),
                            sizeof(float) * 7);
            }
        }
 
        void
        readCoordinatorDataFromSM(const FromSharedMemory& readData)
            requires ContainsStiffness<NJointTaskspaceController>
        {
            // TODO: Add concept that checks if it contains coordinator
 
            if (this->coordinator)
            {
                Eigen::Map<const Eigen::Matrix<float, 7, 1>> desiredCoordinatedPose(
                    readData.desiredCoordinatedPose);
                const Eigen::Vector3f& pos = desiredCoordinatedPose.head(3);
                const Eigen::Vector4f& orientation = desiredCoordinatedPose.tail(4);
                const Eigen::Matrix4f pose =
                    simox::math::pos_quat_to_mat4f(pos, Eigen::Quaternionf(orientation));
                this->coordinator->cfgInNonRt.desiredPose = pose;
 
                this->coordinator->commitNonRt();
            }
        }
 
        void additionalTask() final;
 
    protected:
        std::unique_ptr<SharedMemoryWriter<ToSharedMemory>> writer;
        std::unique_ptr<SharedMemoryReader<FromSharedMemory>> reader;
    };
 
} // namespace armarx::control::njoint_controller::task_space