SyncCoordination.cpp

Go to the documentation of this file.

#include "SyncCoordination.h"
 
#include <cmath>
#include <cstddef>
#include <mutex>
 
#include <VirtualRobot/MathTools.h>
 
#include "../utils.h"
 
namespace armarx::control::common::coordination
{
 
    SyncCoordination::SyncCoordination(const ::armarx::aron::data::dto::DictPtr& dto)
    {
        updateConfig(dto, true);
    }
 
    void
    SyncCoordination::setObjPoseFromHandPoses(Eigen::Matrix4f& objPose,
                                              const Eigen::Matrix4f& poseL,
                                              const Eigen::Matrix4f& poseR)
    {
        /// TODO replaced by update current pose
        setPos(objPose, 0.5f * (getPos(poseL) + getPos(poseR)));
        setOri(objPose, getOri(poseL));
    }
 
    void
    SyncCoordination::Data::reset(const Eigen::Matrix4f& desiredPose,
                                  Eigen::Matrix4f& leftPose,
                                  Eigen::Matrix4f& rightPose,
                                  PoseFrameMode leftPoseMode,
                                  PoseFrameMode rightPoseMode
                                  /*bool followerInLeaderLocalFrame*/)
    {
        /// all positions in poses are in milli-meter
        virtualAcc.setZero();
        virtualVel.setZero();
 
        virtualPose = desiredPose;
        // setObjPoseFromHandPoses(virtualPose, leftPose, rightPose);
 
        targetPose = desiredPose;
        currentPose = desiredPose;
        currentVel.setZero();
        bimanualVel.setZero(12);
 
        currentFT.setZero();
        bimanualFT.setZero(12);
        // wrenchTarget.setZero(12);
        deltaPosForWrenchControlL.setZero();
        deltaPosForWrenchControlR.setZero();
 
        rvec.setZero();
        poseError.setZero();
        oriError.setZero();
        tempAcc.setZero();
        tempVel.setZero();
        deltaObjPose.setZero();
 
        graspMatrix.setZero(6, 12);
        pinvGraspMatrixT.setZero(6, 12);
        dOriInObjL.setZero();
        dOriInObjR.setZero();
        dOriInTCPL.setZero();
        dOriInTCPR.setZero();
        obj2TcpInMeterL.setZero();
        obj2TcpInMeterR.setZero();
        tcp2ObjInMilliMeterL.setZero();
        tcp2ObjInMilliMeterR.setZero();
 
 
        //        resetGraspVariable(desiredPose,
        //                           leftPose,
        //                           rightPose,
        //                           leftPoseMode,
        //                           rightPoseMode,
        //                           true,
        //                           true,
        //                           followerInLeaderLocalFrame);
        //        ARMARX_INFO << "rt run, reset rtData" << VAROUT(desiredPose) << VAROUT(leftPose)
        //                    << VAROUT(rightPose);
        //        ARMARX_INFO << VAROUT(obj2TcpInMeterL) << VAROUT(obj2TcpInMeterR);
    }
 
    void
    SyncCoordination::Data::resetGraspVariableLeft(const Eigen::Matrix4f& desiredPose,
                                                   Eigen::Matrix4f& leftPose,
                                                   PoseFrameMode leftPoseMode/*,
                                                   bool followerInLeaderLocalFrame*/)
    {
        /// Left hand group is a follower
        if (leftPoseMode == PoseFrameMode::leader)
        {
            /// leftPose is represented in the local frame of desiredPose of the object in right hand group
            obj2TcpInMeterL = getPos(leftPose) * 0.001f;
            dOriInObjL = getOri(leftPose);
 
            tcp2ObjInMilliMeterL = -getOriT(leftPose) * getPos(leftPose);
            dOriInTCPL = getOriT(leftPose);
            //            ARMARX_INFO << "update left in local: " << VAROUT(leftPoseMode)
            //                        << VAROUT(obj2TcpInMeterL) << VAROUT(dOriInObjL);
        }
        else /// Left hand group is leader, or follower in sync mode
        {
            /// representation in object local frame (meter)
            obj2TcpInMeterL =
                getOriT(desiredPose) * (getPos(leftPose) - getPos(desiredPose)) * 0.001f;
            dOriInObjL = getOriT(desiredPose) * getOri(leftPose);
 
            /// representation in tcp local frame (milli meter)
            tcp2ObjInMilliMeterL = getOriT(leftPose) * (getPos(desiredPose) - getPos(leftPose));
            dOriInTCPL = getOriT(leftPose) * getOri(desiredPose);
            //            ARMARX_INFO << "update left in root: " << VAROUT(obj2TcpInMeterL) << VAROUT(dOriInObjL);
        }
    }
 
