ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Mon, 27 Jul 2015 02:14:05 -0500How to implement another thread to communicate with real controller loophttps://answers.ros.org/question/214626/how-to-implement-another-thread-to-communicate-with-real-controller-loop/ 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!
Mon, 27 Jul 2015 02:14:05 -0500https://answers.ros.org/question/214626/how-to-implement-another-thread-to-communicate-with-real-controller-loop/