#pragma once
#ifndef __CartForceScan_H__
#define __CartForceScan_H__

#include "AdmittanceBasic.h"

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

    CartesianPos _curCart;
    CartesianForce _curForce;
    CartesianPos _refCart;

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

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

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

    bool first;

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

};
#endif

#pragma once