    void
    SyncCoordination::Data::resetGraspVariableRight(const Eigen::Matrix4f& desiredPose,
                                                    Eigen::Matrix4f& rightPose,
                                                    PoseFrameMode rightPoseMode/*,
                                                    bool followerInLeaderLocalFrame*/)
    {
        /// Right hand group is a follower
        if (rightPoseMode == PoseFrameMode::leader)
        {
            /// rightPose is represented in the local frame of desiredPose of the object in left hand group
            obj2TcpInMeterR = getPos(rightPose) * 0.001f;
            dOriInObjR = getOri(rightPose);
 
            tcp2ObjInMilliMeterR = -getOriT(rightPose) * getPos(rightPose);
            dOriInTCPR = getOriT(rightPose);
            //            ARMARX_INFO << "update right in local: " << VAROUT(rightPoseMode) << " "
            //                        << VAROUT(rightPose) << "\n"
            //                        << VAROUT(desiredPose) << VAROUT(obj2TcpInMeterR) << VAROUT(dOriInObjR);
        }
        else
        {
            /// Right hand group is a follower
            /// representation in object local frame (meter)
            obj2TcpInMeterR =
                getOriT(desiredPose) * (getPos(rightPose) - getPos(desiredPose)) * 0.001f;
            dOriInObjR = getOriT(desiredPose) * getOri(rightPose);
 
            /// representation in tcp local frame (milli meter)
            tcp2ObjInMilliMeterR = getOriT(rightPose) * (getPos(desiredPose) - getPos(rightPose));
            dOriInTCPR = getOriT(rightPose) * getOri(desiredPose);
            //            ARMARX_INFO << "update right in root: " << VAROUT(obj2TcpInMeterR)
            //                        << VAROUT(dOriInObjR);
        }
    }
 
 
    void
    SyncCoordination::rtPreActivate()
    {
        // Reinit all buffers
        
        if (cfg_.flagResetRt)
        {
            initialized_.store(false);
        }
 
        if (not cfgBufferInitialized_.load())
        {
            bufferNonRtCfgToRt_.reinitAllBuffers(cfg_);
            bufferUserCfgToNonRt_.reinitAllBuffers(cfg_);
            bufferUserCfgToPublish.reinitAllBuffers(cfg_);
            cfgBufferInitialized_.store(true);
        }
        else
        {
            bufferNonRtCfgToRt_.getWriteBuffer() = cfg_;
            bufferNonRtCfgToRt_.commitWrite();
            bufferUserCfgToNonRt_.getWriteBuffer() = cfg_;
            bufferUserCfgToNonRt_.commitWrite();
            bufferUserCfgToPublish.getWriteBuffer() = cfg_;
            bufferUserCfgToPublish.commitWrite();
        }
    }
 
    void
    SyncCoordination::reset(std::map<std::string, Eigen::Matrix4f>& initPose)
    {
        /// Not used anymore
        //        const std::scoped_lock lock(mtx_data_non_rt);
        //        dataRt_.reset(initPose.at(cfg_.nodesetLeft), initPose.at(cfg_.nodesetRight));
        //        dataNonRt = dataRt_;
        //        bufferDataRtToNonRt_.reinitAllBuffers(dataRt_);
        //        initialized_.store(true);
        //        ARMARX_INFO << "coordinator initialized";
    }
 
    void
    SyncCoordination::updateNonRt()
    {
        const std::scoped_lock lock(mtx_data_non_rt);
        dataNonRt = bufferDataRtToNonRt_.getUpToDateReadBuffer();
        cfgInNonRt = bufferUserCfgToNonRt_.getUpToDateReadBuffer();
    }
 
    void
    SyncCoordination::commitNonRt()
    {
        bufferNonRtCfgToRt_.getWriteBuffer() = cfgInNonRt;
        bufferNonRtCfgToRt_.commitWrite();
    }
 
