impedance control for robot arm through C++

Define parameters

Initialize M, B, K:

Eigen::VectorXd Mm(6,1)
    Mm<<1./M_x,1./M_y,1./M_z,1./M_rx,1./M_ry,1./M_rz;
auto Mr = Mm.asDiagonal();
Eigen::VectorXd Bm(6,1)
    Mm<<B_x,B_y,B_z,B_rx,B_ry,B_rz;
auto B = Bm.asDiagonal();
Eigen::VectorXd Km(6,1)
    Mm<<K_x,K_y,K_z,K_rx,K_ry,K_rz;
auto K = Km.asDiagonal();

Impedance control iteration

Firstly, initialize xacc, xvel and xpose with zero;

Eigen::VectorXd xacc(6,1);
Eigen::VectorXd xvel(6,1);
Eigen::VectorXd xpose(6,1);

The parameter (exterForce) represents the external force exerted at the model, and the TIME_STEP means the time duration for controlling robot arm. Thus, the iteration code can be written as:

xacc = Mr*(exterForce - B*xvel - K*xpose); 
xvel = xacc*TIME_STEP + xvel;
xpose = xpose + xvel*TIME_STEP;

Lastly, the actual position of the robot is:

auto pos = xpose + plannedPos;