Skip to content

Partial attempt at limiting velocity and force together #1258

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

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
+25 −8
Draft

Partial attempt at limiting velocity and force together #1258

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
33 changes: 25 additions & 8 deletions trajoptlib/src/swerve_trajectory_generator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -247,25 +247,42 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
path.drivetrain.wheel_max_angular_velocity;

// |v|₂² ≤ vₘₐₓ²
problem.subject_to(v_wheel_wrt_robot.squared_norm() <=
max_wheel_velocity * max_wheel_velocity);
// problem.subject_to(v_wheel_wrt_robot.squared_norm() <=
// max_wheel_velocity * max_wheel_velocity);

// w.r.t. field, not robot
Translation2v module_f{Fx.at(index).at(module_index),
Fy.at(index).at(module_index)};
auto F_wrt_robot = module_f.rotate_by(-θ_k);

auto Vmax = 12.0;
// TODO real values. 3 volts per m/s = 12 volts for 4 m/s
Translation2v v_volts_wrt_robot = v_wheel_wrt_robot * (Vmax/max_wheel_velocity);
// projection of F_wrt_robot onto v_wheel_wrt_robot
// signifies the force vector in the direction of current velocity
// This division is giving me "nonfinite cost or constraints"
auto proj_v_F = v_wheel_wrt_robot * (F_wrt_robot.dot(v_wheel_wrt_robot))/(v_wheel_wrt_robot.squared_norm());

// τ = r x F
// F = τ/r
double max_wheel_force =
path.drivetrain.wheel_max_torque / path.drivetrain.wheel_radius;

// friction = μmg
double max_friction_force =
path.drivetrain.wheel_cof * path.drivetrain.mass * 9.8;
double volts_per_newton = 12 / max_wheel_force;
Translation2v F_volts_wrt_robot = proj_v_F * volts_per_newton;
auto wheel_voltage_wrt_robot = v_volts_wrt_robot + F_volts_wrt_robot;

problem.subject_to(wheel_voltage_wrt_robot.squared_norm() <= Vmax * Vmax);

// removing traction limiting for now, unsure how it factors in
// // friction = μmg
// double max_friction_force =
// path.drivetrain.wheel_cof * path.drivetrain.mass * 9.8;

double max_force = std::min(max_wheel_force, max_friction_force);
// double max_force = std::min(max_wheel_force, max_friction_force);

// |F|₂² ≤ Fₘₐₓ²
problem.subject_to(module_f.squared_norm() <= max_force * max_force);
// // |F|₂² ≤ Fₘₐₓ²
// problem.subject_to(module_f.squared_norm() <= max_force * max_force);
}

// Apply dynamics constraints
Expand Down
Loading