    void
    SyncCoordination::resetBufferUserCfgAfterMP()
    {
        bufferUserCfgToNonRt_.reinitAllBuffers(cfgInNonRt);
    }
 
    void
    SyncCoordination::Data::updateGraspMatrix(const bool leftAsLeader,
                                              const bool rightAsLeader,
                                              const bool followerIsolation
                                              /*const Eigen::Matrix4f& desiredPose,
                                              Eigen::Matrix4f& leftPose,
                                              Eigen::Matrix4f& rightPose,
                                              PoseFrameMode leftPoseMode,
                                              PoseFrameMode rightPoseMode,
                                              const bool followerInLeaderLocalFrame*/)
    {
        graspMatrix.setZero(6, 12);
 
        //        /// Reset the grasp variables when one arm is a leader and the other a follower
        //        if (rightAsLeader)
        //        {
        //            /// always reset left grasp variable
        //            resetGraspVariableLeft(desiredPose, leftPose, leftPoseMode, followerInLeaderLocalFrame);
        //        }
        //        if (leftAsLeader)
        //        {
        //            /// reset right grasp variable
        //            resetGraspVariableRight(
        //                desiredPose, rightPose, rightPoseMode, followerInLeaderLocalFrame);
        //        }
        //        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
        //        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
        if (not followerIsolation or not rightAsLeader)
        {
            graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
            rvec = getOri(virtualPose) * obj2TcpInMeterL;
            skew(rvec, graspMatrix.block<3, 3>(3, 0));
        }
        if (not followerIsolation or not leftAsLeader)
        {
            graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
            rvec = getOri(virtualPose) * obj2TcpInMeterR;
            skew(rvec, graspMatrix.block<3, 3>(3, 6));
        }
 
        float lambda = 1;
        pinvGraspMatrixT =
            VirtualRobot::MathTools::getPseudoInverseDamped(graspMatrix.transpose(), lambda);
    }
 
    void
    SyncCoordination::computeWrenchControl(Eigen::Vector6f& deltaPos,
                                           const Eigen::Vector6f& wrenchTarget,
                                           const Eigen::Vector6f& stiffness)
    {
        deltaPos.setZero();
        for (size_t i = 0; i < 6; ++i)
        {
            if (stiffness(i) == 0)
            {
                deltaPos(i) = 0;
            }
            else
            {
                deltaPos(i) = wrenchTarget(i) / stiffness(i);
                if (deltaPos(i) > 0.1f)
                {
                    deltaPos(i) = 0.1f;
                }
                else if (deltaPos(i) < -0.1f)
                {
                    deltaPos(i) = -0.1f;
                }
            }
        }
    }
 
    void
    SyncCoordination::updateCurrentPose(Eigen::Matrix4f& leftPose, Eigen::Matrix4f& rightPose)
    {
        /// when current pose is None, it will be updated based on either left or right arm poses with rigid connection assumption
        /// if one arm is leader, current pose can be updated based on rigid transformation to leader
        /// otherwise both arms are follower paired to leader object, pose is updated based on both arms
 
        if (cfgInRT_.currentPose.has_value())
        {
            dataRt_.currentPose = cfgInRT_.currentPose.value();
            return;
        }
 
        if (cfgInRT_.leadershipMode == arondto::LeadershipMode::left_as_leader)
        {
            setPos(dataRt_.currentPose,
                   getPos(leftPose) + getOri(leftPose) * dataRt_.tcp2ObjInMilliMeterL);
            setOri(dataRt_.currentPose, getOri(leftPose) * dataRt_.dOriInTCPL);
            //            ARMARX_INFO << "left leader: " << VAROUT(getPos(leftPose))
            //                        << VAROUT(dataRt_.tcp2ObjInMilliMeterL) << VAROUT(dataRt_.currentPose);
            return;
        }
 
        if (cfgInRT_.leadershipMode == arondto::LeadershipMode::right_as_leader)
        {
            setPos(dataRt_.currentPose,
                   getPos(rightPose) + getOri(rightPose) * dataRt_.tcp2ObjInMilliMeterR);
            setOri(dataRt_.currentPose, getOri(rightPose) * dataRt_.dOriInTCPR);
            return;
        }
 
        /// both left and right arms are follower
        setPos(dataRt_.currentPose,
               0.5f * (getPos(leftPose) + getOri(leftPose) * dataRt_.tcp2ObjInMilliMeterL +
                       getPos(rightPose) + getOri(rightPose) * dataRt_.tcp2ObjInMilliMeterR));
 
        const Eigen::Matrix3f R1 = (getOri(leftPose) * dataRt_.dOriInTCPL).topLeftCorner<3, 3>();
        const Eigen::Matrix3f R2 = (getOri(rightPose) * dataRt_.dOriInTCPR).topLeftCorner<3, 3>();
 
        // Convert to quaternions (no dynamic allocation here?)
        Eigen::Quaternionf q1(R1);
        Eigen::Quaternionf q2(R2);
 
        // Slerp rotation (t=0.5 midpoint)
        Eigen::Quaternionf q_avg = q1.slerp(0.5f, q2);
        q_avg.normalize();
 
        setOri(dataRt_.currentPose, q_avg.toRotationMatrix());
    }
 
