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();
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;