balance.cpp
Go to the documentation of this file.
1 //
2 // Created by qiayuan on 2022/11/15.
3 //
4 #include "rm_chassis_controllers/balance.h"
5 
6 #include <unsupported/Eigen/MatrixFunctions>
7 #include <rm_common/ros_utilities.h>
8 #include <rm_common/ori_tool.h>
9 #include <geometry_msgs/Quaternion.h>
10 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
11 #include <pluginlib/class_list_macros.hpp>
12 #include <rm_msgs/BalanceState.h>
13 #include <angles/angles.h>
14 
15 namespace rm_chassis_controllers
16 {
17 bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh,
18  ros::NodeHandle& controller_nh)
19 {
20  ChassisBase::init(robot_hw, root_nh, controller_nh);
21 
22  imu_handle_ = robot_hw->get<hardware_interface::ImuSensorInterface>()->getHandle(
23  getParam(controller_nh, "imu_name", std::string("base_imu")));
24  std::string left_wheel_joint, right_wheel_joint, left_momentum_block_joint, right_momentum_block_joint;
25  if (!controller_nh.getParam("left/wheel_joint", left_wheel_joint) ||
26  !controller_nh.getParam("left/block_joint", left_momentum_block_joint) ||
27  !controller_nh.getParam("right/wheel_joint", right_wheel_joint) ||
28  !controller_nh.getParam("right/block_joint", right_momentum_block_joint))
29  {
30  ROS_ERROR("Some Joints' name doesn't given. (namespace: %s)", controller_nh.getNamespace().c_str());
31  return false;
32  }
33  left_wheel_joint_handle_ = robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(left_wheel_joint);
34  right_wheel_joint_handle_ = robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(right_wheel_joint);
35  joint_handles_.push_back(left_wheel_joint_handle_);
36  joint_handles_.push_back(right_wheel_joint_handle_);
37  left_momentum_block_joint_handle_ =
38  robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(left_momentum_block_joint);
39  right_momentum_block_joint_handle_ =
40  robot_hw->get<hardware_interface::EffortJointInterface>()->getHandle(right_momentum_block_joint);
41 
42  // m_w is mass of single wheel
43  // m is mass of the robot except wheels and momentum_blocks
44  // m_b is mass of single momentum_block
45  // i_w is the moment of inertia of the wheel around the rotational axis of the motor
46  // l is the vertical component of the distance between the wheel center and the center of mass of robot
47  // y_b is the y-axis component of the coordinates of the momentum block in the base_link coordinate system
48  // z_b is the vertical component of the distance between the momentum block and the center of mass of robot
49  // i_m is the moment of inertia of the robot around the y-axis of base_link coordinate.
50  double m_w, m, m_b, i_w, l, y_b, z_b, g, i_m;
51 
52  if (!controller_nh.getParam("m_w", m_w))
53  {
54  ROS_ERROR("Params m_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
55  return false;
56  }
57  if (!controller_nh.getParam("m", m))
58  {
59  ROS_ERROR("Params m doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
60  return false;
61  }
62  if (!controller_nh.getParam("m_b", m_b))
63  {
64  ROS_ERROR("Params m_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
65  return false;
66  }
67  if (!controller_nh.getParam("i_w", i_w))
68  {
69  ROS_ERROR("Params i_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
70  return false;
71  }
72  if (!controller_nh.getParam("l", l))
73  {
74  ROS_ERROR("Params l doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
75  return false;
76  }
77  if (!controller_nh.getParam("y_b", y_b))
78  {
79  ROS_ERROR("Params y_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
80  return false;
81  }
82  if (!controller_nh.getParam("z_b", z_b))
83  {
84  ROS_ERROR("Params z_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
85  return false;
86  }
87  if (!controller_nh.getParam("g", g))
88  {
89  ROS_ERROR("Params g doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
90  return false;
91  }
92  if (!controller_nh.getParam("i_m", i_m))
93  {
94  ROS_ERROR("Params i_m doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
95  return false;
96  }
97  if (!controller_nh.getParam("wheel_radius", wheel_radius_))
98  {
99  ROS_ERROR("Params wheel_radius doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
100  return false;
101  }
102  if (!controller_nh.getParam("wheel_base", wheel_base_))
103  {
104  ROS_ERROR("Params wheel_base_ doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
105  return false;
106  }
107  if (!controller_nh.getParam("block_duration", block_duration_))
108  {
109  ROS_ERROR("Params block_duration doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
110  return false;
111  }
112  if (!controller_nh.getParam("block_angle", block_angle_))
113  {
114  ROS_ERROR("Params block_angle doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
115  return false;
116  }
117  if (!controller_nh.getParam("block_effort", block_effort_))
118  {
119  ROS_ERROR("Params block_speed doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
120  return false;
121  }
122  if (!controller_nh.getParam("block_velocity", block_velocity_))
123  {
124  ROS_ERROR("Params block_velocity doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
125  return false;
126  }
127  if (!controller_nh.getParam("anti_block_effort", anti_block_effort_))
128  {
129  ROS_ERROR("Params anti_block_effort doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
130  return false;
131  }
132  if (!controller_nh.getParam("block_overtime", block_overtime_))
133  {
134  ROS_ERROR("Params block_overtime doesn't given (namespace: %s)", controller_nh.getNamespace().c_str());
135  return false;
136  }
137  controller_nh.getParam("position_offset", position_offset_);
138  controller_nh.getParam("position_clear_threshold", position_clear_threshold_);
139 
140  q_.setZero();
141  r_.setZero();
142  XmlRpc::XmlRpcValue q, r;
143  controller_nh.getParam("q", q);
144  controller_nh.getParam("r", r);
145  // Check and get Q
146  ROS_ASSERT(q.getType() == XmlRpc::XmlRpcValue::TypeArray);
147  ROS_ASSERT(q.size() == STATE_DIM);
148  for (int i = 0; i < STATE_DIM; ++i)
149  {
150  ROS_ASSERT(q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || q[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
151  if (q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble)
152  {
153  q_(i, i) = static_cast<double>(q[i]);
154  }
155  else if (q[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
156  {
157  q_(i, i) = static_cast<int>(q[i]);
158  }
159  }
160  // Check and get R
161  ROS_ASSERT(r.getType() == XmlRpc::XmlRpcValue::TypeArray);
162  ROS_ASSERT(r.size() == CONTROL_DIM);
163  for (int i = 0; i < CONTROL_DIM; ++i)
164  {
165  ROS_ASSERT(r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || r[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
166  if (r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble)
167  {
168  r_(i, i) = static_cast<double>(r[i]);
169  }
170  else if (r[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
171  {
172  r_(i, i) = static_cast<int>(r[i]);
173  }
174  }
175 
176  // Continuous model \dot{x} = A x + B u
177  double a_5_2 = -(pow(wheel_radius_, 2) * g * (pow(l, 2) * pow(m, 2) + 2 * m_b * pow(l, 2) * m + 2 * i_m * m_b)) /
178  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
179  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
180  double a_5_3 = -(pow(wheel_radius_, 2) * g * l * m * m_b) /
181  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
182  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
183  double a_5_4 = a_5_3;
184  double a_7_2 =
185  (g * l * m *
186  (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_b + 2 * pow(wheel_radius_, 2) * m_w)) /
187  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
188  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
189  double a_7_3 = (g * m_b * (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w)) /
190  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
191  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
192  double a_7_4 = a_7_3;
193  double a_8_2 =
194  (g * (i_m - l * m * z_b) *
195  (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_b + 2 * pow(wheel_radius_, 2) * m_w)) /
196  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
197  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
198  double a_8_3 = -(g * m_b *
199  (2 * i_w * l + 2 * i_w * z_b + 2 * pow(wheel_radius_, 2) * l * m_w +
200  pow(wheel_radius_, 2) * m * z_b + 2 * pow(wheel_radius_, 2) * m_w * z_b)) /
201  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
202  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
203  double a_8_4 = a_8_3;
204  double a_9_2 = a_8_2;
205  double a_9_3 = a_8_3;
206  double a_9_4 = a_8_4;
207 
208  double b_5_0 = (wheel_radius_ * (m * pow(l, 2) + wheel_radius_ * m * l + i_m)) /
209  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
210  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
211  double b_5_1 = b_5_0;
212  double b_5_2 = (pow(wheel_radius_, 2) * l * m * z_b) /
213  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
214  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
215  double b_5_3 = b_5_2;
216  double b_6_0 = -wheel_radius_ / (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w));
217  double b_6_1 = -b_6_0;
218  double b_6_2 = (2 * pow(wheel_radius_, 2) * y_b) / (pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w));
219  double b_6_3 = -b_6_2;
220  double b_7_0 = -(2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w + wheel_radius_ * l * m) /
221  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
222  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
223  double b_7_1 = b_7_0;
224  double b_7_2 = -(z_b * (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w)) /
225  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
226  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w);
227  double b_7_3 = b_7_2;
228  double b_9_0 =
229  (2 * pow(i_w, 2) * l * wheel_base_ + 2 * pow(i_w, 2) * wheel_base_ * z_b -
230  4 * pow(wheel_radius_, 3) * i_m * m_w * wheel_base_ + pow(wheel_radius_, 3) * i_m * m * y_b +
231  2 * pow(wheel_radius_, 3) * i_m * m_w * y_b + 8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * wheel_base_ +
232  8 * pow(wheel_radius_, 4) * pow(m_w, 2) * wheel_base_ * z_b - wheel_radius_ * i_m * i_w * wheel_base_ +
233  2 * wheel_radius_ * i_m * i_w * y_b + 2 * pow(wheel_radius_, 3) * pow(l, 2) * m * m_w * y_b +
234  10 * pow(wheel_radius_, 2) * i_w * l * m_w * wheel_base_ + 2 * wheel_radius_ * i_w * pow(l, 2) * m * y_b +
235  pow(wheel_radius_, 2) * i_w * m * wheel_base_ * z_b + 10 * pow(wheel_radius_, 2) * i_w * m_w * wheel_base_ * z_b +
236  4 * pow(wheel_radius_, 4) * m * m_w * wheel_base_ * z_b +
237  4 * pow(wheel_radius_, 3) * l * m * m_w * wheel_base_ * z_b + wheel_radius_ * i_w * l * m * wheel_base_ * z_b) /
238  (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w) *
239  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
240  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w));
241  double b_9_1 =
242  (2 * pow(i_w, 2) * l * wheel_base_ + 2 * pow(i_w, 2) * wheel_base_ * z_b -
243  4 * pow(wheel_radius_, 3) * i_m * m_w * wheel_base_ - pow(wheel_radius_, 3) * i_m * m * y_b -
244  2 * pow(wheel_radius_, 3) * i_m * m_w * y_b + 8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * wheel_base_ +
245  8 * pow(wheel_radius_, 4) * pow(m_w, 2) * wheel_base_ * z_b - wheel_radius_ * i_m * i_w * wheel_base_ -
246  2 * wheel_radius_ * i_m * i_w * y_b - 2 * pow(wheel_radius_, 3) * pow(l, 2) * m * m_w * y_b +
247  10 * pow(wheel_radius_, 2) * i_w * l * m_w * wheel_base_ - 2 * wheel_radius_ * i_w * pow(l, 2) * m * y_b +
248  pow(wheel_radius_, 2) * i_w * m * wheel_base_ * z_b + 10 * pow(wheel_radius_, 2) * i_w * m_w * wheel_base_ * z_b +
249  4 * pow(wheel_radius_, 4) * m * m_w * wheel_base_ * z_b +
250  4 * pow(wheel_radius_, 3) * l * m * m_w * wheel_base_ * z_b + wheel_radius_ * i_w * l * m * wheel_base_ * z_b) /
251  (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w) *
252  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
253  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w));
254  double b_9_2 =
255  (-4 * m * pow(wheel_radius_, 4) * pow(l, 2) * m_w * pow(y_b, 2) +
256  8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * pow(wheel_base_, 2) * z_b +
257  8 * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) +
258  4 * m * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) * pow(z_b, 2) -
259  4 * i_m * pow(wheel_radius_, 4) * m_w * pow(y_b, 2) - 2 * i_m * m * pow(wheel_radius_, 4) * pow(y_b, 2) -
260  4 * m * pow(wheel_radius_, 2) * i_w * pow(l, 2) * pow(y_b, 2) +
261  10 * pow(wheel_radius_, 2) * i_w * l * m_w * pow(wheel_base_, 2) * z_b +
262  10 * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) * pow(z_b, 2) +
263  m * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) * pow(z_b, 2) -
264  4 * i_m * pow(wheel_radius_, 2) * i_w * pow(y_b, 2) + 2 * pow(i_w, 2) * l * pow(wheel_base_, 2) * z_b +
265  2 * pow(i_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2)) /
266  (pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w) *
267  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
268  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w));
269  double b_9_3 =
270  (8 * m * pow(wheel_radius_, 4) * pow(l, 2) * pow(m_w, 2) * pow(wheel_base_, 2) +
271  4 * m * m_b * pow(wheel_radius_, 4) * pow(l, 2) * m_w * pow(y_b, 2) +
272  8 * m_b * pow(wheel_radius_, 4) * l * pow(m_w, 2) * pow(wheel_base_, 2) * z_b +
273  8 * m_b * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) +
274  8 * i_m * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) +
275  4 * m * m_b * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) * pow(z_b, 2) +
276  4 * i_m * m * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) +
277  4 * i_m * m_b * pow(wheel_radius_, 4) * m_w * pow(y_b, 2) +
278  2 * i_m * m * m_b * pow(wheel_radius_, 4) * pow(y_b, 2) +
279  10 * m * pow(wheel_radius_, 2) * i_w * pow(l, 2) * m_w * pow(wheel_base_, 2) +
280  4 * m * m_b * pow(wheel_radius_, 2) * i_w * pow(l, 2) * pow(y_b, 2) +
281  10 * m_b * pow(wheel_radius_, 2) * i_w * l * m_w * pow(wheel_base_, 2) * z_b +
282  10 * m_b * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) * pow(z_b, 2) +
283  10 * i_m * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) +
284  m * m_b * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) * pow(z_b, 2) +
285  i_m * m * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) +
286  4 * i_m * m_b * pow(wheel_radius_, 2) * i_w * pow(y_b, 2) +
287  2 * m * pow(i_w, 2) * pow(l, 2) * pow(wheel_base_, 2) + 2 * m_b * pow(i_w, 2) * l * pow(wheel_base_, 2) * z_b +
288  2 * m_b * pow(i_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) + 2 * i_m * pow(i_w, 2) * pow(wheel_base_, 2)) /
289  (m_b * pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w) *
290  (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m +
291  2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w));
292  double b_8_0 = b_9_1;
293  double b_8_1 = b_9_0;
294  double b_8_2 = b_9_3;
295  double b_8_3 = b_9_2;
296 
297  a_ << 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
298  1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., a_5_2,
299  a_5_3, a_5_4, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., a_7_2, a_7_3, a_7_4, 0., 0., 0.,
300  0., 0., 0., 0., a_8_2, a_8_3, a_8_4, 0., 0., 0., 0., 0., 0., 0., a_9_2, a_9_3, a_9_4, 0., 0., 0., 0., 0.;
301  b_ << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., b_5_0, b_5_1, b_5_2, b_5_3,
302  b_6_0, b_6_1, b_6_2, b_6_3, b_7_0, b_7_1, b_7_2, b_7_3, b_8_0, b_8_1, b_8_2, b_8_3, b_9_0, b_9_1, b_9_2, b_9_3;
303 
304  ROS_INFO_STREAM("A:" << a_);
305  ROS_INFO_STREAM("B:" << b_);
306  Lqr<double> lqr(a_, b_, q_, r_);
307  if (!lqr.computeK())
308  {
309  ROS_ERROR("Failed to compute K of LQR.");
310  return false;
311  }
312 
313  k_ = lqr.getK();
314  ROS_INFO_STREAM("K of LQR:" << k_);
315 
316  state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::BalanceState>(root_nh, "/state", 100));
317  balance_mode_ = BalanceMode::NORMAL;
318 
319  return true;
320 }
321 
322 void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period)
323 {
324  geometry_msgs::Vector3 gyro;
325  gyro.x = imu_handle_.getAngularVelocity()[0];
326  gyro.y = imu_handle_.getAngularVelocity()[1];
327  gyro.z = imu_handle_.getAngularVelocity()[2];
328  try
329  {
330  tf2::doTransform(gyro, angular_vel_base_,
331  robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), time));
332  }
333  catch (tf2::TransformException& ex)
334  {
335  ROS_WARN("%s", ex.what());
336  return;
337  }
338  tf2::Transform odom2imu, imu2base, odom2base;
339  try
340  {
341  geometry_msgs::TransformStamped tf_msg;
342  tf_msg = robot_state_handle_.lookupTransform(imu_handle_.getFrameId(), "base_link", time);
343  tf2::fromMsg(tf_msg.transform, imu2base);
344  }
345  catch (tf2::TransformException& ex)
346  {
347  ROS_WARN("%s", ex.what());
348  left_wheel_joint_handle_.setCommand(0.);
349  right_wheel_joint_handle_.setCommand(0.);
350  left_momentum_block_joint_handle_.setCommand(0.);
351  right_momentum_block_joint_handle_.setCommand(0.);
352  return;
353  }
354  tf2::Quaternion odom2imu_quaternion;
355  tf2::Vector3 odom2imu_origin;
356  odom2imu_quaternion.setValue(imu_handle_.getOrientation()[0], imu_handle_.getOrientation()[1],
357  imu_handle_.getOrientation()[2], imu_handle_.getOrientation()[3]);
358  odom2imu_origin.setValue(0, 0, 0);
359  odom2imu.setOrigin(odom2imu_origin);
360  odom2imu.setRotation(odom2imu_quaternion);
361  odom2base = odom2imu * imu2base;
362 
363  quatToRPY(toMsg(odom2base).rotation, roll_, pitch_, yaw_);
364 
365  // Check block
366  if (balance_mode_ != BalanceMode::BLOCK)
367  {
368  if (std::abs(pitch_) > block_angle_ &&
369  (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. >
370  block_effort_ &&
371  (left_wheel_joint_handle_.getVelocity() < block_velocity_ ||
372  right_wheel_joint_handle_.getVelocity() < block_velocity_))
373  {
374  if (!maybe_block_)
375  {
376  block_time_ = time;
377  maybe_block_ = true;
378  }
379  if ((time - block_time_).toSec() >= block_duration_)
380  {
381  balance_mode_ = BalanceMode::BLOCK;
382  balance_state_changed_ = true;
383  ROS_INFO("[balance] Exit NOMAl");
384  }
385  }
386  else
387  {
388  maybe_block_ = false;
389  }
390  }
391 
392  switch (balance_mode_)
393  {
394  case BalanceMode::NORMAL:
395  {
396  normal(time, period);
397  break;
398  }
399  case BalanceMode::BLOCK:
400  {
401  block(time, period);
402  break;
403  }
404  }
405 }
406 
407 void BalanceController::normal(const ros::Time& time, const ros::Duration& period)
408 {
409  if (balance_state_changed_)
410  {
411  ROS_INFO("[balance] Enter NOMAl");
412  balance_state_changed_ = false;
413  }
414 
415  x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 -
416  imu_handle_.getAngularVelocity()[1]) *
417  wheel_radius_;
418  x_[0] += x_[5] * period.toSec();
419  x_[1] = yaw_;
420  x_[2] = pitch_;
421  x_[3] = left_momentum_block_joint_handle_.getPosition();
422  x_[4] = right_momentum_block_joint_handle_.getPosition();
423  x_[6] = angular_vel_base_.z;
424  x_[7] = angular_vel_base_.y;
425  x_[8] = left_momentum_block_joint_handle_.getVelocity();
426  x_[9] = right_momentum_block_joint_handle_.getVelocity();
427  yaw_des_ += vel_cmd_.z * period.toSec();
428  position_des_ += vel_cmd_.x * period.toSec();
429  Eigen::Matrix<double, CONTROL_DIM, 1> u;
430  auto x = x_;
431  x(0) -= position_des_;
432  x(1) = angles::shortest_angular_distance(yaw_des_, x_(1));
433  if (state_ != RAW)
434  x(5) -= vel_cmd_.x;
435  x(6) -= vel_cmd_.z;
436  if (std::abs(x(0) + position_offset_) > position_clear_threshold_)
437  {
438  x_[0] = 0.;
439  position_des_ = position_offset_;
440  }
441  u = k_ * (-x);
442  if (state_pub_->trylock())
443  {
444  state_pub_->msg_.header.stamp = time;
445  state_pub_->msg_.x = x(0);
446  state_pub_->msg_.phi = x(1);
447  state_pub_->msg_.theta = x(2);
448  state_pub_->msg_.x_b_l = x(3);
449  state_pub_->msg_.x_b_r = x(4);
450  state_pub_->msg_.x_dot = x(5);
451  state_pub_->msg_.phi_dot = x(6);
452  state_pub_->msg_.theta_dot = x(7);
453  state_pub_->msg_.x_b_l_dot = x(8);
454  state_pub_->msg_.x_b_r_dot = x(9);
455  state_pub_->msg_.T_l = u(0);
456  state_pub_->msg_.T_r = u(1);
457  state_pub_->msg_.f_b_l = u(2);
458  state_pub_->msg_.f_b_r = u(3);
459  state_pub_->unlockAndPublish();
460  }
461 
462  left_wheel_joint_handle_.setCommand(u(0));
463  right_wheel_joint_handle_.setCommand(u(1));
464  left_momentum_block_joint_handle_.setCommand(u(2));
465  right_momentum_block_joint_handle_.setCommand(u(3));
466 }
467 
468 void BalanceController::block(const ros::Time& time, const ros::Duration& period)
469 {
470  if (balance_state_changed_)
471  {
472  ROS_INFO("[balance] Enter BLOCK");
473  balance_state_changed_ = false;
474 
475  last_block_time_ = ros::Time::now();
476  }
477  if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_)
478  {
479  balance_mode_ = BalanceMode::NORMAL;
480  balance_state_changed_ = true;
481  ROS_INFO("[balance] Exit BLOCK");
482  }
483  else
484  {
485  left_momentum_block_joint_handle_.setCommand(pitch_ > 0 ? -80 : 80);
486  right_momentum_block_joint_handle_.setCommand(pitch_ > 0 ? -80 : 80);
487  left_wheel_joint_handle_.setCommand(pitch_ > 0 ? -anti_block_effort_ : anti_block_effort_);
488  right_wheel_joint_handle_.setCommand(pitch_ > 0 ? -anti_block_effort_ : anti_block_effort_);
489  }
490 }
491 
492 geometry_msgs::Twist BalanceController::odometry()
493 {
494  geometry_msgs::Twist twist;
495  twist.linear.x = x_[5];
496  twist.angular.z = x_[6];
497  return twist;
498 }
499 } // namespace rm_chassis_controllers
500 PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BalanceController, controller_interface::ControllerBase)
rm_control::RobotStateHandle::lookupTransform
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time)
XmlRpc::XmlRpcValue::size
int size() const
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::twist
void twist(const ros::Time &time, const ros::Duration &period)
The mode TWIST: Just moving chassis.
Definition: chassis_base.cpp:241
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
rm_chassis_controllers::BalanceController::odometry
geometry_msgs::Twist odometry() override
Definition: balance.cpp:492
rm_chassis_controllers::BalanceController::block_effort_
double block_effort_
Definition: balance.h:51
rm_chassis_controllers::BalanceController::yaw_des_
double yaw_des_
Definition: balance.h:47
rm_chassis_controllers::BalanceController::imu_handle_
hardware_interface::ImuSensorHandle imu_handle_
Definition: balance.h:54
rm_chassis_controllers::BalanceController::right_wheel_joint_handle_
hardware_interface::JointHandle right_wheel_joint_handle_
Definition: balance.h:55
rm_chassis_controllers::BalanceController
Definition: balance.h:18
tf2::fromMsg
void fromMsg(const A &, B &b)
hardware_interface::InterfaceManager::get
T * get()
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
rm_chassis_controllers::BalanceController::block_overtime_
double block_overtime_
Definition: balance.h:51
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::vel_cmd_
geometry_msgs::Vector3 vel_cmd_
Definition: chassis_base.h:200
XmlRpc::XmlRpcValue::TypeInt
TypeInt
Lqr
hardware_interface::ImuSensorHandle::getFrameId
std::string getFrameId() const
rm_chassis_controllers::BalanceController::roll_
double roll_
Definition: balance.h:61
hardware_interface::ImuSensorHandle::getAngularVelocity
const double * getAngularVelocity() const
rm_chassis_controllers::BalanceController::last_block_time_
ros::Time last_block_time_
Definition: balance.h:50
rm_chassis_controllers::BalanceController::k_
Eigen::Matrix< double, CONTROL_DIM, STATE_DIM > k_
Definition: balance.h:38
rm_chassis_controllers::BalanceController::left_wheel_joint_handle_
hardware_interface::JointHandle left_wheel_joint_handle_
Definition: balance.h:55
hardware_interface::ImuSensorHandle::getOrientation
const double * getOrientation() const
getHandle
ROSCONSOLE_CONSOLE_IMPL_DECL void * getHandle(const std::string &name)
hardware_interface::JointHandle::setCommand
void setCommand(double command)
rm_chassis_controllers::BalanceController::STATE_DIM
static const int STATE_DIM
Definition: balance.h:36
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(RobotLocalization::EkfNodelet, nodelet::Nodelet)
rm_chassis_controllers::BalanceController::block_angle_
double block_angle_
Definition: balance.h:51
rm_chassis_controllers::BalanceController::r_
Eigen::Matrix< double, CONTROL_DIM, CONTROL_DIM > r_
Definition: balance.h:41
realtime_tools::RealtimePublisher
controller_interface::ControllerBase
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
rm_chassis_controllers::BalanceController::block
void block(const ros::Time &time, const ros::Duration &period)
Definition: balance.cpp:468
hardware_interface::RobotHW
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
rm_chassis_controllers::BalanceController::right_momentum_block_joint_handle_
hardware_interface::JointHandle right_momentum_block_joint_handle_
Definition: balance.h:56
rm_chassis_controllers::BalanceController::q_
Eigen::Matrix< double, STATE_DIM, STATE_DIM > q_
Definition: balance.h:39
tf2::Transform
rm_chassis_controllers::BalanceController::anti_block_effort_
double anti_block_effort_
Definition: balance.h:51
hardware_interface::EffortJointInterface
ROS_WARN
#define ROS_WARN(...)
hardware_interface::ImuSensorInterface
rm_chassis_controllers::BalanceController::angular_vel_base_
geometry_msgs::Vector3 angular_vel_base_
Definition: balance.h:60
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
rm_chassis_controllers::BalanceController::block_time_
ros::Time block_time_
Definition: balance.h:50
rm_chassis_controllers::BalanceController::state_pub_
RtpublisherPtr state_pub_
Definition: balance.h:59
Lqr::getK
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > getK()
rm_chassis_controllers::BalanceController::balance_state_changed_
bool balance_state_changed_
Definition: balance.h:52
XmlRpc::XmlRpcValue::getType
const Type & getType() const
rm_chassis_controllers::BalanceController::position_offset_
double position_offset_
Definition: balance.h:45
rm_chassis_controllers::BalanceController::normal
void normal(const ros::Time &time, const ros::Duration &period)
Definition: balance.cpp:407
XmlRpc::XmlRpcValue::TypeArray
TypeArray
rm_chassis_controllers::ChassisBase::init
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
Get and check params for covariances. Setup odometry realtime publisher + odom message constant field...
Definition: chassis_base.cpp:83
rm_chassis_controllers::BalanceController::maybe_block_
bool maybe_block_
Definition: balance.h:52
rm_chassis_controllers::BalanceController::yaw_
double yaw_
Definition: balance.h:61
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::RAW
@ RAW
Definition: chassis_base.h:189
hardware_interface::JointStateHandle::getVelocity
double getVelocity() const
rm_chassis_controllers::BalanceController::position_des_
double position_des_
Definition: balance.h:44
ros::Time
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
hardware_interface::JointStateHandle::getPosition
double getPosition() const
getParam
T getParam(ros::NodeHandle &pnh, const std::string &param_name, const T &default_val)
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::joint_handles_
std::vector< hardware_interface::JointHandle > joint_handles_
Definition: chassis_base.h:178
rm_chassis_controllers::BalanceController::b_
Eigen::Matrix< double, STATE_DIM, CONTROL_DIM > b_
Definition: balance.h:40
ROS_ERROR
#define ROS_ERROR(...)
class_list_macros.hpp
ros_utilities.h
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::robot_state_handle_
rm_control::RobotStateHandle robot_state_handle_
Definition: chassis_base.h:176
rm_chassis_controllers::BalanceController::left_momentum_block_joint_handle_
hardware_interface::JointHandle left_momentum_block_joint_handle_
Definition: balance.h:56
balance.h
tf2::Quaternion
tf2_geometry_msgs.h
toMsg
B toMsg(const A &a)
rm_chassis_controllers::BalanceController::wheel_radius_
double wheel_radius_
Definition: balance.h:43
hardware_interface::JointStateHandle::getEffort
double getEffort() const
rm_chassis_controllers::BalanceController::block_duration_
double block_duration_
Definition: balance.h:51
quatToRPY
void quatToRPY(const geometry_msgs::Quaternion &q, double &roll, double &pitch, double &yaw)
rm_chassis_controllers::BalanceController::x_
Eigen::Matrix< double, STATE_DIM, 1 > x_
Definition: balance.h:42
rm_chassis_controllers::BalanceController::init
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
Definition: balance.cpp:17
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
DurationBase< Duration >::toSec
double toSec() const
rm_chassis_controllers::BalanceController::CONTROL_DIM
static const int CONTROL_DIM
Definition: balance.h:37
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
rm_chassis_controllers::BalanceController::moveJoint
void moveJoint(const ros::Time &time, const ros::Duration &period) override
Definition: balance.cpp:322
ROS_ASSERT
#define ROS_ASSERT(cond)
rm_chassis_controllers::BalanceController::a_
Eigen::Matrix< double, STATE_DIM, STATE_DIM > a_
Definition: balance.h:39
rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >::state_
int state_
Definition: chassis_base.h:193
ros::Duration
rm_chassis_controllers::BalanceController::pitch_
double pitch_
Definition: balance.h:61
rm_chassis_controllers::BalanceController::wheel_base_
double wheel_base_
Definition: balance.h:43
rm_chassis_controllers::BalanceController::balance_mode_
int balance_mode_
Definition: balance.h:49
rm_chassis_controllers::BalanceController::block_velocity_
double block_velocity_
Definition: balance.h:51
XmlRpc::XmlRpcValue
ros::NodeHandle
angles.h
rm_chassis_controllers
Definition: balance.h:15
ori_tool.h
ros::Time::now
static Time now()
rm_chassis_controllers::BalanceController::position_clear_threshold_
double position_clear_threshold_
Definition: balance.h:46
Lqr::computeK
bool computeK()

rm_chassis_controllers
Author(s): Qiayuan Liao
autogenerated on Sun May 4 2025 02:57:17