    // // In-place average of two SE(3) matrices without heap allocation
    // static void
    // SyncCoordination::averageSE3(const Eigen::Matrix4f& pose1,
    //                        const Eigen::Matrix4f& pose2,
    //                        Eigen::Matrix4f& pose_avg)
    // {
    //     // Extract rotation matrices
    //     const Eigen::Matrix3f R1 = pose1.topLeftCorner<3,3>();
    //     const Eigen::Matrix3f R2 = pose2.topLeftCorner<3,3>();
    //
    //     // Convert to quaternions (no dynamic allocation here)
    //     Eigen::Quaternionf q1(R1);
    //     Eigen::Quaternionf q2(R2);
    //
    //     // Slerp rotation (t=0.5 midpoint)
    //     Eigen::Quaternionf q_avg = q1.slerp(0.5f, q2);
    //     q_avg.normalize();
    //
    //     // Average translation directly
    //     const Eigen::Vector3f pose1 = pose1.topRightCorner<3,1>();
    //     const Eigen::Vector3f pose2 = pose2.topRightCorner<3,1>();
    //     const Eigen::Vector3f t_avg = 0.5f * (pose1 + pose2);
    //
    //     // Assemble averaged transformation (overwrite in-place)
    //     pose_avg.setIdentity();
    //     pose_avg.topLeftCorner<3,3>() = q_avg.toRotationMatrix();
    //     pose_avg.topRightCorner<3,1>() = t_avg;
    // }
 
 
    void
    SyncCoordination::runRT(std::map<std::string, InputData>& input, double deltaT)
    {
        dataRt_.deltaT = deltaT;
        cfgInRT_ = bufferNonRtCfgToRt_.getUpToDateReadBuffer();
 
        auto& iL = input.at(cfgInRT_.nodesetLeft);
        auto& iR = input.at(cfgInRT_.nodesetRight);
 
        // not changing left/right pose if coordination is not initialized,
        if (not initialized_.load())
        {
            if (not cfgInRT_.flagResetRt)
            {
                return;
            }
 
            const std::scoped_lock lock(mtx_data_non_rt);
            dataRt_.reset(cfgInRT_.desiredPose,
                          iL.targetPose,
                          iR.targetPose,
                          iL.targetPoseMode,
                          iR.targetPoseMode/*,
                          cfgInRT_.followerInLeaderLocalFrame*/);
            //            ARMARX_INFO << VAROUT(iL.targetPoseMode) << VAROUT(iR.targetPoseMode);
        }
 
        /// init left grasp variable: local mode targetPoseMode is in leader
        const bool leftAsLeader =
            cfgInRT_.leadershipMode == arondto::LeadershipMode::left_as_leader;
        const bool rightAsLeader =
            cfgInRT_.leadershipMode == arondto::LeadershipMode::right_as_leader;
        if (not initialized_.load() or rightAsLeader)
        {
            dataRt_.resetGraspVariableLeft(cfgInRT_.desiredPose, iL.targetPose, iL.targetPoseMode);
        }
        if (not initialized_.load() or leftAsLeader)
        {
            dataRt_.resetGraspVariableRight(cfgInRT_.desiredPose, iR.targetPose, iR.targetPoseMode);
        }
        dataRt_.updateGraspMatrix(leftAsLeader,
                                  rightAsLeader,
                                  cfgInRT_.followerConnectionMode ==
                                      arondto::ConnectionMode::follower_isolated);
        if (not initialized_.load())
        {
            dataNonRt = dataRt_;
            bufferDataRtToNonRt_.reinitAllBuffers(dataRt_);
            bufferDataRtToPublish.reinitAllBuffers(dataRt_);
            initialized_.store(true);
            ARMARX_INFO << "coordinator initialized";
        }
 
        /// position in milli-meter
        updateCurrentPose(iL.pose, iR.pose);
        //        setPos(dataRt_.currentPose, 0.5f * (getPos(iL.pose) + getPos(iR.pose)));
        //        setOri(dataRt_.currentPose, getOri(iL.pose));
 
        dataRt_.bimanualVel << iL.vel, iR.vel;
        dataRt_.currentVel = dataRt_.pinvGraspMatrixT * dataRt_.bimanualVel;
 
        dataRt_.bimanualFT << iL.ft, iR.ft;
        /// TODO look into force mapping from hand to obj
        dataRt_.currentFT = dataRt_.graspMatrix * dataRt_.bimanualFT;
        for (size_t i = 0; i < 6; i++)
        {
            if (fabs(dataRt_.currentFT(i)) < cfg_.forceDeadzone(i))
            {
                dataRt_.currentFT(i) = 0.0f;
            }
            else
            {
                dataRt_.currentFT(i) -=
                    cfg_.forceDeadzone(i) * dataRt_.currentFT(i) / fabs(dataRt_.currentFT(i));
            }
        }
 
        computeWrenchControl(
            dataRt_.deltaPosForWrenchControlL, cfg_.targetWrenchLeft, iL.stiffness);
        computeWrenchControl(
            dataRt_.deltaPosForWrenchControlR, cfg_.targetWrenchRight, iR.stiffness);
 
        /// ------------------------ Object level admittance control ------------------------
        dataRt_.targetPose = cfgInRT_.desiredPose;
        dataRt_.poseError.head<3>() = getPos(dataRt_.virtualPose) - getPos(dataRt_.targetPose);
        dataRt_.oriError = getOri(dataRt_.virtualPose) * getOriT(dataRt_.targetPose);
        dataRt_.poseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(dataRt_.oriError);
 
        for (int i = 0; i < 6; i++)
        {
            dataRt_.tempAcc(i) = dataRt_.currentFT(i) / cfg_.virtualInertia(i) -
                                 cfg_.virtualStiffness(i) * dataRt_.poseError(i) -
                                 cfg_.virtualDamping(i) * dataRt_.virtualVel(i);
        }
        //        dataRt_.tempAcc = cfg_.virtualInertia.cwiseProduct(dataRt_.currentFT) -
        //                          cfg_.virtualStiffness.cwiseProduct(dataRt_.poseError) -
        //                          cfg_.virtualDamping.cwiseProduct(dataRt_.virtualVel);
        dataRt_.tempVel = dataRt_.virtualVel + 0.5f * static_cast<float>(deltaT) *
                                                   (dataRt_.tempAcc + dataRt_.virtualAcc);
 
        dataRt_.poseError =
            0.5f * static_cast<float>(deltaT) * (dataRt_.virtualVel + dataRt_.tempVel);
 
        dataRt_.virtualVel = dataRt_.tempVel;
        dataRt_.virtualAcc = dataRt_.tempAcc;
        setOri(dataRt_.virtualPose,
               VirtualRobot::MathTools::rpy2eigen3f(
                   dataRt_.poseError(3), dataRt_.poseError(4), dataRt_.poseError(5)) *
                   getOri(dataRt_.virtualPose));
        setPos(dataRt_.virtualPose, getPos(dataRt_.virtualPose) + dataRt_.poseError.head<3>());
 
        /// -------------------------------  convert to tcp pose -------------------------------
        setOri(iL.targetPose, getOri(dataRt_.virtualPose) * dataRt_.dOriInObjL);
        setOri(iR.targetPose, getOri(dataRt_.virtualPose) * dataRt_.dOriInObjR);
        setPos(iL.targetPose,
               getPos(dataRt_.virtualPose) +
                   getOri(dataRt_.virtualPose) * (1000.0f * dataRt_.obj2TcpInMeterL -
                                                  dataRt_.deltaPosForWrenchControlL.head<3>()));
        setPos(iR.targetPose,
               getPos(dataRt_.virtualPose) +
                   getOri(dataRt_.virtualPose) * (1000.0f * dataRt_.obj2TcpInMeterR -
                                                  dataRt_.deltaPosForWrenchControlR.head<3>()));
 
        bufferDataRtToNonRt_.getWriteBuffer() = dataRt_;
        bufferDataRtToNonRt_.commitWrite();
        bufferDataRtToPublish.getWriteBuffer() = dataRt_;
        bufferDataRtToPublish.commitWrite();
    }
 
