#pragma once
#ifndef __CartFreeDrag_H__
#define __CartFreeDrag_H__

#include "AdmittanceBasic.h"

class CartFreeDrag : public AdmittanceBasic
{
public:
    void Init();
    void LoadData(WayPoint curWayPt, FTData curForce, WayPoint refWayPt);
    void GetActualTarget(WayPoint& wayPoint);
    void RefreshAdmittancePara();
private:
    void CartDragFunction(WayPoint& wayPoint);

    CartesianPos _curCart;
    CartesianForce _curForce;

    typedef Eigen::DiagonalMatrix<double, 6> Diag6d;

    // 历史数据
    Sophus::Vector6d deltaFkminus1;
    Sophus::Vector6d deltaFkminus2;
    Sophus::Vector6d Vkminus1;
    Sophus::Vector6d Vkminus2;

    Eigen::Matrix4d tool2Basekminus1 = Eigen::Matrix4d::Identity();

    // 阻抗控制参数
    Diag6d M;
    Diag6d B;
    Diag6d K;
    Sophus::Matrix6d Md;
    Sophus::Matrix6d Bd;
    Sophus::Matrix6d Kd;

    // 拖动
    Sophus::Vector6d dragFThr;
    bool isDrag;
    int Y = 0;
    int N = 0;
};
#endif

#pragma once