How to implement another thread to communicate with real controller loop

edit

Hi everyone, I recently try to implement a impedance controller in PR2. First I try to follow the routine of pr2 controller tutorial(http://wiki.ros.org/pr2_mechanism/Tutorials/Writing%20a%20realtime%20Cartesian%20controller). I implement the update function to involve all the computation needed in a impedance controller. But when I launch it in real pr2, it exports an error:

3 2015/07/25 18:57 Full Name: /Realtime Controllers/Realtime Control Loop Component: Realtime Control Loop Hardware ID: Level: ERROR Message: Realtime loop used too much time in the last 30 seconds.; Halting, realtime loop only ran at 623.3333 Hz

Max EtherCAT roundtrip (us): 14740.77 Avg EtherCAT roundtrip (us): 435.66 Max Controller Manager roundtrip (us): 4241.98 Avg Controller Manager roundtrip (us): 609.05 Max Total Loop roundtrip (us): 15001.66 Avg Total Loop roundtrip (us): 1688.03 Max Loop Jitter (us): 236.06 Avg Loop Jitter (us): 7.49 Control Loop Overruns: 66601 Recent Control Loop Overruns: 408 Last Control Loop Overrun Cause: ec: 487.11us, cm: 598.53us Last Overrun Loop Time (us): 91.31 Realtime Loop Frequency: 603.3333

In update loop, the computation involve lots of matrix multiplication and svd ,so the time in each update will exceed 1000us. I think there is two way to solve this:

The first, try to optimize the code in update loop and try to cut the computation time. I pose my code here:

  MCG_solver_->JntToMass(q_, M_);
  MCG_solver_->JntToCoriolis(q_, qdot_.qdot, C_); //coriolis matrix seems worthless

 //KDL matrix to eigen matrix to easily utilize linear algebra
 InertiaMatrixKDLToEigen(M_, M_e);
 ArrayKDLToEigen(C_, C_e);

//Compute cartesian space inertia matrix
M_inv =  M_e.inverse();
M_cart_inv = J_e * M_inv * J_e.transpose();
 Eigen::JacobiSVD<CartesianInertiaMatrix, Eigen::HouseholderQRPreconditioner> svd(M_cart_inv, Eigen::ComputeFullU | Eigen::ComputeFullV);

//Compute desired mass and damping matrix
Mass_d = (1 / M_des) * Eigen::MatrixXd::Identity(6, 6); //actually is inverse of desired mass matrix
Damp_d = D_des * Eigen::MatrixXd::Identity(6, 6);

//Compute dynamiclly consistent generlized inverse matrix and projection matrix
J_bar_transpose = svd.solve(J_e * M_inv);
null_proj = Eigen::MatrixXd::Identity(Joints, Joints) - J_e.transpose() * J_bar_transpose;

//compute task torque
Cart6Vector temp = -J_e * M_inv * C_e + Mass_d * (Damp_d * xdot_e);
tau_task_e = -J_e.transpose() * svd.solve(temp);

The second, implement another thread involve these matrix multiplication and svd. I have littile experience about multi thread. So could someone give a example about how to implement another thread in ros controller.

Any comments helps. Thank you in advance!