Skip to content

[Feature] Speed control based on IMU values #33

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
hiikariri wants to merge 7 commits into
base: master
Choose a base branch
from
Open

[Feature] Speed control based on IMU values #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ed815ef
feat: add subscriber for imu
hiikariri Apr 18, 2024
a60f38f
feat: add speed control in locomotion
hiikariri Apr 18, 2024
9c210a0
feat: use time to determine stability
hiikariri May 3, 2024
4f84ba2
Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/…
hiikariri May 21, 2024
f002696
Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/…
hiikariri Jul 7, 2024
9e23698
fix: fix function argument, add enable/disable config
hiikariri Jul 7, 2024
9fcca2e
fix: fix stabilization
hiikariri Jul 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions include/suiryoku/locomotion/model/robot.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@ class Robot
double x_amplitude;
double y_amplitude;
double a_amplitude;

keisan::Vector<3> rpy;

// member for setting
double x_speed;
Expand Down
48 changes: 29 additions & 19 deletions include/suiryoku/locomotion/process/locomotion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,42 +42,45 @@ class Locomotion
bool walk_in_position();
bool walk_in_position_until_stop();

void move_backward(const keisan::Angle<double> & direction);
bool move_backward_to(const keisan::Point2 & target);
void move_backward(const keisan::Angle<double> & direction, float delta_sec);
bool move_backward_to(const keisan::Point2 & target, float delta_sec);

void move_forward(const keisan::Angle<double> & direction);
bool move_forward_to(const keisan::Point2 & target);
void move_forward(const keisan::Angle<double> & direction, float delta_sec);
bool move_forward_to(const keisan::Point2 & target, float delta_sec);

bool rotate_to_target(const keisan::Angle<double> & direction);
bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only);
bool rotate_to_target(const keisan::Angle<double> & direction, float delta_sec);
bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only, float delta_sec);

bool move_follow_head();
bool move_follow_head(const keisan::Angle<double> & min_tilt);
bool move_follow_head(float delta_sec);
bool move_follow_head(const keisan::Angle<double> & min_tilt, float delta_sec);

bool move_skew(const keisan::Angle<double> & direction);
bool move_skew(const keisan::Angle<double> & direction, bool skew_left);
bool move_skew(const keisan::Angle<double> & direction, float delta_sec);
bool move_skew(const keisan::Angle<double> & direction, bool skew_left, float delta_sec);

bool dribble(const keisan::Angle<double> & direction);
bool pivot(const keisan::Angle<double> & direction);
bool pivot_new(const keisan::Angle<double> & direction);
bool dribble(const keisan::Angle<double> & direction, float delta_sec);
bool pivot(const keisan::Angle<double> & direction, float delta_sec);
bool pivot_new(const keisan::Angle<double> & direction, float delta_sec);

bool position_until(
const keisan::Angle<double> & target_pan,
const keisan::Angle<double> & target_tilt,
const keisan::Angle<double> & direction);
bool position_left_kick(const keisan::Angle<double> & direction);
bool position_right_kick(const keisan::Angle<double> & direction);
const keisan::Angle<double> & direction,
float delta_sec);
bool position_left_kick(const keisan::Angle<double> & direction, float delta_sec);
bool position_right_kick(const keisan::Angle<double> & direction, float delta_sec);
bool position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan,
const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt,
const keisan::Angle<double> & max_tilt);
bool position_kick_general(const keisan::Angle<double> & direction);
bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center);
const keisan::Angle<double> & max_tilt, float delta_sec);
bool position_kick_general(const keisan::Angle<double> & direction, float delta_sec);
bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center, 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, float delta_sec);

std::shared_ptr<Robot> get_robot() const;
void update_move_amplitude(double x_amplitude, double y_amplitude);

Expand Down Expand Up @@ -161,6 +164,13 @@ class Locomotion
double skew_pan_comp;
double skew_delta_direction_comp;

double pitch_unstable_threshold;
double roll_unstable_threshold;
bool unstable_pitch;
bool unstable_roll;
float stable_time;
bool using_speed_control;

keisan::Angle<double> left_kick_target_pan;
keisan::Angle<double> left_kick_target_tilt;

Expand Down
28 changes: 14 additions & 14 deletions src/suiryoku/locomotion/control/node/control_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
auto direction = keisan::make_degree(val.get<double>());

process = [this, direction]() {
this->locomotion->move_backward(direction);
this->locomotion->move_backward(direction, 0.0);

return false;
};
Expand All @@ -108,7 +108,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
val["x"].get<double>(), val["y"].get<double>());

process = [this, target]() {
return this->locomotion->move_backward_to(target);
return this->locomotion->move_backward_to(target, 0.0);
};

break;
Expand All @@ -125,7 +125,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
auto direction = keisan::make_degree(val.get<double>());

process = [this, direction]() {
this->locomotion->move_forward(direction);
this->locomotion->move_forward(direction, 0.0);

return false;
};
Expand All @@ -136,7 +136,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
val["x"].get<double>(), val["y"].get<double>());

process = [this, target]() {
return this->locomotion->move_forward_to(target);
return this->locomotion->move_forward_to(target, 0.0);
};
}
}
Expand All @@ -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;
Expand All @@ -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);
};
}

Expand All @@ -196,7 +196,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
auto direction = keisan::make_degree(val.get<double>());

process = [this, direction]() {
return !this->locomotion->move_skew(direction);
return !this->locomotion->move_skew(direction, 0.0);
};
}
}
Expand All @@ -211,7 +211,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
auto direction = keisan::make_degree(val.get<double>());

process = [this, direction]() {
return !this->locomotion->dribble(direction);
return !this->locomotion->dribble(direction, 0.0);
};
}
}
Expand All @@ -226,7 +226,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
auto direction = keisan::make_degree(val.get<double>());

process = [this, direction]() {
return this->locomotion->pivot(direction);
return this->locomotion->pivot(direction, 0.0);
};
}
}
Expand Down Expand Up @@ -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
};
}

Expand Down
3 changes: 2 additions & 1 deletion src/suiryoku/locomotion/model/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
rpy(0.0, 0.0, 0.0)
{
}

Expand Down
2 changes: 2 additions & 0 deletions src/suiryoku/locomotion/node/locomotion_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -50,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<Point2>(
Expand Down
Loading