    void
    SyncCoordination::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                   const bool flagResetRt)
    {
        //        auto prevCfg = cfg_;
        cfg_.fromAron(dto);
        // validate cfg_
        // if not valid, cfg_ = prevCfg;
        //        if (cfg_.leftAsLeader and cfg_.rightAsLeader)
        //        {
        //            ARMARX_WARNING
        //                << "It is not allowed to set both arms as leader, reconfigure your setting";
        //            return;
        //        }
        if (cfg_.leadershipMode == arondto::LeadershipMode::object_as_leader and
            cfg_.followerConnectionMode == arondto::ConnectionMode::follower_isolated)
        {
            ARMARX_WARNING << "When both arms are follower, you cannot set follower isolation "
                              "flag. I changed to `ConnectionMode::follower_connected` for you";
            cfg_.followerConnectionMode = arondto::ConnectionMode::follower_connected;
        }
        if (flagResetRt)
        {
            cfg_.flagResetRt = flagResetRt;
        }
        if (cfg_.flagResetRt)
        {
            initialized_.store(false);
        }
 
        /// TODO: check if current pose is all zero and unset it, seems a bug in ARON
        if (cfg_.currentPose.has_value())
        {
            ARMARX_INFO << "current object pose: " << VAROUT(cfg_.currentPose.value());
            if (cfg_.currentPose.value()(3, 3) < 1.0)
            {
                cfg_.currentPose.reset();
            }
            ARMARX_INFO << "unset current pose, has value? "
                        << VAROUT(cfg_.currentPose.has_value());
        }
        else
        {
            ARMARX_INFO << "User does not provide a current pose";
        }
 
        //        /// TODO: just for comparison, vector works but matrix not
        //        if (cfg_.currentTest.has_value())
        //        {
        //            ARMARX_INFO << "current object test: " << VAROUT(cfg_.currentTest.value());
        //        }
        //        else
        //        {
        //            ARMARX_INFO << "User does not provide a current test";
        //        }
 
        if (not cfgBufferInitialized_.load())
        {
            bufferNonRtCfgToRt_.reinitAllBuffers(cfg_);
            bufferUserCfgToNonRt_.reinitAllBuffers(cfg_);
            bufferUserCfgToPublish.reinitAllBuffers(cfg_);
            cfgBufferInitialized_.store(true);
        }
        else
        {
            bufferNonRtCfgToRt_.getWriteBuffer() = cfg_;
            bufferNonRtCfgToRt_.commitWrite();
            bufferUserCfgToNonRt_.getWriteBuffer() = cfg_;
            bufferUserCfgToNonRt_.commitWrite();
            bufferUserCfgToPublish.getWriteBuffer() = cfg_;
            bufferUserCfgToPublish.commitWrite();
        }
 
        setNodesetList(cfg_.nodesetLeft, cfg_.nodesetRight);
        std::string role;
        if (cfg_.leadershipMode == arondto::LeadershipMode::left_as_leader)
        {
            role = cfg_.nodesetLeft + " as leader";
        }
        else if (cfg_.leadershipMode == arondto::LeadershipMode::right_as_leader)
        {
            role = cfg_.nodesetRight + " as leader";
        }
        else
        {
            role = "object as leader";
        }
 
 
        if (cfg_.followerFrameMode == armarx::control::common::arondto::PoseFrameMode::leader)
        {
            role += ", follower in local frame of leader";
        }
        ARMARX_INFO << "Request coordinator with " << role;
    }
 
    void
    SyncCoordination::updateTargetPose(const Eigen::Matrix4f& targetPose)
    {
        cfg_.desiredPose = targetPose;
        bufferUserCfgToNonRt_.getWriteBuffer() = cfg_;
        bufferUserCfgToNonRt_.commitWrite();
        bufferUserCfgToPublish.getWriteBuffer() = cfg_;
        bufferUserCfgToPublish.commitWrite();
    }
 
} // namespace armarx::control::common::coordination