SyncCoordination.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/>.
 *
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <map>
#include <mutex>
#include <string>
 
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h>
#include <RobotAPI/interface/aron/Aron.h>
 
#include <armarx/control/common/common.aron.generated.h>
#include <armarx/control/common/coordination/CoordinationCore.h>
#include <armarx/control/common/coordination/aron/CoordinationConfig.aron.generated.h>
 
namespace armarx::control::common::coordination
{
    /* experimental codee for sync mode
     * */
    class SyncCoordination : public Coordination
    {
    public:
        using Config = arondto::SyncModeConfig;
        //        using PoseFrameMode = armarx::control::common::arondto::pose_frame_mode_details::Enum;
 
        SyncCoordination(const ::armarx::aron::data::dto::DictPtr& dto);
        ~SyncCoordination() override = default;
 
        struct Data
        {
            /// Note: all positions in matrix4f poses are in milli-meter
            /// but in grasp matrix, it must be in meter
            double deltaT = 0.0;
            Eigen::Vector6f virtualAcc;
            Eigen::Vector6f virtualVel;
            Eigen::Matrix4f virtualPose;
 
            Eigen::Matrix4f targetPose;
            Eigen::Matrix4f currentPose;
            Eigen::Vector6f currentVel;
            Eigen::VectorXf bimanualVel; // 12-d vec
 
            Eigen::Vector6f currentFT;
            Eigen::VectorXf bimanualFT;
            // Eigen::VectorXf wrenchTarget;
            Eigen::Vector6f deltaPosForWrenchControlL; // should be in milli-meter
            Eigen::Vector6f deltaPosForWrenchControlR;
 
            /// Temporal variables to avoid memory allocation
            Eigen::Vector3f rvec;
            Eigen::Vector6f poseError;
            Eigen::Matrix3f oriError;
            Eigen::Vector6f tempAcc;
            Eigen::Vector6f tempVel;
            Eigen::Vector6f deltaObjPose;
 
            /// vectors represented in the object local frame
            Eigen::Vector3f obj2TcpInMeterL, obj2TcpInMeterR;
            Eigen::Vector3f tcp2ObjInMilliMeterL, tcp2ObjInMilliMeterR;
 
            Eigen::MatrixXf graspMatrix; // should be (6, 12), position in meter
            Eigen::MatrixXf pinvGraspMatrixT;
 
            // Eigen::Matrix3f fixedLeftRightRotOffset;
            Eigen::Matrix3f dOriInObjL; // the {obj}^T_{left}
            Eigen::Matrix3f dOriInObjR; // the {obj}^T_{right}
            Eigen::Matrix3f dOriInTCPL; // the {obj}^T_{left}
            Eigen::Matrix3f dOriInTCPR; // the {obj}^T_{right}
 
            void updateGraspMatrix(const bool leftAsLeader,
                                   const bool rightAsLeader,
                                   const bool followerIsolation);
            void reset(const Eigen::Matrix4f& desiredPose,
                       Eigen::Matrix4f& leftPose,
                       Eigen::Matrix4f& rightPose,
                       PoseFrameMode leftPoseMode,
                       PoseFrameMode rightPoseMode);
            void resetGraspVariableLeft(const Eigen::Matrix4f& desiredPose,
                                        Eigen::Matrix4f& leftPose,
                                        PoseFrameMode leftPoseMode);
            void resetGraspVariableRight(const Eigen::Matrix4f& desiredPose,
                                         Eigen::Matrix4f& rightPose,
                                         PoseFrameMode rightPoseMode);
        };
 
        void rtPreActivate() final;
        void reset(std::map<std::string, Eigen::Matrix4f>& initPose) override;
        void runRT(std::map<std::string, InputData>& input, double deltaT) override;
        void updateNonRt() override;
        void commitNonRt() override;
 
        void resetBufferUserCfgAfterMP();
 
        void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                          bool flagResetRt = false) override;
        void updateTargetPose(const Eigen::Matrix4f& targetPose);
 
        // bool isInitialized() override;
        // const std::vector<std::string>& getNodesetList();
        // std::string& getLeftNodesetName();
        // std::string& getRightNodesetName();
 
        /// TODO private?
        mutable std::recursive_mutex mtx_data_non_rt;
        Data dataNonRt;
        TripleBuffer<Data> bufferDataRtToPublish;
        TripleBuffer<Config> bufferUserCfgToPublish;
 
        Config cfgInNonRt; /// for non rt
 
    private:
        Data dataRt_;
        TripleBuffer<Data> bufferDataRtToNonRt_;
 
        Config cfg_; /// for user
        Config cfgInRT_; /// for rt
        TripleBuffer<Config> bufferNonRtCfgToRt_;
        TripleBuffer<Config> bufferUserCfgToNonRt_;
        std::atomic_bool cfgBufferInitialized_{false};
 
        static void computeWrenchControl(Eigen::Vector6f& deltaPos,
                                         const Eigen::Vector6f& wrenchTarget,
                                         const Eigen::Vector6f& stiffness);
        static void setObjPoseFromHandPoses(Eigen::Matrix4f& objPose,
                                            const Eigen::Matrix4f& poseL,
                                            const Eigen::Matrix4f& poseR);
 
        void updateCurrentPose(Eigen::Matrix4f& leftPose, Eigen::Matrix4f& rightPose);
        // static void averageSE3(const Eigen::Matrix4f& pose1, const Eigen::Matrix4f& pose2, Eigen::Matrix4f& pose_avg);
    };
 
} // namespace armarx::control::common::coordination