From ed815efe9e56230d6344fb0e2ecaedb9e9641d05 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Fri, 19 Apr 2024 03:41:57 +0700 Subject: [PATCH 1/5] feat: add subscriber for imu --- include/suiryoku/locomotion/model/robot.hpp | 3 +++ include/suiryoku/locomotion/node/locomotion_node.hpp | 4 ++++ src/suiryoku/locomotion/model/robot.cpp | 3 ++- src/suiryoku/locomotion/node/locomotion_node.cpp | 10 ++++++++++ 4 files changed, 19 insertions(+), 1 deletion(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 86c4517..9842362 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -52,6 +52,9 @@ class Robot double y_amplitude; double a_amplitude; + keisan::Vector<3> gyro; + keisan::Vector<3> accelero; + // member for setting double x_speed; double y_speed; diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index ce8ac3e..4ac972a 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -29,6 +29,7 @@ #include "aruku_interfaces/msg/status.hpp" #include "atama_interfaces/msg/head.hpp" #include "kansei_interfaces/msg/status.hpp" +#include "kansei_interfaces/msg/unit.hpp" #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" #include "suiryoku/locomotion/process/locomotion.hpp" @@ -44,6 +45,7 @@ class LocomotionNode using Point2 = aruku_interfaces::msg::Point2; using SetWalking = aruku_interfaces::msg::SetWalking; using WalkingStatus = aruku_interfaces::msg::Status; + using Unit = kansei_interfaces::msg::Unit; static std::string get_node_prefix(); @@ -66,6 +68,8 @@ class LocomotionNode rclcpp::Subscription::SharedPtr head_subscriber; + rclcpp::Subscription::SharedPtr imu_subscriber; + std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 6e18c78..3beacae 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -33,7 +33,8 @@ Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), - a_amplitude(0.0), is_calibrated(false) + a_amplitude(0.0), is_calibrated(false), + gyro(0.0, 0.0, 0.0), accelero(0.0, 0.0, 0.0) { } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 0ca5e76..95351ed 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -24,6 +24,7 @@ #include "suiryoku/locomotion/node/locomotion_node.hpp" #include "aruku/walking/walking.hpp" +#include "tachimawari/imu/imu.hpp" #include "kansei/measurement/measurement.hpp" #include "keisan/keisan.hpp" #include "nlohmann/json.hpp" @@ -73,6 +74,15 @@ LocomotionNode::LocomotionNode( this->robot->pan = keisan::make_degree(message->pan_angle); this->robot->tilt = keisan::make_degree(message->tilt_angle); }); + + imu_subscriber = node->create_subscription( + tachimawari::imu::ImuNode::unit_topic(), 10, + [this](const Unit::SharedPtr message) { + this->robot->gyro = keisan::Vector<3>( + message->gyro.roll, message->gyro.pitch, message->gyro.yaw); + this->robot->accelero = keisan::Vector<3>( + message->accelero.x, message->accelero.y, message->accelero.z); + }); locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; From a60f38fbe9fe766f0fb31ca51d3c50be691a4937 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Fri, 19 Apr 2024 04:17:19 +0700 Subject: [PATCH 2/5] feat: add speed control in locomotion --- .../locomotion/process/locomotion.hpp | 2 + .../locomotion/process/locomotion.cpp | 80 +++++++------------ 2 files changed, 32 insertions(+), 50 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 8f1703a..14d50e6 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -72,6 +72,8 @@ class Locomotion bool in_pan_kick_range(); bool in_tilt_kick_range(); + void speed_control(double x_speed, double y_speed, double a_speed, bool aim_on); + std::shared_ptr get_robot() const; void update_move_amplitude(double x_amplitude, double y_amplitude); diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index d899dbf..37ff18f 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -190,11 +190,7 @@ void Locomotion::move_backward(const keisan::Angle & direction) x_speed = 0.0; } - robot->x_speed = x_speed; - robot->y_speed = 0.0; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, 0.0, a_speed, false); } bool Locomotion::move_backward_to(const keisan::Point2 & target) @@ -219,11 +215,7 @@ bool Locomotion::move_backward_to(const keisan::Point2 & target) x_speed = 0.0; } - robot->x_speed = x_speed; - robot->y_speed = 0.0; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, 0.0, a_speed, false); return false; } @@ -240,11 +232,7 @@ void Locomotion::move_forward(const keisan::Angle & direction) x_speed = 0.0; } - robot->x_speed = x_speed; - robot->y_speed = 0.0; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, 0.0, a_speed, false); } bool Locomotion::move_forward_to(const keisan::Point2 & target) @@ -274,11 +262,7 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target) x_speed = 0.0; } - robot->x_speed = x_speed; - robot->y_speed = 0.0; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, 0.0, a_speed, false); return false; } @@ -298,11 +282,7 @@ bool Locomotion::rotate_to(const keisan::Angle & direction, bool a_move_ double a_speed = keisan::sign(delta_direction) * -move_max_a; - robot->x_speed = 0.0; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(0.0, y_speed, a_speed, false); return false; } @@ -320,11 +300,7 @@ bool Locomotion::move_follow_head(const keisan::Angle & min_tilt) double x_speed = keisan::map(std::abs(a_speed), 0.0, follow_max_a, follow_max_x, 0.); x_speed = keisan::map((robot->tilt - min_tilt).degree(), 10.0, 0.0, x_speed, 0.0); - robot->x_speed = x_speed; - robot->y_speed = 0.0; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, 0.0, a_speed, false); return robot->tilt < min_tilt; } @@ -381,11 +357,7 @@ bool Locomotion::move_skew(const keisan::Angle & direction, bool skew_le move_x = keisan::map(std::abs(move_a), 0.0, skew_max_a, skew_max_x, 0.0); } - robot->x_speed = move_x; - robot->y_speed = 0.0; - robot->a_speed = move_a; - robot->aim_on = false; - start(); + speed_control(move_x, 0.0, move_a, false); } else { move_follow_head(); } @@ -415,11 +387,7 @@ bool Locomotion::dribble(const keisan::Angle & direction) double a_speed = keisan::map( delta_direction, -15.0, 15.0, dribble_max_a, -dribble_max_a); - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, y_speed, a_speed, false); return is_dribble; } @@ -446,11 +414,7 @@ bool Locomotion::pivot(const keisan::Angle & direction) double a_speed = keisan::map( robot->get_pan().degree(), -10.0, 10.0, pivot_max_a, -pivot_max_a); - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = true; - start(); + speed_control(x_speed, y_speed, a_speed, false); return false; } @@ -491,11 +455,7 @@ bool Locomotion::position_until( double a_speed = keisan::map( delta_direction, -15.0, 15.0, position_max_a, -position_max_a); - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, y_speed, a_speed, false); if (abs_delta_pan < position_min_delta_pan && abs_delta_tilt < position_min_delta_tilt) { return true; @@ -572,6 +532,26 @@ bool Locomotion::in_tilt_kick_range() return tilt > min_target_tilt && tilt < max_target_tilt; } +void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, bool aim_on) +{ + enum gyro{roll, pitch, yaw}; + // Speed control process + if (std::abs(robot->gyro[pitch]) > 15.0 && std::abs(x_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + x_speed = keisan::map(std::abs(robot->gyro[pitch]), 40.0, 15.0, 0.0, x_speed); // TODO: Find the range of max stable imu value to allow x_speed + a_speed = keisan::map(std::abs(robot->gyro[pitch]), 40.0, 15.0, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + } + if (std::abs(robot->gyro[roll]) > 15.0 && std::abs(y_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + y_speed = keisan::map(std::abs(robot->gyro[roll]), 40.0, 15.0, 0.0, y_speed); // TODO: Find the range of max stable imu value to allow x_speed + a_speed = keisan::map(std::abs(robot->gyro[roll]), 40.0, 15.0, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + } + + robot->x_speed = x_speed; + robot->y_speed = y_speed; + robot->a_speed = a_speed; + robot->aim_on = aim_on; + start(); +} + std::shared_ptr Locomotion::get_robot() const { return robot; From 9c210a0a23a2829ab084d8207157922ae327f228 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Sat, 4 May 2024 06:19:43 +0700 Subject: [PATCH 3/5] feat: use time to determine stability --- include/suiryoku/locomotion/model/robot.hpp | 5 +- .../locomotion/node/locomotion_node.hpp | 4 - .../locomotion/process/locomotion.hpp | 41 ++++---- .../locomotion/control/node/control_node.cpp | 28 +++--- src/suiryoku/locomotion/model/robot.cpp | 2 +- .../locomotion/node/locomotion_node.cpp | 10 +- .../locomotion/process/locomotion.cpp | 95 +++++++++++-------- 7 files changed, 99 insertions(+), 86 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 9842362..d710fdf 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -51,9 +51,8 @@ class Robot double x_amplitude; double y_amplitude; double a_amplitude; - - keisan::Vector<3> gyro; - keisan::Vector<3> accelero; + + keisan::Vector<3> rpy; // member for setting double x_speed; diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index 4ac972a..ce8ac3e 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -29,7 +29,6 @@ #include "aruku_interfaces/msg/status.hpp" #include "atama_interfaces/msg/head.hpp" #include "kansei_interfaces/msg/status.hpp" -#include "kansei_interfaces/msg/unit.hpp" #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" #include "suiryoku/locomotion/process/locomotion.hpp" @@ -45,7 +44,6 @@ class LocomotionNode using Point2 = aruku_interfaces::msg::Point2; using SetWalking = aruku_interfaces::msg::SetWalking; using WalkingStatus = aruku_interfaces::msg::Status; - using Unit = kansei_interfaces::msg::Unit; static std::string get_node_prefix(); @@ -68,8 +66,6 @@ class LocomotionNode rclcpp::Subscription::SharedPtr head_subscriber; - rclcpp::Subscription::SharedPtr imu_subscriber; - std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 14d50e6..7074e2a 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -42,37 +42,38 @@ class Locomotion bool walk_in_position(); bool walk_in_position_until_stop(); - void move_backward(const keisan::Angle & direction); - bool move_backward_to(const keisan::Point2 & target); + void move_backward(const keisan::Angle & direction, float delta_sec); + bool move_backward_to(const keisan::Point2 & target, float delta_sec); - void move_forward(const keisan::Angle & direction); - bool move_forward_to(const keisan::Point2 & target); + void move_forward(const keisan::Angle & direction, float delta_sec); + bool move_forward_to(const keisan::Point2 & target, float delta_sec); - bool rotate_to(const keisan::Angle & direction, bool a_move_only); + bool rotate_to(const keisan::Angle & direction, bool a_move_only, float delta_sec); - bool move_follow_head(); - bool move_follow_head(const keisan::Angle & min_tilt); + bool move_follow_head(float delta_sec); + bool move_follow_head(const keisan::Angle & min_tilt, float delta_sec); - bool move_skew(const keisan::Angle & direction); - bool move_skew(const keisan::Angle & direction, bool skew_left); + bool move_skew(const keisan::Angle & direction, float delta_sec); + bool move_skew(const keisan::Angle & direction, bool skew_left, float delta_sec); - bool dribble(const keisan::Angle & direction); - bool pivot(const keisan::Angle & direction); + bool dribble(const keisan::Angle & direction, float delta_sec); + bool pivot(const keisan::Angle & direction, float delta_sec); bool position_until( const keisan::Angle & target_pan, const keisan::Angle & target_tilt, - const keisan::Angle & direction); - bool position_left_kick(const keisan::Angle & direction); - bool position_right_kick(const keisan::Angle & direction); - bool position_kick_general(const keisan::Angle & direction); + const keisan::Angle & direction, + float delta_sec); + bool position_left_kick(const keisan::Angle & direction, float delta_sec); + bool position_right_kick(const keisan::Angle & direction, float delta_sec); + bool position_kick_general(const keisan::Angle & direction, float delta_sec); bool is_time_to_follow(); bool pivot_fulfilled(); bool in_pan_kick_range(); bool in_tilt_kick_range(); - void speed_control(double x_speed, double y_speed, double a_speed, bool aim_on); + void speed_control(double x_speed, double y_speed, double a_speed, bool aim_on, float delta_sec); std::shared_ptr get_robot() const; void update_move_amplitude(double x_amplitude, double y_amplitude); @@ -126,6 +127,14 @@ class Locomotion double skew_pan_comp; double skew_delta_direction_comp; + double pitch_offset = 6.0; + double roll_offset = 1.0; + double pitch_unstable_threshold = 12.0; + double roll_unstable_threshold = 8.0; + bool unstable_pitch; + bool unstable_roll; + float stable_time; + keisan::Angle left_kick_target_pan; keisan::Angle left_kick_target_tilt; diff --git a/src/suiryoku/locomotion/control/node/control_node.cpp b/src/suiryoku/locomotion/control/node/control_node.cpp index 557481c..2471c21 100644 --- a/src/suiryoku/locomotion/control/node/control_node.cpp +++ b/src/suiryoku/locomotion/control/node/control_node.cpp @@ -97,7 +97,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message auto direction = keisan::make_degree(val.get()); process = [this, direction]() { - this->locomotion->move_backward(direction); + this->locomotion->move_backward(direction, 0.0); return false; }; @@ -108,7 +108,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message val["x"].get(), val["y"].get()); process = [this, target]() { - return this->locomotion->move_backward_to(target); + return this->locomotion->move_backward_to(target, 0.0); }; break; @@ -125,7 +125,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message auto direction = keisan::make_degree(val.get()); process = [this, direction]() { - this->locomotion->move_forward(direction); + this->locomotion->move_forward(direction, 0.0); return false; }; @@ -136,7 +136,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message val["x"].get(), val["y"].get()); process = [this, target]() { - return this->locomotion->move_forward_to(target); + return this->locomotion->move_forward_to(target, 0.0); }; } } @@ -158,7 +158,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message } process = [this, direction, a_move_only]() { - return this->locomotion->rotate_to(direction, a_move_only); + return this->locomotion->rotate_to(direction, a_move_only, 0.0); }; break; @@ -178,11 +178,11 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message if (is_default) { process = [this]() { - return this->locomotion->move_follow_head(); + return this->locomotion->move_follow_head(0.0); }; } else { process = [this, min_tilt]() { - return this->locomotion->move_follow_head(min_tilt); + return this->locomotion->move_follow_head(min_tilt, 0.0); }; } @@ -196,7 +196,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message auto direction = keisan::make_degree(val.get()); process = [this, direction]() { - return !this->locomotion->move_skew(direction); + return !this->locomotion->move_skew(direction, 0.0); }; } } @@ -211,7 +211,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message auto direction = keisan::make_degree(val.get()); process = [this, direction]() { - return !this->locomotion->dribble(direction); + return !this->locomotion->dribble(direction, 0.0); }; } } @@ -226,7 +226,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message auto direction = keisan::make_degree(val.get()); process = [this, direction]() { - return this->locomotion->pivot(direction); + return this->locomotion->pivot(direction, 0.0); }; } } @@ -257,19 +257,19 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message if (is_left_kick && is_right_kick) { process = [this, direction]() { - return this->locomotion->position_kick_general(direction); + return this->locomotion->position_kick_general(direction, 0.0); }; } else if (is_left_kick) { process = [this, direction]() { - return this->locomotion->position_left_kick(direction); + return this->locomotion->position_left_kick(direction, 0.0); }; } else if (is_right_kick) { process = [this, direction]() { - return this->locomotion->position_right_kick(direction); + return this->locomotion->position_right_kick(direction, 0.0); }; } else { process = [this, target_pan, target_tilt, direction]() { - return this->locomotion->position_until(target_pan, target_tilt, direction); + return this->locomotion->position_until(target_pan, target_tilt, direction, 0.0); // TODO }; } diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 3beacae..78ecb0a 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -34,7 +34,7 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), - gyro(0.0, 0.0, 0.0), accelero(0.0, 0.0, 0.0) + rpy(0.0, 0.0, 0.0) { } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 95351ed..f8d9a53 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -51,6 +51,7 @@ LocomotionNode::LocomotionNode( [this](const MeasurementStatus::SharedPtr message) { this->robot->is_calibrated = message->is_calibrated; this->robot->orientation = keisan::make_degree(message->orientation.yaw); + this->robot->rpy = keisan::Vector<3>(message->orientation.roll, message->orientation.pitch, message->orientation.yaw); }); set_odometry_publisher = node->create_publisher( @@ -74,15 +75,6 @@ LocomotionNode::LocomotionNode( this->robot->pan = keisan::make_degree(message->pan_angle); this->robot->tilt = keisan::make_degree(message->tilt_angle); }); - - imu_subscriber = node->create_subscription( - tachimawari::imu::ImuNode::unit_topic(), 10, - [this](const Unit::SharedPtr message) { - this->robot->gyro = keisan::Vector<3>( - message->gyro.roll, message->gyro.pitch, message->gyro.yaw); - this->robot->accelero = keisan::Vector<3>( - message->accelero.x, message->accelero.y, message->accelero.z); - }); locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 37ff18f..4433d4b 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -178,7 +178,7 @@ bool Locomotion::walk_in_position_until_stop() return !robot->is_walking; } -void Locomotion::move_backward(const keisan::Angle & direction) +void Locomotion::move_backward(const keisan::Angle & direction, float delta_sec) { auto delta_direction = (direction - robot->orientation).normalize().degree(); @@ -190,10 +190,10 @@ void Locomotion::move_backward(const keisan::Angle & direction) x_speed = 0.0; } - speed_control(x_speed, 0.0, a_speed, false); + speed_control(x_speed, 0.0, a_speed, false, delta_sec); } -bool Locomotion::move_backward_to(const keisan::Point2 & target) +bool Locomotion::move_backward_to(const keisan::Point2 & target, float delta_sec) { double delta_x = (robot->position.x - target.x); double delta_y = (robot->position.y - target.y); @@ -215,12 +215,12 @@ bool Locomotion::move_backward_to(const keisan::Point2 & target) x_speed = 0.0; } - speed_control(x_speed, 0.0, a_speed, false); + speed_control(x_speed, 0.0, a_speed, false, delta_sec); return false; } -void Locomotion::move_forward(const keisan::Angle & direction) +void Locomotion::move_forward(const keisan::Angle & direction, float delta_sec) { auto delta_direction = (direction - robot->orientation).normalize().degree(); @@ -232,10 +232,10 @@ void Locomotion::move_forward(const keisan::Angle & direction) x_speed = 0.0; } - speed_control(x_speed, 0.0, a_speed, false); + speed_control(x_speed, 0.0, a_speed, false, delta_sec); } -bool Locomotion::move_forward_to(const keisan::Point2 & target) +bool Locomotion::move_forward_to(const keisan::Point2 & target, float delta_sec) { double delta_x = (target.x - robot->position.x); double delta_y = (target.y - robot->position.y); @@ -262,12 +262,12 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target) x_speed = 0.0; } - speed_control(x_speed, 0.0, a_speed, false); + speed_control(x_speed, 0.0, a_speed, false, delta_sec); return false; } -bool Locomotion::rotate_to(const keisan::Angle & direction, bool a_move_only) +bool Locomotion::rotate_to(const keisan::Angle & direction, bool a_move_only, float delta_sec) { auto delta_direction = (direction - robot->orientation).normalize().degree(); @@ -282,17 +282,17 @@ bool Locomotion::rotate_to(const keisan::Angle & direction, bool a_move_ double a_speed = keisan::sign(delta_direction) * -move_max_a; - speed_control(0.0, y_speed, a_speed, false); + speed_control(0.0, y_speed, a_speed, false, delta_sec); return false; } -bool Locomotion::move_follow_head() +bool Locomotion::move_follow_head(float delta_sec) { - return move_follow_head(follow_min_tilt); + return move_follow_head(follow_min_tilt, delta_sec); } -bool Locomotion::move_follow_head(const keisan::Angle & min_tilt) +bool Locomotion::move_follow_head(const keisan::Angle & min_tilt, float delta_sec) { double a_speed = keisan::map( robot->pan.degree(), -10.0, 10.0, -follow_max_a, follow_max_a); @@ -300,19 +300,19 @@ bool Locomotion::move_follow_head(const keisan::Angle & min_tilt) double x_speed = keisan::map(std::abs(a_speed), 0.0, follow_max_a, follow_max_x, 0.); x_speed = keisan::map((robot->tilt - min_tilt).degree(), 10.0, 0.0, x_speed, 0.0); - speed_control(x_speed, 0.0, a_speed, false); + speed_control(x_speed, 0.0, a_speed, false, delta_sec); return robot->tilt < min_tilt; } -bool Locomotion::move_skew(const keisan::Angle & direction) +bool Locomotion::move_skew(const keisan::Angle & direction, float delta_sec) { auto current_direction = (robot->orientation - robot->pan).normalize(); auto delta_direction = (direction - current_direction).normalize().degree(); - return move_skew(direction, delta_direction > 0); + return move_skew(direction, delta_direction > 0, delta_sec); } -bool Locomotion::move_skew(const keisan::Angle & direction, bool skew_left) +bool Locomotion::move_skew(const keisan::Angle & direction, bool skew_left, float delta_sec) { auto current_direction = (robot->orientation - robot->pan).normalize(); double delta_direction = (direction - current_direction).normalize().degree(); @@ -357,13 +357,13 @@ bool Locomotion::move_skew(const keisan::Angle & direction, bool skew_le move_x = keisan::map(std::abs(move_a), 0.0, skew_max_a, skew_max_x, 0.0); } - speed_control(move_x, 0.0, move_a, false); + speed_control(move_x, 0.0, move_a, false, delta_sec); } else { - move_follow_head(); + move_follow_head(delta_sec); } } -bool Locomotion::dribble(const keisan::Angle & direction) +bool Locomotion::dribble(const keisan::Angle & direction, float delta_sec) { double pan = robot->get_pan().degree(); bool is_dribble = true; @@ -387,12 +387,12 @@ bool Locomotion::dribble(const keisan::Angle & direction) double a_speed = keisan::map( delta_direction, -15.0, 15.0, dribble_max_a, -dribble_max_a); - speed_control(x_speed, y_speed, a_speed, false); + speed_control(x_speed, y_speed, a_speed, false, delta_sec); return is_dribble; } -bool Locomotion::pivot(const keisan::Angle & direction) +bool Locomotion::pivot(const keisan::Angle & direction, float delta_sec) { auto delta_direction = (direction - robot->orientation).normalize().degree(); @@ -414,7 +414,7 @@ bool Locomotion::pivot(const keisan::Angle & direction) double a_speed = keisan::map( robot->get_pan().degree(), -10.0, 10.0, pivot_max_a, -pivot_max_a); - speed_control(x_speed, y_speed, a_speed, false); + speed_control(x_speed, y_speed, a_speed, true, delta_sec); return false; } @@ -422,7 +422,8 @@ bool Locomotion::pivot(const keisan::Angle & direction) bool Locomotion::position_until( const keisan::Angle & target_pan, const keisan::Angle & target_tilt, - const keisan::Angle & direction) + const keisan::Angle & direction, + float delta_sec) { double delta_pan = std::abs((target_pan - robot->get_pan()).degree()); double delta_tilt = std::abs((target_tilt - robot->get_tilt()).degree()); @@ -455,7 +456,7 @@ bool Locomotion::position_until( double a_speed = keisan::map( delta_direction, -15.0, 15.0, position_max_a, -position_max_a); - speed_control(x_speed, y_speed, a_speed, false); + speed_control(x_speed, y_speed, a_speed, false, delta_sec); if (abs_delta_pan < position_min_delta_pan && abs_delta_tilt < position_min_delta_tilt) { return true; @@ -464,19 +465,19 @@ bool Locomotion::position_until( return false; } -bool Locomotion::position_left_kick(const keisan::Angle & direction) +bool Locomotion::position_left_kick(const keisan::Angle & direction, float delta_sec) { return position_until( - left_kick_target_pan, left_kick_target_tilt, direction); + left_kick_target_pan, left_kick_target_tilt, direction, delta_sec); } -bool Locomotion::position_right_kick(const keisan::Angle & direction) +bool Locomotion::position_right_kick(const keisan::Angle & direction, float delta_sec) { return position_until( - right_kick_target_pan, right_kick_target_tilt, direction); + right_kick_target_pan, right_kick_target_tilt, direction, delta_sec); } -bool Locomotion::position_kick_general(const keisan::Angle & direction) +bool Locomotion::position_kick_general(const keisan::Angle & direction, float delta_sec) { double delta_pan_left = std::abs((left_kick_target_pan - robot->get_pan()).degree()); double delta_tilt_left = std::abs((left_kick_target_tilt - robot->get_tilt()).degree()); @@ -503,7 +504,7 @@ bool Locomotion::position_kick_general(const keisan::Angle & direction) target_tilt = right_kick_target_tilt; } - return position_until(target_pan, target_tilt, direction); + return position_until(target_pan, target_tilt, direction, delta_sec); } bool Locomotion::is_time_to_follow() @@ -532,17 +533,33 @@ bool Locomotion::in_tilt_kick_range() return tilt > min_target_tilt && tilt < max_target_tilt; } -void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, bool aim_on) +void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, bool aim_on, float delta_sec) { - enum gyro{roll, pitch, yaw}; + enum rpy{roll, pitch, yaw}; // Speed control process - if (std::abs(robot->gyro[pitch]) > 15.0 && std::abs(x_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking - x_speed = keisan::map(std::abs(robot->gyro[pitch]), 40.0, 15.0, 0.0, x_speed); // TODO: Find the range of max stable imu value to allow x_speed - a_speed = keisan::map(std::abs(robot->gyro[pitch]), 40.0, 15.0, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + if (std::abs(robot->rpy[pitch]) - pitch_offset > pitch_unstable_threshold && std::abs(x_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + std::cout << "Unstable walking detected!" << std::endl; + unstable_pitch = true; + stable_time = 0.0; + } else if (std::abs(robot->rpy[roll]) > 15.0 && std::abs(y_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + std::cout << "Unstable walking detected!" << std::endl; + unstable_roll = true; + stable_time = 0.0; + } else { + stable_time += delta_sec; + if (stable_time > 5.0) { + unstable_pitch = false; + unstable_roll = false; + } + } + + if (unstable_pitch) { + x_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, x_speed); // TODO: Find the range of max stable imu value to allow x_speed + a_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed } - if (std::abs(robot->gyro[roll]) > 15.0 && std::abs(y_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking - y_speed = keisan::map(std::abs(robot->gyro[roll]), 40.0, 15.0, 0.0, y_speed); // TODO: Find the range of max stable imu value to allow x_speed - a_speed = keisan::map(std::abs(robot->gyro[roll]), 40.0, 15.0, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + if (unstable_roll) { + y_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, y_speed); // TODO: Find the range of max stable imu value to allow x_speed + a_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed } robot->x_speed = x_speed; From 9e2369859dda9a3363c9d4f159ab7f9fee9cf37b Mon Sep 17 00:00:00 2001 From: hiikariri Date: Mon, 8 Jul 2024 00:30:54 +0700 Subject: [PATCH 4/5] fix: fix function argument, add enable/disable config --- .../locomotion/process/locomotion.hpp | 11 +-- .../locomotion/process/locomotion.cpp | 72 +++++++++++-------- 2 files changed, 49 insertions(+), 34 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index fd24892..7ecd795 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -59,7 +59,7 @@ class Locomotion bool dribble(const keisan::Angle & direction, float delta_sec); bool pivot(const keisan::Angle & direction, float delta_sec); - bool pivot_new(const keisan::Angle & direction); + bool pivot_new(const keisan::Angle & direction, float delta_sec); bool position_until( const keisan::Angle & target_pan, @@ -164,13 +164,14 @@ class Locomotion double skew_pan_comp; double skew_delta_direction_comp; - double pitch_offset = 6.0; - double roll_offset = 1.0; - double pitch_unstable_threshold = 12.0; - double roll_unstable_threshold = 8.0; + double pitch_offset; + double roll_offset; + double pitch_unstable_threshold; + double roll_unstable_threshold; bool unstable_pitch; bool unstable_roll; float stable_time; + bool using_speed_control; keisan::Angle left_kick_target_pan; keisan::Angle left_kick_target_tilt; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index e22b031..28f63db 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -287,6 +287,24 @@ void Locomotion::set_config(const nlohmann::json & json) valid_config = false; } + nlohmann::json speed_control_section; + if (jitsuyo::assign_val(json, "speed_control", speed_control_section)) { + bool valid_section = true; + + valid_section &= jitsuyo::assign_val(speed_control_section, "pitch_offset", pitch_offset); + valid_section &= jitsuyo::assign_val(speed_control_section, "roll_offset", roll_offset); + valid_section &= jitsuyo::assign_val(speed_control_section, "pitch_unstable_threshold", pitch_unstable_threshold); + valid_section &= jitsuyo::assign_val(speed_control_section, "roll_unstable_threshold", roll_unstable_threshold); + valid_section &= jitsuyo::assign_val(speed_control_section, "using_speed_control", using_speed_control); + + if (!valid_section) { + std::cout << "Error found at section `speed_control`" << std::endl; + valid_config = false; + } + } else { + valid_config = false; + } + if (!valid_config) { throw std::runtime_error("Failed to load config file `locomotion.json`"); } @@ -566,11 +584,7 @@ bool Locomotion::move_skew(const keisan::Angle & direction, bool skew_le move_x = keisan::map(std::abs(move_a), 0.0, skew_max_a, skew_max_x, 0.0); } - robot->x_speed = move_x; - robot->y_speed = 0.0; - robot->a_speed = move_a; - robot->aim_on = false; - start(); + speed_control(move_x, 0.0, move_a, false, delta_sec); return false; } else { @@ -626,16 +640,12 @@ bool Locomotion::dribble(const keisan::Angle & direction, float delta_se x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio); y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio); - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = false; - start(); + speed_control(x_speed, y_speed, a_speed, false, delta_sec); return false; } -bool Locomotion::pivot(const keisan::Angle & direction) +bool Locomotion::pivot(const keisan::Angle & direction, float delta_sec) { if (initial_pivot) { initial_pivot = false; @@ -673,17 +683,13 @@ bool Locomotion::pivot(const keisan::Angle & direction) if (fabs(pan) > pivot_pan_range_a_speed) { a_speed = keisan::map(pan, -pivot_pan_range_a_speed, pivot_pan_range_a_speed, pivot_max_a, -pivot_max_a); } - - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = true; - start(); + + speed_control(x_speed, y_speed, a_speed, true, delta_sec); return false; } -bool Locomotion::pivot_new(const keisan::Angle & direction) +bool Locomotion::pivot_new(const keisan::Angle & direction, float delta_sec) { if (initial_pivot) { initial_pivot = false; @@ -728,11 +734,7 @@ bool Locomotion::pivot_new(const keisan::Angle & direction) : keisan::map(delta_direction, 180.0, 0.0, pivot_max_a, pivot_max_a * 0.9); } - robot->x_speed = x_speed; - robot->y_speed = y_speed; - robot->a_speed = a_speed; - robot->aim_on = true; - start(); + speed_control(x_speed, y_speed, a_speed, true, delta_sec); return false; } @@ -783,6 +785,7 @@ bool Locomotion::position_until( y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio); speed_control(x_speed, y_speed, a_speed, false, delta_sec); + if (std::abs(delta_tilt) < position_min_delta_tilt.degree() && std::abs(delta_pan) < position_min_delta_pan.degree()) { printf("done by pan tilt\n"); return true; @@ -939,6 +942,7 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio); y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio); a_speed = keisan::smooth(robot->a_speed, a_speed, smooth_ratio); + speed_control(x_speed, y_speed, a_speed, false, delta_sec); printf("delta pan %.1f, delta tilt %.1f, delta direction %.1f\n", delta_pan, delta_tilt, delta_direction); @@ -976,11 +980,21 @@ void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, b { enum rpy{roll, pitch, yaw}; // Speed control process - if (std::abs(robot->rpy[pitch]) - pitch_offset > pitch_unstable_threshold && std::abs(x_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + + if (!using_speed_control) { + robot->x_speed = x_speed; + robot->y_speed = y_speed; + robot->a_speed = a_speed; + robot->aim_on = aim_on; + start(); + return; + } + + if (std::abs(robot->rpy[pitch]) - pitch_offset > pitch_unstable_threshold) { std::cout << "Unstable walking detected!" << std::endl; unstable_pitch = true; stable_time = 0.0; - } else if (std::abs(robot->rpy[roll]) > 15.0 && std::abs(y_speed) > 0.0) { // TODO: Find the correct value to determine unstable walking + } else if (std::abs(robot->rpy[roll]) - roll_offset > roll_unstable_threshold) { std::cout << "Unstable walking detected!" << std::endl; unstable_roll = true; stable_time = 0.0; @@ -993,12 +1007,12 @@ void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, b } if (unstable_pitch) { - x_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, x_speed); // TODO: Find the range of max stable imu value to allow x_speed - a_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + x_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, x_speed); + a_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, a_speed); } if (unstable_roll) { - y_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, y_speed); // TODO: Find the range of max stable imu value to allow x_speed - a_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, a_speed); // TODO: Find the range of max stable imu value to allow a_speed + y_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, y_speed); + a_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, a_speed); } robot->x_speed = x_speed; From 9fcca2ee6f75b2fd8ea22fa894b7dd25a8521208 Mon Sep 17 00:00:00 2001 From: hiikariri Date: Mon, 8 Jul 2024 20:38:31 +0700 Subject: [PATCH 5/5] fix: fix stabilization --- .../locomotion/process/locomotion.hpp | 2 -- .../locomotion/process/locomotion.cpp | 23 +++++++++++-------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 7ecd795..7e74b38 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -164,8 +164,6 @@ class Locomotion double skew_pan_comp; double skew_delta_direction_comp; - double pitch_offset; - double roll_offset; double pitch_unstable_threshold; double roll_unstable_threshold; bool unstable_pitch; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 28f63db..4d07350 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -291,8 +291,6 @@ void Locomotion::set_config(const nlohmann::json & json) if (jitsuyo::assign_val(json, "speed_control", speed_control_section)) { bool valid_section = true; - valid_section &= jitsuyo::assign_val(speed_control_section, "pitch_offset", pitch_offset); - valid_section &= jitsuyo::assign_val(speed_control_section, "roll_offset", roll_offset); valid_section &= jitsuyo::assign_val(speed_control_section, "pitch_unstable_threshold", pitch_unstable_threshold); valid_section &= jitsuyo::assign_val(speed_control_section, "roll_unstable_threshold", roll_unstable_threshold); valid_section &= jitsuyo::assign_val(speed_control_section, "using_speed_control", using_speed_control); @@ -989,17 +987,20 @@ void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, b start(); return; } + std::cout << "pitch " << std::abs(robot->rpy[pitch]) << std::endl; + std::cout << "roll " << std::abs(robot->rpy[roll]) << std::endl; - if (std::abs(robot->rpy[pitch]) - pitch_offset > pitch_unstable_threshold) { - std::cout << "Unstable walking detected!" << std::endl; + if (std::abs(robot->rpy[pitch]) > pitch_unstable_threshold) { + std::cout << "unstable walking detected!" << std::endl; unstable_pitch = true; stable_time = 0.0; - } else if (std::abs(robot->rpy[roll]) - roll_offset > roll_unstable_threshold) { - std::cout << "Unstable walking detected!" << std::endl; + } else if (std::abs(robot->rpy[roll]) > roll_unstable_threshold) { + std::cout << "unstable walking detected!" << std::endl; unstable_roll = true; stable_time = 0.0; } else { stable_time += delta_sec; + std::cout << "stable time: " << stable_time << std::endl; if (stable_time > 5.0) { unstable_pitch = false; unstable_roll = false; @@ -1007,12 +1008,14 @@ void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, b } if (unstable_pitch) { - x_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, x_speed); - a_speed = keisan::map(std::abs(robot->rpy[pitch]) - pitch_offset, 25.0, pitch_unstable_threshold, 0.0, a_speed); + std::cout << "pitch speed stabilization trigerred!" << std::endl; + x_speed = keisan::map(std::abs(robot->rpy[pitch]), 25.0, pitch_unstable_threshold, 0.0, x_speed - 5.0); + a_speed = keisan::map(std::abs(robot->rpy[pitch]), 25.0, pitch_unstable_threshold, 0.0, a_speed); } if (unstable_roll) { - y_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, y_speed); - a_speed = keisan::map(std::abs(robot->rpy[roll]) - roll_offset, 20.0, roll_unstable_threshold, 0.0, a_speed); + std::cout << "roll speed stabilization trigerred!" << std::endl; + y_speed = keisan::map(std::abs(robot->rpy[roll]), 20.0, roll_unstable_threshold, 0.0, y_speed- 5.0); + a_speed = keisan::map(std::abs(robot->rpy[roll]), 20.0, roll_unstable_threshold, 0.0, a_speed); } robot->x_speed = x_speed;