Tetrapod Project
debug_utils.cpp
Go to the documentation of this file.
1 /*******************************************************************/
2 /* AUTHOR: Paal Arthur S. Thorseth */
3 /* ORGN: Dept of Eng Cybernetics, NTNU Trondheim */
4 /* FILE: debug_utils.cpp */
5 /* DATE: Apr 12, 2021 */
6 /* */
7 /* Copyright (C) 2021 Paal Arthur S. Thorseth, */
8 /* Adrian B. Ghansah */
9 /* */
10 /* This program is free software: you can redistribute it */
11 /* and/or modify it under the terms of the GNU General */
12 /* Public License as published by the Free Software Foundation, */
13 /* either version 3 of the License, or (at your option) any */
14 /* later version. */
15 /* */
16 /* This program is distributed in the hope that it will be useful, */
17 /* but WITHOUT ANY WARRANTY; without even the implied warranty */
18 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. */
19 /* See the GNU General Public License for more details. */
20 /* */
21 /* You should have received a copy of the GNU General Public */
22 /* License along with this program. If not, see */
23 /* <https://www.gnu.org/licenses/>. */
24 /* */
25 /*******************************************************************/
26 
27 #include <debug_utils/debug_utils.h>
28 
29 namespace debug_utils
30 {
31 
32 // Print Base State
33 void printBaseState(const Eigen::Matrix<double, 6, 1> &_base_state)
34 {
35  ROS_INFO_STREAM("\n --------------- Base State --------------- ");
36  ROS_INFO_STREAM("x: \t" << _base_state(0));
37  ROS_INFO_STREAM("y: \t" << _base_state(1));
38  ROS_INFO_STREAM("z: \t" << _base_state(2));
39  ROS_INFO_STREAM("roll: \t" << _base_state(3));
40  ROS_INFO_STREAM("pitch: \t" << _base_state(4));
41  ROS_INFO_STREAM("yaw: \t" << _base_state(5));
42  ROS_INFO_STREAM(" ------------------------------------------ ");
43 }
44 
45 // Print Base Twist
46 void printBaseTwist(const Eigen::Matrix<double, 6, 1> &_base_twist)
47 {
48  ROS_INFO_STREAM("\n --------------- Base Twist --------------- ");
49  ROS_INFO_STREAM("x_vel: \t" << _base_twist(0));
50  ROS_INFO_STREAM("y_vel: \t" << _base_twist(1));
51  ROS_INFO_STREAM("z_vel: \t" << _base_twist(2));
52  ROS_INFO_STREAM("roll_rate: \t" << _base_twist(3));
53  ROS_INFO_STREAM("pitch_rate: \t" << _base_twist(4));
54  ROS_INFO_STREAM("yaw_rate: \t" << _base_twist(5));
55  ROS_INFO_STREAM(" ------------------------------------------ ");
56 }
57 
58 // Print Base Accel
59 void printBaseAccel(const Eigen::Matrix<double, 6, 1> &_base_accel)
60 {
61  ROS_INFO_STREAM("\n --------------- Base Accel --------------- ");
62  ROS_INFO_STREAM("x_accel: \t" << _base_accel(0));
63  ROS_INFO_STREAM("y_accel: \t" << _base_accel(1));
64  ROS_INFO_STREAM("z_accel: \t" << _base_accel(2));
65  ROS_INFO_STREAM("roll_accel: \t" << _base_accel(3));
66  ROS_INFO_STREAM("pitch_accel: \t" << _base_accel(4));
67  ROS_INFO_STREAM("yaw_accel: \t" << _base_accel(5));
68  ROS_INFO_STREAM(" ------------------------------------------ ");
69 }
70 
71 // Print Joint State
72 void printJointState(const Eigen::Matrix<double, 12, 1> &_joint_state)
73 {
74  ROS_INFO_STREAM("\n ---------- Joint State [deg] ----------- ");
75  ROS_INFO_STREAM("fl,hy: \t" << _joint_state(0) * 360 * math_utils::ONE_DIV_TWO_PI);
76  ROS_INFO_STREAM("fl,hp: \t" << _joint_state(1) * 360 * math_utils::ONE_DIV_TWO_PI);
77  ROS_INFO_STREAM("fl,kp: \t" << _joint_state(2) * 360 * math_utils::ONE_DIV_TWO_PI);
78  ROS_INFO_STREAM("fr,hy: \t" << _joint_state(3) * 360 * math_utils::ONE_DIV_TWO_PI);
79  ROS_INFO_STREAM("fr,hp: \t" << _joint_state(4) * 360 * math_utils::ONE_DIV_TWO_PI);
80  ROS_INFO_STREAM("fr,kp: \t" << _joint_state(5) * 360 * math_utils::ONE_DIV_TWO_PI);
81  ROS_INFO_STREAM("rl,hy: \t" << _joint_state(6) * 360 * math_utils::ONE_DIV_TWO_PI);
82  ROS_INFO_STREAM("rl,hp: \t" << _joint_state(7) * 360 * math_utils::ONE_DIV_TWO_PI);
83  ROS_INFO_STREAM("rl,kp: \t" << _joint_state(8) * 360 * math_utils::ONE_DIV_TWO_PI);
84  ROS_INFO_STREAM("rr,hy: \t" << _joint_state(9) * 360 * math_utils::ONE_DIV_TWO_PI);
85  ROS_INFO_STREAM("rr,hp: \t" << _joint_state(10) * 360 * math_utils::ONE_DIV_TWO_PI);
86  ROS_INFO_STREAM("rr,kp: \t" << _joint_state(11) * 360 * math_utils::ONE_DIV_TWO_PI);
87  ROS_INFO_STREAM(" ------------------------------------------ ");
88 }
89 
90 // Print Joint Velocities
91 void printJointVelocities(const Eigen::Matrix<double, 12, 1> &_joint_vel)
92 {
93  ROS_INFO_STREAM("\n ------ Joint Velocites [deg/s] --------- ");
94  ROS_INFO_STREAM("fl,hy: \t" << _joint_vel(0) * 360 * math_utils::ONE_DIV_TWO_PI);
95  ROS_INFO_STREAM("fl,hp: \t" << _joint_vel(1) * 360 * math_utils::ONE_DIV_TWO_PI);
96  ROS_INFO_STREAM("fl,kp: \t" << _joint_vel(2) * 360 * math_utils::ONE_DIV_TWO_PI);
97  ROS_INFO_STREAM("fr,hy: \t" << _joint_vel(3) * 360 * math_utils::ONE_DIV_TWO_PI);
98  ROS_INFO_STREAM("fr,hp: \t" << _joint_vel(4) * 360 * math_utils::ONE_DIV_TWO_PI);
99  ROS_INFO_STREAM("fr,kp: \t" << _joint_vel(5) * 360 * math_utils::ONE_DIV_TWO_PI);
100  ROS_INFO_STREAM("rl,hy: \t" << _joint_vel(6) * 360 * math_utils::ONE_DIV_TWO_PI);
101  ROS_INFO_STREAM("rl,hp: \t" << _joint_vel(7) * 360 * math_utils::ONE_DIV_TWO_PI);
102  ROS_INFO_STREAM("rl,kp: \t" << _joint_vel(8) * 360 * math_utils::ONE_DIV_TWO_PI);
103  ROS_INFO_STREAM("rr,hy: \t" << _joint_vel(9) * 360 * math_utils::ONE_DIV_TWO_PI);
104  ROS_INFO_STREAM("rr,hp: \t" << _joint_vel(10) * 360 * math_utils::ONE_DIV_TWO_PI);
105  ROS_INFO_STREAM("rr,kp: \t" << _joint_vel(11) * 360 * math_utils::ONE_DIV_TWO_PI);
106  ROS_INFO_STREAM(" ------------------------------------------ ");
107 }
108 
109 // Print Joint Accelerations
110 void printJointAccelerations(const Eigen::Matrix<double, 12, 1> &_joint_accel)
111 {
112  ROS_INFO_STREAM("\n ------ Joint Accelerations [deg/s^2] --------- ");
113  ROS_INFO_STREAM("fl,hy: \t" << _joint_accel(0) * 360 * math_utils::ONE_DIV_TWO_PI);
114  ROS_INFO_STREAM("fl,hp: \t" << _joint_accel(1) * 360 * math_utils::ONE_DIV_TWO_PI);
115  ROS_INFO_STREAM("fl,kp: \t" << _joint_accel(2) * 360 * math_utils::ONE_DIV_TWO_PI);
116  ROS_INFO_STREAM("fr,hy: \t" << _joint_accel(3) * 360 * math_utils::ONE_DIV_TWO_PI);
117  ROS_INFO_STREAM("fr,hp: \t" << _joint_accel(4) * 360 * math_utils::ONE_DIV_TWO_PI);
118  ROS_INFO_STREAM("fr,kp: \t" << _joint_accel(5) * 360 * math_utils::ONE_DIV_TWO_PI);
119  ROS_INFO_STREAM("rl,hy: \t" << _joint_accel(6) * 360 * math_utils::ONE_DIV_TWO_PI);
120  ROS_INFO_STREAM("rl,hp: \t" << _joint_accel(7) * 360 * math_utils::ONE_DIV_TWO_PI);
121  ROS_INFO_STREAM("rl,kp: \t" << _joint_accel(8) * 360 * math_utils::ONE_DIV_TWO_PI);
122  ROS_INFO_STREAM("rr,hy: \t" << _joint_accel(9) * 360 * math_utils::ONE_DIV_TWO_PI);
123  ROS_INFO_STREAM("rr,hp: \t" << _joint_accel(10) * 360 * math_utils::ONE_DIV_TWO_PI);
124  ROS_INFO_STREAM("rr,kp: \t" << _joint_accel(11) * 360 * math_utils::ONE_DIV_TWO_PI);
125  ROS_INFO_STREAM(" ------------------------------------------ ");
126 }
127 
128 // Print Joint Torques
129 void printJointTorques(const Eigen::Matrix<double, 12, 1> &_joint_trq)
130 {
131  ROS_INFO_STREAM("\n ------ Joint Torques [Nm] --------- ");
132  ROS_INFO_STREAM("fl,hy: \t" << _joint_trq(0));
133  ROS_INFO_STREAM("fl,hp: \t" << _joint_trq(1));
134  ROS_INFO_STREAM("fl,kp: \t" << _joint_trq(2));
135  ROS_INFO_STREAM("fr,hy: \t" << _joint_trq(3));
136  ROS_INFO_STREAM("fr,hp: \t" << _joint_trq(4));
137  ROS_INFO_STREAM("fr,kp: \t" << _joint_trq(5));
138  ROS_INFO_STREAM("rl,hy: \t" << _joint_trq(6));
139  ROS_INFO_STREAM("rl,hp: \t" << _joint_trq(7));
140  ROS_INFO_STREAM("rl,kp: \t" << _joint_trq(8));
141  ROS_INFO_STREAM("rr,hy: \t" << _joint_trq(9));
142  ROS_INFO_STREAM("rr,hp: \t" << _joint_trq(10));
143  ROS_INFO_STREAM("rr,kp: \t" << _joint_trq(11));
144  ROS_INFO_STREAM(" ------------------------------------------ ");
145 }
146 
147 // Print Generalized Coordinates
148 void printGeneralizedCoordinates(const Eigen::Matrix<double, 18, 1> &_gen_coord)
149 {
150  printBaseState(_gen_coord.block(0,0,5,0));
151  printJointState(_gen_coord.block(6,0,17,0));
152 }
153 
154 // Print Generalized Velocities
155 void printGeneralizedVelocities(const Eigen::Matrix<double, 18, 1> &_gen_vel)
156 {
157  printBaseTwist(_gen_vel.block(0,0,5,0));
158  printJointVelocities(_gen_vel.block(6,0,17,0));
159 }
160 
161 // Print Generalized Accelerations
162 void printGeneralizedAccelerations(const Eigen::Matrix<double, 18, 1> &_gen_accel)
163 {
164  printBaseAccel(_gen_accel.topRows(6));
165  printJointAccelerations(_gen_accel.bottomRows(12));
166 }
167 
168 // Print Footstep Positions
169 void printFootstepPositions(const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_pos)
170 {
171  ROS_INFO_STREAM("\n --------- Footstep Positions ------------- ");
172  ROS_INFO_STREAM("****front left****");
173  ROS_INFO_STREAM("x: " << _f_pos(0)(0));
174  ROS_INFO_STREAM("y: " << _f_pos(0)(1));
175  ROS_INFO_STREAM("z: " << _f_pos(0)(2));
176  ROS_INFO_STREAM("*******************");
177  ROS_INFO_STREAM("****front right****");
178  ROS_INFO_STREAM("x: " << _f_pos(1)(0));
179  ROS_INFO_STREAM("y: " << _f_pos(1)(1));
180  ROS_INFO_STREAM("z: " << _f_pos(1)(2));
181  ROS_INFO_STREAM("*******************");
182  ROS_INFO_STREAM("****rear left****");
183  ROS_INFO_STREAM("x: " << _f_pos(2)(0));
184  ROS_INFO_STREAM("y: " << _f_pos(2)(1));
185  ROS_INFO_STREAM("z: " << _f_pos(2)(2));
186  ROS_INFO_STREAM("******************");
187  ROS_INFO_STREAM("****rear right****");
188  ROS_INFO_STREAM("x: " << _f_pos(3)(0));
189  ROS_INFO_STREAM("y: " << _f_pos(3)(1));
190  ROS_INFO_STREAM("z: " << _f_pos(3)(2));
191  ROS_INFO_STREAM("******************");
192  ROS_INFO_STREAM(" ------------------------------------------ ");
193 }
194 
195 // Print Footstep Forces
196 void printFootstepForces(const int* _contact_state,
197  const Eigen::VectorXd &_F_c)
198 {
199  // Index
200  int i = 0;
201 
202  ROS_INFO_STREAM("\n --------- Footstep Forces ------------- ");
203 
204  if (_contact_state[0])
205  {
206  ROS_INFO_STREAM("****front left****");
207  ROS_INFO_STREAM("x: " << _F_c(i));
208  ROS_INFO_STREAM("y: " << _F_c(i+1));
209  ROS_INFO_STREAM("z: " << _F_c(i+2));
210  ROS_INFO_STREAM("*******************");
211 
212  // Increment
213  i += 3;
214  }
215 
216  if (_contact_state[1])
217  {
218  ROS_INFO_STREAM("****front right****");
219  ROS_INFO_STREAM("x: " << _F_c(i));
220  ROS_INFO_STREAM("y: " << _F_c(i+1));
221  ROS_INFO_STREAM("z: " << _F_c(i+2));
222  ROS_INFO_STREAM("*******************");
223 
224  // Increment
225  i += 3;
226  }
227 
228  if (_contact_state[2])
229  {
230  ROS_INFO_STREAM("****rear left****");
231  ROS_INFO_STREAM("x: " << _F_c(i));
232  ROS_INFO_STREAM("y: " << _F_c(i+1));
233  ROS_INFO_STREAM("z: " << _F_c(i+2));
234  ROS_INFO_STREAM("*******************");
235 
236  // Increment
237  i += 3;
238  }
239 
240  if (_contact_state[3])
241  {
242  ROS_INFO_STREAM("****rear right****");
243  ROS_INFO_STREAM("x: " << _F_c(i));
244  ROS_INFO_STREAM("y: " << _F_c(i+1));
245  ROS_INFO_STREAM("z: " << _F_c(i+2));
246  ROS_INFO_STREAM("*******************");
247 
248  // Increment
249  i += 3;
250  }
251 
252 }
253 
254 } // namespace debug_utils
debug_utils.h
debug_utils
Definition: debug_utils.h:42
debug_utils::printJointVelocities
void printJointVelocities(const Eigen::Matrix< double, 12, 1 > &_joint_vel)
The printJointVelocities function prints given joint- velocities to ROS using print level INFO.
Definition: debug_utils.cpp:91
debug_utils::printJointAccelerations
void printJointAccelerations(const Eigen::Matrix< double, 12, 1 > &_joint_vel)
The printJointAccelerations function prints given joint- accelerations to ROS using print level INFO.
Definition: debug_utils.cpp:110
debug_utils::printJointState
void printJointState(const Eigen::Matrix< double, 12, 1 > &_joint_state)
The printJointState function prints a given joint- state to ROS using print level INFO.
Definition: debug_utils.cpp:72
debug_utils::printGeneralizedAccelerations
void printGeneralizedAccelerations(const Eigen::Matrix< double, 18, 1 > &_gen_accel)
The printGeneralizedAccelerations function prints a given set of generalized accelerations to ROS usi...
Definition: debug_utils.cpp:162
debug_utils::printGeneralizedCoordinates
void printGeneralizedCoordinates(const Eigen::Matrix< double, 18, 1 > &_gen_coord)
The printGeneralizedCoordinates function prints a given set of generalized coordinates to ROS using p...
Definition: debug_utils.cpp:148
debug_utils::printJointTorques
void printJointTorques(const Eigen::Matrix< double, 12, 1 > &_joint_trq)
The printJointTorques function prints given joint- torques to ROS using print level INFO.
Definition: debug_utils.cpp:129
debug_utils::printBaseAccel
void printBaseAccel(const Eigen::Matrix< double, 6, 1 > &_base_accel)
The printBaseAccel function prints a given base- accel to ROS using print level INFO.
Definition: debug_utils.cpp:59
debug_utils::printFootstepPositions
void printFootstepPositions(const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_f_pos)
The printFootstepPositions function prints a given set of footstep positions to ROS using print level...
Definition: debug_utils.cpp:169
debug_utils::printBaseTwist
void printBaseTwist(const Eigen::Matrix< double, 6, 1 > &_base_twist)
The printBaseTwist function prints a given base- twist to ROS using print level INFO.
Definition: debug_utils.cpp:46
debug_utils::printBaseState
void printBaseState(const Eigen::Matrix< double, 6, 1 > &_base_state)
The printBaseState function prints a given base- state to ROS using print level INFO.
Definition: debug_utils.cpp:33
debug_utils::printGeneralizedVelocities
void printGeneralizedVelocities(const Eigen::Matrix< double, 18, 1 > &_gen_vel)
The printGeneralizedVelocities function prints a given set of generalized velocities to ROS using pri...
Definition: debug_utils.cpp:155
debug_utils::printFootstepForces
void printFootstepForces(const int *_contact_state, const Eigen::VectorXd &_F_c)
The printFootstepPositions function prints a given set of footstep forces to ROS using print level IN...
Definition: debug_utils.cpp:196
math_utils::ONE_DIV_TWO_PI
static constexpr double ONE_DIV_TWO_PI
Define 1/2PI.
Definition: angle_utils.h:62