Skip to content

fix(crane_physics): FLYINGボールの着地時間計算でpos_zを考慮するよう修正 #1390

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
HansRobo wants to merge 1 commit into
base: develop
Choose a base branch
from
Open

fix(crane_physics): FLYINGボールの着地時間計算でpos_zを考慮するよう修正 #1390

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 7 additions & 2 deletions utility/crane_physics/include/crane_physics/ball_physics_model.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ class BallPhysicsModel
const Point & position, const Point & velocity, Ball::State state, double pos_z, double vel_z,
double time_ahead) const -> Point;

[[nodiscard]] auto getStopTime(const Point & velocity, Ball::State state, double vel_z) const
-> double;
[[nodiscard]] auto getStopTime(
const Point & velocity, Ball::State state, double pos_z, double vel_z) const -> double;

[[nodiscard]] auto getMaxDistance(
const Point & position, const Point & velocity, Ball::State state, double pos_z,
Expand All @@ -86,6 +86,11 @@ class BallPhysicsModel
Config config_;

// ヘルパー関数
// FLYING状態のボールが着地するまでの時間を計算する。
// z(t) = pos_z + vel_z * t + 0.5 * gravity * t^2 = 0 の正の最小根を返す。
// 判別式が負、または正の根が存在しない場合は 0 を返す。
[[nodiscard]] auto getFlyingLandingTime(double pos_z, double vel_z) const -> double;

[[nodiscard]] auto getRollingStopTime(const Point & velocity) const -> double;

[[nodiscard]] auto getRollingMaxDistance(const Point & velocity) const -> double;
Expand Down
5 changes: 4 additions & 1 deletion utility/crane_physics/src/ball_info.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,10 @@ auto Ball::getPredictedVelocity(double time_ahead) const -> Point
return physics_model_->predictVelocity(pos, vel, state, pos_z, vel_z, time_ahead);
}

auto Ball::getStopTime() const -> double { return physics_model_->getStopTime(vel, state, vel_z); }
auto Ball::getStopTime() const -> double
{
return physics_model_->getStopTime(vel, state, pos_z, vel_z);
}

auto Ball::getMaxDistance() const -> double
{
Expand Down
48 changes: 35 additions & 13 deletions utility/crane_physics/src/ball_physics_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -233,8 +233,8 @@ auto BallPhysicsModel::predictVelocity(
return {0, 0};
}

auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, double vel_z) const
-> double
auto BallPhysicsModel::getStopTime(
const Point & velocity, Ball::State state, double pos_z, double vel_z) const -> double
{
switch (state) {
case Ball::State::STOPPED:
Expand All @@ -244,11 +244,8 @@ auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, do
return getRollingStopTime(velocity);

case Ball::State::FLYING: {
// 簡単な着地時間計算（pos_z=0と仮定）
double landing_time = 0.0;
if (vel_z != 0) {
landing_time = std::max(0.0, -2.0 * vel_z / config_.gravity);
}
// 現在高度 pos_z を考慮した着地時間を計算する。
double landing_time = getFlyingLandingTime(pos_z, vel_z);

double rolling_stop_time = getRollingStopTime(velocity);
return landing_time + rolling_stop_time;
Expand All @@ -258,7 +255,7 @@ auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, do
}

auto BallPhysicsModel::getMaxDistance(
const Point & position, const Point & velocity, Ball::State state, [[maybe_unused]] double pos_z,
const Point & position, const Point & velocity, Ball::State state, double pos_z,
double vel_z) const -> double
{
switch (state) {
Expand All @@ -269,11 +266,8 @@ auto BallPhysicsModel::getMaxDistance(
return getRollingMaxDistance(velocity);

case Ball::State::FLYING: {
// 着地位置計算
double landing_time = 0.0;
if (vel_z != 0) {
landing_time = std::max(0.0, -2.0 * vel_z / config_.gravity);
}
// 現在高度 pos_z を考慮した着地時間から着地位置を計算する。
double landing_time = getFlyingLandingTime(pos_z, vel_z);

Point landing_pos;
landing_pos.x() = position.x() + velocity.x() * landing_time;
Expand All @@ -288,6 +282,34 @@ auto BallPhysicsModel::getMaxDistance(
return 0.0;
}

auto BallPhysicsModel::getFlyingLandingTime(double pos_z, double vel_z) const -> double
{
// z(t) = pos_z + vel_z * t + 0.5 * gravity * t^2 = 0 を解く（gravity は負値）。
// predictPosition / predictVelocity と同一のロジックで現在高度 pos_z を考慮する。
double a = 0.5 * config_.gravity;
double b = vel_z;
double c = pos_z;

double landing_time = 0.0;
double discriminant = b * b - 4 * a * c;

if (discriminant >= 0) {
double sqrt_discriminant = std::sqrt(discriminant);
double t1 = (-b + sqrt_discriminant) / (2 * a);
double t2 = (-b - sqrt_discriminant) / (2 * a);

if (t1 > 1e-6 && t2 > 1e-6) {
landing_time = std::min(t1, t2);
} else if (t1 > 1e-6) {
landing_time = t1;
} else if (t2 > 1e-6) {
landing_time = t2;
}
}

return landing_time;
}

auto BallPhysicsModel::getRollingStopTime(const Point & velocity) const -> double
{
double speed = velocity.norm();
Expand Down
Loading