@@ -13,43 +13,36 @@ namespace meta_chassis_controller {
1313AgvWheelKinematics::AgvWheelKinematics (const double agv_wheel_center_x, const double agv_wheel_center_y, const double agv_wheel_radius) :
1414 agv_wheel_center_x_ (agv_wheel_center_x), agv_wheel_center_y_(agv_wheel_center_y), agv_wheel_radius_(agv_wheel_radius) {
1515 agv_radius_ = sqrt (agv_wheel_center_x * agv_wheel_center_x + agv_wheel_center_y * agv_wheel_center_y);
16- // init();
1716}
1817
19- // void AgvWheelKinematics::init() {
20-
21- // }
18+ std::pair<double ,double > AgvWheelKinematics::xy2polar (double curr_pos, double target_x, double target_y) {
19+ double target_vel = sqrt (target_x * target_x + target_y * target_y) / agv_wheel_radius_;
2220
23- std::tuple< double , double > AgvWheelKinematics::get_output ( double curr_pos, double curr_vel, double target_pos, double target_vel){
24- if (target_vel == 0.0 ){
21+ // If the target velocity is zero, atan2 is meaningless, we should preserve current position
22+ if (target_vel == 0.0 ) {
2523 return {curr_pos, 0.0 };
2624 }
2725
28- double angle_diff = angles::shortest_angular_distance (curr_pos, target_pos );
29- if (curr_vel > 0 && angle_diff < M_PI / 2 && angle_diff > - M_PI / 2 ){ // FIXME: This is not correct here
30- return {target_pos, target_vel};
31- } else if (curr_vel < 0 && ( angle_diff > M_PI / 2 || angle_diff < - M_PI / 2 )){
32- return {target_pos , target_vel};
33- }else {
34- return {angles::normalize_angle (M_PI * 2 - target_pos ), -target_vel};
26+ double target_angle = atan2 (target_y, target_x );
27+ double angle_diff = angles::shortest_angular_distance (curr_pos, target_angle);
28+
29+ if (angle_diff < (M_PI / 2 ) && angle_diff > -( M_PI / 2 ) ) { // FIXME: This is not correct here
30+ return {target_angle , target_vel};
31+ } else {
32+ return {angles::normalize_angle (M_PI + target_angle ), -target_vel};
3533 }
3634}
3735
38- void AgvWheelKinematics::inverse (Eigen::VectorXd twist , std::vector <double > & curr_pos, std::vector <double > & curr_vel) {
36+ std::pair<std::array< double , 4 > , std::array <double , 4 >> AgvWheelKinematics::inverse (Eigen::VectorXd twist, const std::array <double , 4 > & curr_pos) {
3937 // twist: [vx, vy, wz]
4038 // curr_pos: [left_forward, left_back, right_forward, right_back]
4139 // curr_vel: [left_forward, left_back, right_forward, right_back]
4240
43- if ( curr_pos.size () != 4 || curr_pos.size () != 4 ){
44- // RCLCPP_ERROR();
45- // std::raise(ERROR); // Raise an error
46- }
4741 double vx = twist[0 ];
4842 double vy = twist[1 ];
4943 double wz = twist[2 ];
50- double v = sqrt (vx * vx + vy * vy) / agv_wheel_radius_;
5144 double rot_vel = wz * agv_radius_;
52-
45+
5346 double left_foward_x = vx - rot_vel;
5447 double left_foward_y = vy + rot_vel;
5548 double left_back_x = vx - rot_vel;
@@ -59,32 +52,15 @@ void AgvWheelKinematics::inverse(Eigen::VectorXd twist, std::vector<double> & cu
5952 double right_back_x = vx + rot_vel;
6053 double right_back_y = vy - rot_vel;
6154
62- double left_forward_angle = atan2 (left_foward_y, left_foward_x);
63- double left_back_angle = atan2 (left_back_y, left_back_x);
64- double right_forward_angle = atan2 (right_forward_y, right_forward_x);
65- double right_back_angle = atan2 (right_back_y, right_back_x);
66-
67- // const auto& [curr_pos[0], curr_vel[0]] = get_output(curr_pos[0], curr_vel[0], left_forward_angle, v);
68- // const auto& [curr_pos[1], curr_vel[1]] = get_output(curr_pos[1], curr_vel[1], left_back_angle, v);
69- // const auto& [curr_pos[2], curr_vel[2]] = get_output(curr_pos[2], curr_vel[2], right_forward_angle, v);
70- // const auto& [curr_pos[3], curr_vel[3]] = get_output(curr_pos[3], curr_vel[3], right_back_angle, v);
55+ const auto [left_forward_pos, left_forward_vel] = xy2polar (curr_pos[0 ], left_foward_x, left_foward_y);
56+ const auto [left_back_pos, left_back_vel] = xy2polar (curr_pos[1 ], left_back_x, left_back_y);
57+ const auto [right_forward_pos, right_forward_vel] = xy2polar (curr_pos[2 ], right_forward_x, right_forward_y);
58+ const auto [right_back_pos, right_back_vel] = xy2polar (curr_pos[3 ], right_back_x, right_back_y);
7159
72- const auto & [left_forward_pos, left_forward_vel] = get_output (curr_pos[0 ], curr_vel[0 ], left_forward_angle, v);
73- const auto & [left_back_pos, left_back_vel] = get_output (curr_pos[1 ], curr_vel[1 ], left_back_angle, v);
74- const auto & [right_forward_pos, right_forward_vel] = get_output (curr_pos[2 ], curr_vel[2 ], right_forward_angle, v);
75- const auto & [right_back_pos, right_back_vel] = get_output (curr_pos[3 ], curr_vel[3 ], right_back_angle, v);
60+ std::array<double , 4 > target_pos = {left_forward_pos, left_back_pos, right_forward_pos, right_back_pos};
61+ std::array<double , 4 > target_vel = {left_forward_vel, left_back_vel, right_forward_vel, right_back_vel};
7662
77- curr_pos[0 ] = left_forward_pos;
78- curr_pos[1 ] = left_back_pos;
79- curr_pos[2 ] = right_forward_pos;
80- curr_pos[3 ] = right_back_pos;
81- curr_vel[0 ] = left_forward_vel;
82- curr_vel[1 ] = left_back_vel;
83- curr_vel[2 ] = right_forward_vel;
84- curr_vel[3 ] = right_back_vel;
85-
86-
87-
88- }
63+ return {target_pos, target_vel};
64+ }
8965
9066} // namespace meta_chassis_controller
0 commit comments