diff --git a/src-core/src/generation/generate.rs b/src-core/src/generation/generate.rs index d5999318ec..8bc43c8aaa 100644 --- a/src-core/src/generation/generate.rs +++ b/src-core/src/generation/generate.rs @@ -3,7 +3,7 @@ use std::sync::OnceLock; use std::sync::mpsc::{Receiver, Sender, channel}; -use trajoptlib::{DifferentialTrajectory, SwerveTrajectory}; +use trajoptlib::{DifferentialTrajectory, MecanumTrajectory, SwerveTrajectory}; use super::transformers::{ CallbackSetter, ConstraintSetter, DrivetrainAndBumpersSetter, IntervalCountSetter, @@ -30,6 +30,9 @@ pub enum LocalProgressUpdate { // Diff variant update: Vec, }, + MecanumTrajectory { + update: Vec, + }, DiagnosticText { update: String, }, @@ -68,6 +71,14 @@ impl From> for LocalProgressUpdate { } } +impl From for LocalProgressUpdate { + fn from(trajectory: MecanumTrajectory) -> Self { + LocalProgressUpdate::MecanumTrajectory { + update: trajectory.samples.iter().map(Sample::from).collect(), + } + } +} + pub struct HandledLocalProgressUpdate { pub handle: i64, pub update: LocalProgressUpdate, diff --git a/src-core/src/generation/remote.rs b/src-core/src/generation/remote.rs index 9046bb1a7f..0f709807e3 100644 --- a/src-core/src/generation/remote.rs +++ b/src-core/src/generation/remote.rs @@ -94,6 +94,9 @@ pub enum RemoteProgressUpdate { IncompleteTankTrajectory(Vec), IntervalCounts(Vec), CompleteTrajectory(TrajectoryFile), + // Mecanum variant + IncompleteMecanumTrajectory(Vec), + CompleteTrajectory(Trajectory), Error(ChoreoError), } @@ -123,6 +126,11 @@ pub fn remote_generate_child(args: RemoteArgs) { update: LocalProgressUpdate::IntervalCounts { update }, .. } => serde_json::to_string(&RemoteProgressUpdate::IntervalCounts(update)), + update: LocalProgressUpdate::MecanumTrajectory { update }, + .. + } => serde_json::to_string(&RemoteProgressUpdate::IncompleteMecanumTrajectory( + update, + )), _ => continue, } .expect("Failed to serialize progress update"); @@ -317,6 +325,13 @@ pub async fn remote_generate_parent( }.handled(handle) ); }, + Ok(RemoteProgressUpdate::IncompleteMecanumTrajectory(trajectory)) => { + remote_resources.emit_progress( + LocalProgressUpdate::MecanumTrajectory { + update: trajectory + }.handled(handle) + ); + }, Ok(RemoteProgressUpdate::CompleteTrajectory(trajectory)) => { break Ok(trajectory); }, diff --git a/src-core/src/generation/transformers/callback.rs b/src-core/src/generation/transformers/callback.rs index b2e1b87c4c..b4ae16fc53 100644 --- a/src-core/src/generation/transformers/callback.rs +++ b/src-core/src/generation/transformers/callback.rs @@ -1,4 +1,4 @@ -use trajoptlib::{DifferentialTrajectory, SwerveTrajectory}; +use trajoptlib::{DifferentialTrajectory, MecanumTrajectory, SwerveTrajectory}; use crate::{ ResultExt, @@ -7,7 +7,7 @@ use crate::{ use super::{ DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, - SwerveGenerationTransformer, + MecanumGenerationTransformer, SwerveGenerationTransformer, }; pub struct CallbackSetter; @@ -30,6 +30,15 @@ fn differential_status_callback(trajectory: DifferentialTrajectory, handle: i64) }; } +fn mecanum_status_callback(trajectory: MecanumTrajectory, handle: i64) { + let tx_opt = PROGRESS_SENDER_LOCK.get(); + if let Some(tx) = tx_opt { + let _ = tx + .send(LocalProgressUpdate::from(trajectory).handled(handle)) + .trace_warn(); + }; +} + impl SwerveGenerationTransformer for CallbackSetter { fn initialize(_: &GenerationContext) -> FeatureLockedTransformer { FeatureLockedTransformer::always(Self) @@ -49,3 +58,13 @@ impl DifferentialGenerationTransformer for CallbackSetter { generator.add_callback(differential_status_callback); } } + +impl MecanumGenerationTransformer for CallbackSetter { + fn initialize(_: &GenerationContext) -> FeatureLockedTransformer { + FeatureLockedTransformer::always(Self) + } + + fn transform(&self, generator: &mut trajoptlib::MecanumTrajectoryGenerator) { + generator.add_callback(mecanum_status_callback); + } +} diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 6b6b78df8c..b673addd87 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -2,7 +2,7 @@ use crate::spec::trajectory::{ConstraintData, ConstraintIDX, ConstraintScope, Wa use super::{ DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, - SwerveGenerationTransformer, + MecanumGenerationTransformer, SwerveGenerationTransformer, }; fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { @@ -228,3 +228,71 @@ impl DifferentialGenerationTransformer for ConstraintSetter { } } } + +impl MecanumGenerationTransformer for ConstraintSetter { + fn initialize(context: &GenerationContext) -> FeatureLockedTransformer { + Self::initialize(context) + } + + fn transform(&self, generator: &mut trajoptlib::MecanumTrajectoryGenerator) { + for constraint in &self.constraint_idx { + let from = fix_scope(constraint.from, &self.guess_points); + let to_opt = constraint.to.map(|idx| fix_scope(idx, &self.guess_points)); + match constraint.data { + ConstraintData::PointAt { + x, + y, + tolerance, + flip, + } => match to_opt { + None => generator.wpt_point_at(from, x, y, tolerance, flip), + Some(to) => generator.sgmt_point_at(from, to, x, y, tolerance, flip), + }, + ConstraintData::MaxVelocity { max } => match to_opt { + None => generator.wpt_linear_velocity_max_magnitude(from, max), + Some(to) => generator.sgmt_linear_velocity_max_magnitude(from, to, max), + }, + ConstraintData::MaxAcceleration { max } => match to_opt { + None => generator.wpt_linear_acceleration_max_magnitude(from, max), + Some(to) => generator.sgmt_linear_acceleration_max_magnitude(from, to, max), + }, + ConstraintData::MaxAngularVelocity { max } => match to_opt { + None => generator.wpt_angular_velocity_max_magnitude(from, max), + Some(to) => generator.sgmt_angular_velocity_max_magnitude(from, to, max), + }, + ConstraintData::StopPoint {} => { + if to_opt.is_none() { + generator.wpt_linear_velocity_max_magnitude(from, 0.0f64); + generator.wpt_angular_velocity_max_magnitude(from, 0.0f64); + } + } + ConstraintData::KeepInCircle { x, y, r } => match to_opt { + None => generator.wpt_keep_in_circle(from, x, y, r), + Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r), + }, + ConstraintData::KeepInRectangle { x, y, w, h } => { + let xs = vec![x, x + w, x + w, x]; + let ys = vec![y, y, y + h, y + h]; + match to_opt { + None => generator.wpt_keep_in_polygon(from, xs, ys), + Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys), + } + } + ConstraintData::KeepInLane { tolerance } => { + if let Some(idx_to) = to_opt + && let Some(wpt_from) = self.waypoint_idx.get(from) + && let Some(wpt_to) = self.waypoint_idx.get(idx_to) + { + generator.sgmt_keep_in_lane( + from, idx_to, wpt_from.x, wpt_from.y, wpt_to.x, wpt_to.y, tolerance, + ); + } + } + ConstraintData::KeepOutCircle { x, y, r } => match to_opt { + None => generator.wpt_keep_out_circle(from, x, y, r), + Some(to) => generator.sgmt_keep_out_circle(from, to, x, y, r), + }, + }; + } + } +} diff --git a/src-core/src/generation/transformers/drivetrain_and_bumpers.rs b/src-core/src/generation/transformers/drivetrain_and_bumpers.rs index e85f18be52..8f4c042523 100644 --- a/src-core/src/generation/transformers/drivetrain_and_bumpers.rs +++ b/src-core/src/generation/transformers/drivetrain_and_bumpers.rs @@ -1,10 +1,10 @@ -use trajoptlib::{DifferentialDrivetrain, SwerveDrivetrain}; +use trajoptlib::{DifferentialDrivetrain, MecanumDrivetrain, SwerveDrivetrain}; use crate::spec::project::RobotConfig; use super::{ DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, - SwerveGenerationTransformer, + MecanumGenerationTransformer, SwerveGenerationTransformer, }; pub struct DrivetrainAndBumpersSetter { @@ -70,3 +70,35 @@ impl DifferentialGenerationTransformer for DrivetrainAndBumpersSetter { ); } } + +impl MecanumGenerationTransformer for DrivetrainAndBumpersSetter { + fn initialize(context: &GenerationContext) -> FeatureLockedTransformer { + FeatureLockedTransformer::always(Self { + config: context.project.config.snapshot(), + }) + } + + fn transform(&self, generator: &mut trajoptlib::MecanumTrajectoryGenerator) { + let config = &self.config; + let drivetrain = MecanumDrivetrain { + mass: config.mass, + moi: config.inertia, + wheel_radius: config.radius, + // rad per sec + wheel_max_angular_velocity: config.vmax / config.gearing, + wheel_max_torque: config.tmax * config.gearing, + wheel_cof: config.cof, + static_friction_coefficient: 0.0, + strafe_efficiency: 0.75, + modules: config.module_translations(), + }; + + generator.set_drivetrain(&drivetrain); + generator.set_bumpers( + config.bumper.front, + config.bumper.side, + config.bumper.side, + config.bumper.back, + ); + } +} diff --git a/src-core/src/generation/transformers/interval_count.rs b/src-core/src/generation/transformers/interval_count.rs index 887b4859c9..f092ee91f7 100644 --- a/src-core/src/generation/transformers/interval_count.rs +++ b/src-core/src/generation/transformers/interval_count.rs @@ -4,7 +4,7 @@ use crate::spec::trajectory::Waypoint; use super::{ DifferentialGenerationTransformer, FeatureLockedTransformer, GenerationContext, - SwerveGenerationTransformer, + MecanumGenerationTransformer, SwerveGenerationTransformer, }; pub struct IntervalCountSetter { @@ -108,3 +108,56 @@ impl DifferentialGenerationTransformer for IntervalCountSetter { generator.set_control_interval_counts(control_interval_counts); } } + +impl MecanumGenerationTransformer for IntervalCountSetter { + fn initialize(context: &GenerationContext) -> FeatureLockedTransformer { + FeatureLockedTransformer::always(Self { + counts: guess_control_interval_counts( + &context.project.config.snapshot(), + &context.params, + ) + .unwrap_or_default(), + waypoints: context.params.waypoints.clone(), + }) + } + + fn transform(&self, generator: &mut trajoptlib::MecanumTrajectoryGenerator) { + let waypoints = &self.waypoints; + let mut guess_points_after_waypoint = Vec::new(); + let mut control_interval_counts = Vec::new(); + let mut wpt_cnt = 0; + for i in 0..waypoints.len() { + let wpt = &waypoints[i]; + // add initial guess points (actually unconstrained empty wpts in Choreo terms) + if wpt.is_initial_guess && !wpt.fix_heading && !wpt.fix_translation { + let guess_point = Pose2d { + x: wpt.x, + y: wpt.y, + heading: wpt.heading, + }; + guess_points_after_waypoint.push(guess_point); + if let Some(last) = control_interval_counts.last_mut() { + *last += self.counts[i]; + } + } else { + if wpt_cnt > 0 { + generator.sgmt_initial_guess_points(wpt_cnt - 1, &guess_points_after_waypoint); + } + guess_points_after_waypoint.clear(); + if wpt.fix_heading && wpt.fix_translation { + generator.pose_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading); + } else if wpt.fix_translation { + generator.translation_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading); + } else { + generator.empty_wpt(wpt_cnt, wpt.x, wpt.y, wpt.heading); + } + wpt_cnt += 1; + if i != waypoints.len() - 1 { + control_interval_counts.push(self.counts[i]); + } + } + } + + generator.set_control_interval_counts(control_interval_counts); + } +} diff --git a/src-core/src/generation/transformers/mod.rs b/src-core/src/generation/transformers/mod.rs index 6fb0b7863a..c88c1999ad 100644 --- a/src-core/src/generation/transformers/mod.rs +++ b/src-core/src/generation/transformers/mod.rs @@ -3,8 +3,8 @@ use std::collections::{HashMap, HashSet}; use trajoptlib::{ - DifferentialTrajectory, DifferentialTrajectoryGenerator, SwerveTrajectory, - SwerveTrajectoryGenerator, + DifferentialTrajectory, DifferentialTrajectoryGenerator, MecanumTrajectory, + MecanumTrajectoryGenerator, SwerveTrajectory, SwerveTrajectoryGenerator, }; use crate::{ @@ -101,6 +101,7 @@ pub(super) struct TrajectoryFileGenerator { swerve_transformers: HashMap>>, differential_transformers: HashMap>>, + mecanum_transformers: HashMap>>, } impl TrajectoryFileGenerator { @@ -130,7 +131,8 @@ impl TrajectoryFileGenerator { original_file: trajectory_file, swerve_transformers: HashMap::new(), differential_transformers: HashMap::new(), - }) + mecanum_transformers: HashMap::new(), + } } /// Add a transformer to the generator that is only applied when generating @@ -157,15 +159,29 @@ impl TrajectoryFileGenerator { .push(transformer); } - /// Add a transformer to the generator that is applied when generating both - /// swerve and differential trajectories + /// Add a transformer to the generator that is only applied when generating a mecanum trajectory + pub fn add_mecanum_transformer(&mut self) { + let featurelocked_transformer = T::initialize(&self.ctx); + let feature = featurelocked_transformer.feature; + let transformer = Box::new(featurelocked_transformer.inner); + self.mecanum_transformers + .entry(feature) + .or_default() + .push(transformer); + } + + /// Add a transformer to the generator that is applied when generating both swerve and differential trajectories pub fn add_omni_transformer< - T: SwerveGenerationTransformer + DifferentialGenerationTransformer + 'static, + T: SwerveGenerationTransformer + + DifferentialGenerationTransformer + + MecanumGenerationTransformer + + 'static, >( &mut self, ) { self.add_swerve_transformer::(); self.add_differential_transformer::(); + self.add_mecanum_transformer::(); } fn generate_swerve(&self, handle: i64) -> ChoreoResult { @@ -202,84 +218,23 @@ impl TrajectoryFileGenerator { generator.generate(true, handle).map_err(Into::into) } - fn postprocess(&self, result: &[Sample]) -> TrajectoryFile { - let mut original_params = self.original_file.params.clone(); - // Update the interval counts on each params waypoint. - original_params - .waypoints - .iter_mut() - .zip(&self.ctx.counts_vec) - .for_each(|(waypoint, counts)| { - if !waypoint.override_intervals { - waypoint.intervals = *counts; + fn generate_mecanum(&self, handle: i64) -> ChoreoResult { + let mut generator = MecanumTrajectoryGenerator::new(); + let mut feature_set = HashSet::new(); + feature_set.extend(self.ctx.project.generation_features.clone()); + feature_set.insert("".to_string()); + + for feature in feature_set.iter() { + if let Some(transformers) = self.mecanum_transformers.get(feature) { + for transformer in transformers.iter() { + transformer.trans(&mut generator); } - }); - // Snapshot, capturing that interval count update - let snapshot = original_params.snapshot(); - let mut original_events = self.original_file.events.clone(); - - // Calculate the waypoint timing (a vec of the timestamps of each - // waypoint) starting value of 0, plus 0 (intervals before the - // first waypoint) = 0 (index of the first waypoint) - let mut interval = 0; - // `intervals` contains ( - // was the waypoint either a non-ending split point or the start - // point (i.e, was it the beginning of a split segment) - // the total number of intervals before this waypoint (not including - // the one the waypoint constrains), The timestamp of the - // sample indexed by the previous parameter ) - let intervals = snapshot - .waypoints - .iter() - .enumerate() - .map(|pt| { - let total_intervals = interval; - interval += pt.1.intervals; - ( - pt.0 == 0 || (pt.1.split && pt.0 != snapshot.waypoints.len() - 1), - total_intervals, - result.get(total_intervals).map_or(0.0, |s| match s { - Sample::Swerve { t, .. } => *t, - Sample::DifferentialDrive { t, .. } => *t, - }), - ) - }) - .collect::>(); - - let waypoint_times = intervals.iter().map(|a| a.2).collect::>(); - // Calculate splits - let splits = intervals - .iter() - .filter(|a| a.0) // filter by "start of split" flag - .map(|a| a.1) // map to associate an index in the samples array - .collect::>(); - - // update event markers' target timestamps with the corresponding - // timestamp from waypoint_times or None if the targeted index - // is None or out of bounds - original_events.iter_mut().for_each(|marker| { - marker.from.target_timestamp = marker.from.target.and_then(|idx| { - waypoint_times - .get(idx) - .copied() - .or(marker.from.target_timestamp) - }); - }); - TrajectoryFile { - name: self.original_file.name.clone(), - version: self.original_file.version, - snapshot: Some(snapshot), - params: original_params, - trajectory: Trajectory { - sample_type: Some(self.ctx.project.r#type), - waypoints: waypoint_times, - samples: result.to_vec(), - splits, - config: Some(self.ctx.project.config.snapshot()), - }, - events: original_events, + } } + + generator.generate(true, handle).map_err(Into::into) } + /// Generate the trajectory file pub fn generate(self) -> ChoreoResult { let samples: Vec = match &self.ctx.project.r#type { @@ -295,6 +250,12 @@ impl TrajectoryFileGenerator { .into_iter() .map(Into::into) .collect(), + DriveType::Mecanum => self + .generate_mecanum(self.ctx.handle)? + .samples + .into_iter() + .map(Into::into) + .collect(), }; Ok(self.postprocess(&samples)) @@ -359,3 +320,92 @@ impl InitializedDifferentialGenerationTran self.transform(generator); } } + +/// An object safe variant of the [`MecanumGenerationTransformer`] trait, +/// +/// Should not be implemented directly, instead implement [`MecanumGenerationTransformer`] +pub(super) trait InitializedMecanumGenerationTransformer { + fn trans(&self, generator: &mut MecanumTrajectoryGenerator); +} + +/// A trait for objects that can transform a [`MecanumTrajectoryGenerator`] +pub(super) trait MecanumGenerationTransformer: + InitializedMecanumGenerationTransformer + Sized +{ + fn initialize(context: &GenerationContext) -> FeatureLockedTransformer; + fn transform(&self, generator: &mut MecanumTrajectoryGenerator); +} + +impl InitializedMecanumGenerationTransformer for T { + fn trans(&self, generator: &mut MecanumTrajectoryGenerator) { + self.transform(generator); + } +} + +fn postprocess( + result: &[Sample], + mut path: TrajectoryFile, + project: ProjectFile, + counts_vec: Vec, +) -> TrajectoryFile { + let mut snapshot = path.params.snapshot(); + // Update the `intervals` field of each waypoint with the corresponding entry from `counts_vec` + path.params + .waypoints + .iter_mut() + .zip(snapshot.waypoints.iter_mut()) + .zip(counts_vec) + .for_each(|w| { + w.0.0.intervals = w.1; + w.0.1.intervals = w.1; + }); + // Calculate the waypoint timing (a vec of the timestamps of each waypoint) + // starting value of 0, plus 0 (intervals before the first waypoint) = 0 (index of the first waypoint) + let mut interval = 0; + // `intervals` contains ( + // was the waypoint either a non-ending split point or the start point (i.e, was it the beginning of a split segment) + // the total number of intervals before this waypoint (not including the one the waypoint constrains), + // The timestamp of the sample indexed by the previous parameter + // ) + let intervals = snapshot + .waypoints + .iter() + .enumerate() + .map(|pt| { + let total_intervals = interval; + interval += pt.1.intervals; + ( + pt.0 == 0 || (pt.1.split && pt.0 != snapshot.waypoints.len() - 1), + total_intervals, + result.get(total_intervals).map_or(0.0, |s| match s { + Sample::Swerve { t, .. } => *t, + Sample::DifferentialDrive { t, .. } => *t, + Sample::Mecanum { t, .. } => *t, + }), + ) + }) + .collect::>(); + + let waypoint_times = intervals.iter().map(|a| a.2).collect::>(); + // Calculate splits + let splits = intervals + .iter() + .filter(|a| a.0) // filter by "start of split" flag + .map(|a| a.1) // map to associate an index in the samples array + .collect::>(); + // copy the above into the TrajectoryFile + path.trajectory.sample_type = Some(project.r#type); + path.trajectory.splits = splits; + path.trajectory.samples = result.to_vec(); + path.trajectory.waypoints = waypoint_times; + path.snapshot = Some(snapshot); + // update event markers' target timestamps with the corresponding timestamp from trajectory.waypoints + // or None if the targeted index is None or out of bounds + path.events.iter_mut().for_each(|marker| { + marker.from.target_timestamp = marker + .from + .target + .and_then(|idx| path.trajectory.waypoints.get(idx).copied()); + }); + path +} diff --git a/src-core/src/spec/trajectory.rs b/src-core/src/spec/trajectory.rs index 5315680c19..3a1d6104f1 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -1,5 +1,5 @@ use serde::{Deserialize, Serialize}; -use trajoptlib::{DifferentialTrajectorySample, SwerveTrajectorySample}; +use trajoptlib::{DifferentialTrajectorySample, MecanumTrajectorySample, SwerveTrajectorySample}; use crate::spec::project::RobotConfig; @@ -292,6 +292,20 @@ pub enum Sample { fl: f64, fr: f64, }, + /// A sample for a mecanum drive. + Mecanum { + t: f64, + x: f64, + y: f64, + heading: f64, + vx: f64, + vy: f64, + omega: f64, + ax: f64, + ay: f64, + alpha: f64, + f: [f64; 4], + }, } impl Sample { pub fn t(&self) -> f64 { @@ -386,6 +400,34 @@ impl From for Sample { } } +impl From<&MecanumTrajectorySample> for Sample { + fn from(mecanum_sample: &MecanumTrajectorySample) -> Self { + Sample::Mecanum { + t: round(mecanum_sample.timestamp), + x: round(mecanum_sample.x), + y: round(mecanum_sample.y), + vx: round(mecanum_sample.velocity_x), + vy: round(mecanum_sample.velocity_y), + heading: round(mecanum_sample.heading), + omega: round(mecanum_sample.angular_velocity), + ax: round(mecanum_sample.acceleration_x), + ay: round(mecanum_sample.acceleration_y), + alpha: round(mecanum_sample.angular_acceleration), + f: [ + round(mecanum_sample.wheel_forces[0]), + round(mecanum_sample.wheel_forces[1]), + round(mecanum_sample.wheel_forces[2]), + round(mecanum_sample.wheel_forces[3]), + ], + } + } +} +impl From for Sample { + fn from(value: MecanumTrajectorySample) -> Self { + Self::from(&value) + } +} + /// The type of samples in a trajectory. #[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)] pub enum DriveType { @@ -394,6 +436,8 @@ pub enum DriveType { Swerve, /// The variant for [`Sample::DifferentialDrive`]. Differential, + /// The variant for [`Sample::Mecanum`]. + Mecanum, } /// The parameters used for generating a trajectory. diff --git a/src/components/config/FieldSettingsPanel.tsx b/src/components/config/FieldSettingsPanel.tsx new file mode 100644 index 0000000000..975379a81c --- /dev/null +++ b/src/components/config/FieldSettingsPanel.tsx @@ -0,0 +1,567 @@ +import { open } from "@tauri-apps/plugin-dialog"; +import { + Box, + Button, + Card, + CardContent, + Chip, + FormControl, + InputLabel, + MenuItem, + Select, + Stack, + TextField, + Typography, + Paper +} from "@mui/material"; +import type { SelectChangeEvent } from "@mui/material/Select"; +import { observer } from "mobx-react"; +import { useCallback } from "react"; +import { convertFileSrc } from "@tauri-apps/api/core"; +import { + CUSTOM_FIELD_ID, + FIELD_PRESETS, + FieldPresetId, + getPresetById +} from "../../document/FieldSettingsStore"; +import { uiState } from "../../document/DocumentManager"; +import FieldImage2025 from "../field/svg/fields/FieldImage2025"; +import FieldImageDecode from "../field/svg/fields/FieldImageDecode"; + +const BUILTIN_FIELD_COMPONENTS: Record = { + reefscape2025: FieldImage2025, + decode: FieldImageDecode +}; + +const FieldPreview = observer(() => { + const fieldSettings = uiState.fieldSettings; + const selectedPresetId = fieldSettings.selectedPresetId; + const preset = getPresetById(selectedPresetId); + const fieldLength = Math.max(fieldSettings.fieldLength, 0.001); + const fieldWidth = Math.max(fieldSettings.fieldWidth, 0.001); + const spacing = Math.max(fieldSettings.previewGridSpacing, 0.01); + const aspectRatio = fieldLength / fieldWidth; + const verticalCount = Math.floor(fieldLength / spacing); + const horizontalCount = Math.floor(fieldWidth / spacing); + const verticalPositions = Array.from( + { length: Math.max(verticalCount - 1, 0) }, + (_, idx) => (idx + 1) * spacing + ); + const horizontalPositions = Array.from( + { length: Math.max(horizontalCount - 1, 0) }, + (_, idx) => (idx + 1) * spacing + ); + const FieldComponent = preset + ? BUILTIN_FIELD_COMPONENTS[preset.id as FieldPresetId] + : undefined; + const showCustomImage = + selectedPresetId === CUSTOM_FIELD_ID && + fieldSettings.customImagePath !== undefined; + const imageHref = showCustomImage + ? convertFileSrc(fieldSettings.customImagePath as string) + : undefined; + + return ( + + + + + Live Preview + + + Real-time rendering with meter grid overlay + + + + + + + {imageHref && ( + + )} + {!imageHref && FieldComponent && preset && ( + + + + )} + {/* Grid */} + {verticalPositions.map((x) => ( + + ))} + {horizontalPositions.map((y) => ( + + ))} + + + + + + + + + + + ); +}); + +const FieldSettingsPanel = () => { + const fieldSettings = uiState.fieldSettings; + const selectedPresetId = fieldSettings.selectedPresetId; + const selectedPreset = getPresetById(selectedPresetId ?? undefined); + + const handlePresetChange = useCallback((event: SelectChangeEvent) => { + uiState.selectFieldPreset(event.target.value as string); + }, []); + + const handleLengthChange = useCallback( + (event: React.ChangeEvent) => { + const next = parseFloat(event.target.value); + if (!Number.isNaN(next) && next > 0) { + uiState.setFieldDimensions(next, fieldSettings.fieldWidth); + } + }, + [fieldSettings.fieldWidth] + ); + + const handleWidthChange = useCallback( + (event: React.ChangeEvent) => { + const next = parseFloat(event.target.value); + if (!Number.isNaN(next) && next > 0) { + uiState.setFieldDimensions(fieldSettings.fieldLength, next); + } + }, + [fieldSettings.fieldLength] + ); + + const handleGridSpacingChange = useCallback( + (event: React.ChangeEvent) => { + const next = parseFloat(event.target.value); + if (!Number.isNaN(next) && next > 0) { + uiState.setPreviewGridSpacing(next); + } + }, + [] + ); + + const handleChooseCustomImage = useCallback(async () => { + const result = await open({ + multiple: false, + directory: false, + filters: [ + { + name: "Images", + extensions: ["png", "jpg", "jpeg", "svg", "bmp", "gif", "webp"] + } + ] + }); + const selectedPath = Array.isArray(result) ? result[0] : result; + if (typeof selectedPath === "string" && selectedPath.length > 0) { + uiState.selectFieldPreset(CUSTOM_FIELD_ID); + uiState.setCustomFieldImage(selectedPath); + } + }, []); + + const handleClearCustomImage = useCallback(() => { + uiState.setCustomFieldImage(undefined); + }, []); + + const hasCustomImage = + selectedPresetId === CUSTOM_FIELD_ID && fieldSettings.customImagePath; + + return ( + + + + + + + Field Configuration + + + Configure your field image and dimensions + + + + + + Image Source + + + + Select field image + + + + {selectedPreset && selectedPresetId !== CUSTOM_FIELD_ID && ( + + Default: {selectedPreset.defaultLength.toFixed(3)} m ×{" "} + {selectedPreset.defaultWidth.toFixed(3)} m + + )} + + + + + Dimensions (meters) + + + + + + + + + + + + + Preview Grid + + + + + {selectedPresetId === CUSTOM_FIELD_ID && ( + + + Custom Image + + + + + {hasCustomImage && ( + + )} + + + + {hasCustomImage + ? fieldSettings.customImagePath + : "No custom image selected"} + + + + + )} + + + + Field dimensions affect coordinate scaling, constraints, and + overlays throughout the application. + + + + + + + + + ); +}; + +export default observer(FieldSettingsPanel); diff --git a/src/components/config/robotconfig/MecanumConfigPanel.tsx b/src/components/config/robotconfig/MecanumConfigPanel.tsx new file mode 100644 index 0000000000..eeacb5da2e --- /dev/null +++ b/src/components/config/robotconfig/MecanumConfigPanel.tsx @@ -0,0 +1,54 @@ +import { observer } from "mobx-react"; +import { Component } from "react"; +import { doc } from "../../../document/DocumentManager"; +import ExpressionInput from "../../input/ExpressionInput"; +import ExpressionInputList from "../../input/ExpressionInputList"; + +type Props = { rowGap: number }; + +type State = object; + +class MecanumConfigPanel extends Component { + render() { + const config = doc.robotConfig; + return ( + + + + + + + + + ); + } +} +export default observer(MecanumConfigPanel); diff --git a/src/components/config/robotconfig/RobotConfigPanel.tsx b/src/components/config/robotconfig/RobotConfigPanel.tsx index 5f31e860d2..19787dc3de 100644 --- a/src/components/config/robotconfig/RobotConfigPanel.tsx +++ b/src/components/config/robotconfig/RobotConfigPanel.tsx @@ -1,13 +1,18 @@ -import { Divider, FormHelperText, Switch } from "@mui/material"; +import { + Autocomplete, + Divider, + FormHelperText, + TextField +} from "@mui/material"; import { observer } from "mobx-react"; import { Component } from "react"; -import inputStyles from "../../input/InputList.module.css"; import DimensionsConfigPanel from "./DimensionsConfigPanel"; import ModuleConfigPanel from "./ModuleConfigPanel"; import TheoreticalPanel from "./TheoreticalPanel"; import { doc } from "../../../document/DocumentManager"; import DifferentialConfigPanel from "./DifferentialConfigPanel"; import SwerveConfigPanel from "./SwerveConfigPanel"; +import MecanumConfigPanel from "./MecanumConfigPanel"; type Props = object; @@ -50,44 +55,33 @@ class RobotConfigPanel extends Component { gridRow: 1 }} > - - DRIVE TYPE - + DRIVE TYPE
- - Swerve - - - - doc.setType(checked ? "Differential" : "Swerve") + value={doc.type} + onChange={(_e, new_value) => + doc.setType(new_value as "Differential" | "Mecanum" | "Swerve") } - > - - Differential - + options={["Swerve", "Mecanum", "Differential"]} + disableClearable={true} + renderInput={(params) => } + >
{doc.type === "Differential" ? ( - ) : ( + ) : doc.type === "Swerve" ? ( + ) : ( + )} {/* Theoreticals */} diff --git a/src/components/field/svg/FieldOverlayRoot.tsx b/src/components/field/svg/FieldOverlayRoot.tsx index feba765b0e..4f5ff6c792 100644 --- a/src/components/field/svg/FieldOverlayRoot.tsx +++ b/src/components/field/svg/FieldOverlayRoot.tsx @@ -5,6 +5,7 @@ import { ToggleButtonGroup, Tooltip } from "@mui/material"; +import { convertFileSrc } from "@tauri-apps/api/core"; import * as d3 from "d3"; import { observer } from "mobx-react"; import React, { Component } from "react"; @@ -205,11 +206,51 @@ class FieldOverlayRoot extends Component { }); } render() { - this.canvasHeightMeters = FieldDimensions.FIELD_WIDTH + 1; - this.canvasWidthMeters = FieldDimensions.FIELD_LENGTH + 1; + const fieldSettings = uiState.fieldSettings; + const fieldLength = fieldSettings.fieldLength; + const fieldWidth = fieldSettings.fieldWidth; + this.canvasHeightMeters = fieldWidth + 1; + this.canvasWidthMeters = fieldLength + 1; const layers = uiState.layers; const constraintSelected = uiState.isConstraintSelected(); const eventMarkerSelected = uiState.isEventMarkerSelected(); + const selectedPresetId = fieldSettings.selectedPresetId; + const preset = getPresetById(selectedPresetId); + const defaultBackground = ( + + + + ); + let fieldBackground: React.ReactNode = defaultBackground; + if (selectedPresetId === CUSTOM_FIELD_ID) { + if (fieldSettings.customImagePath) { + const imageSrc = convertFileSrc(fieldSettings.customImagePath); + fieldBackground = ( + + + + ); + } + } else if (preset) { + const component = BUILTIN_FIELD_COMPONENTS[preset.id as FieldPresetId]; + if (component) { + const scaleX = + preset.defaultLength > 0 ? fieldLength / preset.defaultLength : 1; + const scaleY = + preset.defaultWidth > 0 ? fieldWidth / preset.defaultWidth : 1; + const FieldComponent = component; + fieldBackground = ( + + + + ); + } + } return ( { + WIDTH_M = 3.6576; + LENGTH_M = 3.6576; + state = {}; + + render() { + return ( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ); + } +} diff --git a/src/document/DocumentManager.ts b/src/document/DocumentManager.ts index 9cf77539d8..f4de9111f8 100644 --- a/src/document/DocumentManager.ts +++ b/src/document/DocumentManager.ts @@ -64,6 +64,7 @@ import { SavingState, UIStateStore } from "./UIStateStore"; import { findUUIDIndex } from "./path/utils"; import { ChoreoError, Commands } from "./tauriCommands"; import { tracing } from "./tauriTracing"; +import { DEFAULT_FIELD_SETTINGS } from "./FieldSettingsStore"; const TRAJ_DATA_FILENAME = "ChoreoTraj"; const VARS_FILENAME = "ChoreoVars"; @@ -77,8 +78,10 @@ export const uiState = UIStateStore.create({ settingsTab: 0, projectSavingState: SavingState.NO_LOCATION, projectSaveTime: new Date(), - layers: ViewLayerDefaults + layers: ViewLayerDefaults, + fieldSettings: DEFAULT_FIELD_SETTINGS }); +uiState.loadFieldSettingsFromLocalStorage(); type ConstraintDataConstructor = ( data: Partial ) => IConstraintDataStore; @@ -239,6 +242,7 @@ const env = { }, history: () => doc.history, vars: () => doc.variables, + fieldSettings: () => uiState.fieldSettings, renameVariable: renameVariable, exporter: async (uuid: string) => writeTrajectory(uuid).catch(tracing.error), create: getConstructors(() => doc.variables) diff --git a/src/document/DocumentModel.tsx b/src/document/DocumentModel.tsx index a59be6e1d4..a9b4ba41f1 100644 --- a/src/document/DocumentModel.tsx +++ b/src/document/DocumentModel.tsx @@ -4,6 +4,7 @@ import { UndoManager } from "mst-middlewares"; import { toast } from "react-toastify"; import { DifferentialSample, + MecanumSample, ProgressUpdate, Project, PROJECT_SCHEMA_VERSION, @@ -62,7 +63,8 @@ function itemType( } export const ISampleType = types.enumeration([ "Swerve", - "Differential" + "Differential", + "Mecanum" ]); // When adding new fields, consult @@ -254,6 +256,52 @@ export const DocumentStore = types ); } }) + await Commands.guessIntervals(config, pathStore.serialize) + .catch((e) => { + tracing.error("guessIntervals:", e); + throw e; + }) + .then((counts) => { + tracing.debug(counts); + counts.forEach((count, i) => { + const waypoint = pathStore.params.waypoints[i]; + if (waypoint.overrideIntervals && count !== waypoint.intervals) { + console.assert( + false, + "Control interval guessing did not ignore override intervals! %o", + { + path: pathStore.name, + waypoint: i, + override: waypoint.intervals, + calculated: count + } + ); + } else { + waypoint.setIntervals(count); + } + }); + }) + .then(() => { + tracing.debug("generatePathPre"); + return listen(`solver-status-${handle}`, async (rawEvent) => { + const event: Event = + rawEvent as Event; + if ( + event.payload!.type === "swerveTrajectory" || + event.payload!.type === "differentialTrajectory" || + event.payload!.type === "mecanumTrajectory" + ) { + const samples = event.payload.update as + | SwerveSample[] + | DifferentialSample[] + | MecanumSample[]; + pathStore.ui.setInProgressTrajectory(samples); + pathStore.ui.setIterationNumber( + pathStore.ui.generationIterationNumber + 1 + ); + } + }); + }) .then((unlistener) => { unlisten = unlistener; return Commands.generate( diff --git a/src/document/FieldSettingsStore.ts b/src/document/FieldSettingsStore.ts new file mode 100644 index 0000000000..917d84d33d --- /dev/null +++ b/src/document/FieldSettingsStore.ts @@ -0,0 +1,110 @@ +import { SnapshotIn, types } from "mobx-state-tree"; + +export const FIELD_PRESETS = [ + { + id: "reefscape2025", + label: "Reefscape 2025", + defaultLength: 17.548, + defaultWidth: 8.052 + }, + { + id: "decode", + label: "DECODE Practice Field", + defaultLength: 3.6576, + defaultWidth: 3.6576 + } +] as const; + +export type FieldPresetId = (typeof FIELD_PRESETS)[number]["id"]; +export const CUSTOM_FIELD_ID = "custom" as const; + +export const DEFAULT_FIELD_SETTINGS = { + selectedPresetId: FIELD_PRESETS[0].id, + customImagePath: undefined, + fieldLength: FIELD_PRESETS[0].defaultLength, + fieldWidth: FIELD_PRESETS[0].defaultWidth, + previewGridSpacing: 1 +}; + +export type SerializedFieldSettings = { + selectedPresetId: string; + customImagePath?: string; + fieldLength: number; + fieldWidth: number; + previewGridSpacing: number; +}; +export const getPresetById = (id: string | undefined) => + FIELD_PRESETS.find((preset) => preset.id === id); + +export const FieldSettingsStore = types + .model("FieldSettingsStore", { + selectedPresetId: types.string, + customImagePath: types.maybe(types.string), + fieldLength: types.number, + fieldWidth: types.number, + previewGridSpacing: types.number + }) + .views((self) => ({ + get isCustom() { + return self.selectedPresetId === CUSTOM_FIELD_ID; + }, + get dimensions() { + return { length: self.fieldLength, width: self.fieldWidth }; + }, + get serialize() { + return { + selectedPresetId: self.selectedPresetId, + customImagePath: self.customImagePath ?? undefined, + fieldLength: self.fieldLength, + fieldWidth: self.fieldWidth, + previewGridSpacing: self.previewGridSpacing + }; + } + })) + .actions((self) => ({ + selectPreset(presetId: string) { + self.selectedPresetId = presetId; + const preset = FIELD_PRESETS.find((p) => p.id === presetId); + if (preset) { + self.fieldLength = preset.defaultLength; + self.fieldWidth = preset.defaultWidth; + self.customImagePath = undefined; + } + }, + setDimensions(length: number, width: number) { + self.fieldLength = length; + self.fieldWidth = width; + }, + setPreviewGridSpacing(spacing: number) { + self.previewGridSpacing = spacing; + }, + resetToPresetDefaults() { + const preset = FIELD_PRESETS.find((p) => p.id === self.selectedPresetId); + if (preset) { + self.fieldLength = preset.defaultLength; + self.fieldWidth = preset.defaultWidth; + } + }, + setCustomImagePath(path: string | undefined) { + self.customImagePath = path; + }, + hydrate(snapshot: Partial) { + if (snapshot.selectedPresetId) { + self.selectedPresetId = snapshot.selectedPresetId; + } + if (snapshot.fieldLength !== undefined) { + self.fieldLength = snapshot.fieldLength; + } + if (snapshot.fieldWidth !== undefined) { + self.fieldWidth = snapshot.fieldWidth; + } + if (snapshot.previewGridSpacing !== undefined) { + self.previewGridSpacing = snapshot.previewGridSpacing; + } + if (snapshot.customImagePath !== undefined) { + self.customImagePath = snapshot.customImagePath ?? undefined; + } + } + })); + +export type FieldSettingsSnapshot = SnapshotIn; diff --git a/src/document/PathListStore.ts b/src/document/PathListStore.ts index 49b3f39ac6..583bf3c653 100644 --- a/src/document/PathListStore.ts +++ b/src/document/PathListStore.ts @@ -5,7 +5,6 @@ import { HolonomicPathStore, IHolonomicPathStore } from "./path/HolonomicPathStore"; -import * as FieldDimensions from "../components/field/svg/fields/FieldDimensions"; import { SavingState } from "./UIStateStore"; import { addErrorMessages, @@ -142,6 +141,9 @@ export const PathListStore = types if (contents !== undefined) { path.deserialize(contents); } else { + const fieldSettings = env.fieldSettings(); + const fieldLength = fieldSettings.fieldLength; + const fieldWidth = fieldSettings.fieldWidth; path.setName(usedName); path.params.addConstraint("StopPoint", true, "first"); path.params.addConstraint("StopPoint", true, "last"); @@ -154,12 +156,12 @@ export const PathListStore = types x: { exp: "0 m", val: 0.0 }, y: { exp: "0 m", val: 0.0 }, w: { - exp: `${FieldDimensions.FIELD_LENGTH} m`, - val: FieldDimensions.FIELD_LENGTH + exp: `${fieldLength} m`, + val: fieldLength }, h: { - exp: `${FieldDimensions.FIELD_WIDTH} m`, - val: FieldDimensions.FIELD_WIDTH + exp: `${fieldWidth} m`, + val: fieldWidth } } ); diff --git a/src/document/UIData.tsx b/src/document/UIData.tsx index c933904d0a..efe2ebe5c9 100644 --- a/src/document/UIData.tsx +++ b/src/document/UIData.tsx @@ -12,6 +12,7 @@ import { import { ReactElement } from "react"; import Waypoint from "../assets/Waypoint"; import KeyboardShortcutsPanel from "../components/config/KeyboardShortcutsPanel"; +import FieldSettingsPanel from "../components/config/FieldSettingsPanel"; import RobotConfigPanel from "../components/config/robotconfig/RobotConfigPanel"; import { ConstraintDefinition, @@ -189,6 +190,8 @@ export const SETTINGS_TABS = [ { name: "Code Generation", component: () => + name: "Field", + component: () => } ] as const; export const NUM_SETTINGS_TABS = SETTINGS_TABS.length; diff --git a/src/document/UIStateStore.tsx b/src/document/UIStateStore.tsx index 19ada92b31..bf5be4b240 100644 --- a/src/document/UIStateStore.tsx +++ b/src/document/UIStateStore.tsx @@ -7,6 +7,13 @@ import { } from "../components/config/robotconfig/PathGradient"; import LocalStorageKeys from "../util/LocalStorageKeys"; import { ConstraintKey } from "./ConstraintDefinitions"; +import { + CUSTOM_FIELD_ID, + DEFAULT_FIELD_SETTINGS, + FieldSettingsStore, + SerializedFieldSettings, + getPresetById +} from "./FieldSettingsStore"; import { NUM_SETTINGS_TABS, NavbarData, @@ -58,6 +65,7 @@ export const UIStateStore = types types.array(types.boolean), (arr) => arr?.length == ViewItemData.length ), + fieldSettings: FieldSettingsStore, selectedNavbarItem: NavbarLabels.FullWaypoint, selectedPathGradient: types.maybe( types.union( @@ -200,6 +208,81 @@ export const UIStateStore = types localStorage.getItem(LocalStorageKeys.PATH_GRADIENT) ?? PathGradients.LinearVelocity.name; }, + selectFieldPreset(presetId: string) { + if ( + presetId === CUSTOM_FIELD_ID || + getPresetById(presetId) !== undefined + ) { + self.fieldSettings.selectPreset(presetId); + this._saveFieldSettingsToLocalStorage(); + } + }, + setFieldDimensions(length: number, width: number) { + if ( + Number.isFinite(length) && + Number.isFinite(width) && + length > 0 && + width > 0 + ) { + self.fieldSettings.setDimensions(length, width); + this._saveFieldSettingsToLocalStorage(); + } + }, + setPreviewGridSpacing(spacing: number) { + if (Number.isFinite(spacing) && spacing > 0) { + self.fieldSettings.setPreviewGridSpacing(spacing); + this._saveFieldSettingsToLocalStorage(); + } + }, + setCustomFieldImage(path: string | undefined) { + self.fieldSettings.setCustomImagePath(path); + this._saveFieldSettingsToLocalStorage(); + }, + resetFieldDimensionsToDefault() { + self.fieldSettings.resetToPresetDefaults(); + this._saveFieldSettingsToLocalStorage(); + }, + loadFieldSettingsFromLocalStorage() { + const stored = localStorage.getItem(LocalStorageKeys.FIELD_SETTINGS); + if (!stored) { + self.fieldSettings.hydrate(DEFAULT_FIELD_SETTINGS); + return; + } + try { + const parsed = JSON.parse(stored) as SerializedFieldSettings; + const targetPreset = + parsed.selectedPresetId === CUSTOM_FIELD_ID || + getPresetById(parsed.selectedPresetId) + ? parsed.selectedPresetId + : DEFAULT_FIELD_SETTINGS.selectedPresetId; + self.fieldSettings.selectPreset(targetPreset); + if (parsed.fieldLength && parsed.fieldWidth) { + self.fieldSettings.setDimensions( + parsed.fieldLength, + parsed.fieldWidth + ); + } + if ( + parsed.previewGridSpacing !== undefined && + parsed.previewGridSpacing > 0 + ) { + self.fieldSettings.setPreviewGridSpacing(parsed.previewGridSpacing); + } + if (targetPreset === CUSTOM_FIELD_ID) { + self.fieldSettings.setCustomImagePath(parsed.customImagePath); + } + } catch (err) { + console.error("Failed to load field settings", err); + self.fieldSettings.hydrate(DEFAULT_FIELD_SETTINGS); + } + }, + _saveFieldSettingsToLocalStorage() { + const serialized = self.fieldSettings.serialize; + localStorage.setItem( + LocalStorageKeys.FIELD_SETTINGS, + JSON.stringify(serialized) + ); + }, setContextMenuSelectedWaypoint(waypointIndex: number | undefined) { self.contextMenuSelectedWaypoint = waypointIndex; }, diff --git a/src/document/schema/DocumentTypes.ts b/src/document/schema/DocumentTypes.ts index ae6cd47484..08decffb01 100644 --- a/src/document/schema/DocumentTypes.ts +++ b/src/document/schema/DocumentTypes.ts @@ -126,6 +126,20 @@ export interface DifferentialSample { fr: number; } +export interface MecanumSample { + t: number; + x: number; + y: number; + heading: number; + vx: number; + vy: number; + omega: number; + ax: number; + ay: number; + alpha: number; + f: [number, number, number, number]; +} + export interface ProgressUpdate { type: | "swerveTrajectory" @@ -141,7 +155,7 @@ export interface ChoreoPath { targetDt: T; } -export type SampleType = "Swerve" | "Differential"; +export type SampleType = "Swerve" | "Differential" | "Mecanum"; export interface Output { config: RobotConfig | null; sampleType: SampleType | undefined; diff --git a/src/util/LocalStorageKeys.ts b/src/util/LocalStorageKeys.ts index 05d97f8c42..ac48709090 100644 --- a/src/util/LocalStorageKeys.ts +++ b/src/util/LocalStorageKeys.ts @@ -1,6 +1,7 @@ enum LocalStorageKeys { LAST_OPENED_FILE_LOCATION = "last_opened_project_payload", - PATH_GRADIENT = "path_gradient" + PATH_GRADIENT = "path_gradient", + FIELD_SETTINGS = "field_settings" } export default LocalStorageKeys; diff --git a/trajoptlib/include/trajopt/differential_trajectory_generator.hpp b/trajoptlib/include/trajopt/differential_trajectory_generator.hpp index fbaf25cd60..6cb2251c7f 100644 --- a/trajoptlib/include/trajopt/differential_trajectory_generator.hpp +++ b/trajoptlib/include/trajopt/differential_trajectory_generator.hpp @@ -191,14 +191,14 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectory { /// A differential drive path. using DifferentialPath = Path; -/// Builds a differential drive path using information about how the robot -/// must travel through a series of waypoints. This path can be converted -/// to a trajectory using DifferentialTrajectoryGenerator. +/// Builds a differential drive path using information about how the robot must +/// travel through a series of waypoints. This path can be converted to a +/// trajectory using DifferentialTrajectoryGenerator. using DifferentialPathBuilder = PathBuilder; -/// This trajectory generator class contains functions to generate -/// time-optimal trajectories for differential drivetrain types. +/// This trajectory generator class contains functions to generate time-optimal +/// trajectories for differential drivetrain types. class TRAJOPT_DLLEXPORT DifferentialTrajectoryGenerator { public: /// Construct a new swerve trajectory optimization problem. diff --git a/trajoptlib/include/trajopt/mecanum_trajectory_generator.hpp b/trajoptlib/include/trajopt/mecanum_trajectory_generator.hpp new file mode 100644 index 0000000000..488ccb9d6c --- /dev/null +++ b/trajoptlib/include/trajopt/mecanum_trajectory_generator.hpp @@ -0,0 +1,247 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "trajopt/geometry/translation2.hpp" +#include "trajopt/path/path_builder.hpp" +#include "trajopt/util/symbol_exports.hpp" + +namespace trajopt { + +/// A mecanum drivetrain physical model. +struct TRAJOPT_DLLEXPORT MecanumDrivetrain { + /// The mass of the robot (kg). + double mass; + + /// The moment of inertia of the robot about the origin (kg−m²). + double moi; + + /// Radius of wheel (m). + double wheel_radius; + + /// Maximum angular velocity of wheel (rad/s). + double wheel_max_angular_velocity; + + /// Maximum torque applied to wheel (N−m). + double wheel_max_torque; + + /// The Coefficient of Friction (CoF) of the wheels. + double wheel_cof; + + /// Strafing efficiency [0 -> 1] penalizes strafing by a certain percent. + double strafe_efficiency = 0.75; + + /// Translation of each wheel from the origin of the robot coordinate + /// system to the center of the wheel (m). + std::vector wheels; +}; + +/// The mecanum drive trajectory optimization solution. +struct TRAJOPT_DLLEXPORT MecanumSolution { + /// Times between samples. + std::vector dt; + + /// X positions. + std::vector x; + + /// Y positions. + std::vector y; + + /// Heading cosine. + std::vector thetacos; + + /// Heading sine. + std::vector thetasin; + + /// The x velocities. + std::vector vx; + + /// The y velocities. + std::vector vy; + + /// The angular velocities. + std::vector omega; + + /// The x accelerations. + std::vector ax; + + /// The y accelerations. + std::vector ay; + + /// The angular accelerations. + std::vector alpha; + + /// The forces along the active axis of each wheel. + std::vector> wheel_f; +}; + +/// Mecanum drive trajectory sample. +class TRAJOPT_DLLEXPORT MecanumTrajectorySample { + public: + /// The timestamp. + double timestamp = 0.0; + + /// The x coordinate. + double x = 0.0; + + /// The y coordinate. + double y = 0.0; + + /// The heading. + double heading = 0.0; + + /// The velocity's x component. + double velocity_x = 0.0; + + /// The velocity's y component. + double velocity_y = 0.0; + + /// The angular velocity. + double angular_velocity = 0.0; + + /// The acceleration's x component. + double acceleration_x = 0.0; + + /// The acceleration's y component. + double acceleration_y = 0.0; + + /// The angular acceleration. + double angular_acceleration = 0.0; + + /// The force on each wheel along its active axis. + std::vector wheel_forces; + + MecanumTrajectorySample() = default; + + /// Construct a MecanumTrajectorySample. + /// + /// @param timestamp The timestamp. + /// @param x The x coordinate. + /// @param y The y coordinate. + /// @param heading The heading. + /// @param velocity_x The velocity's x component. + /// @param velocity_y The velocity's y component. + /// @param angular_velocity The angular velocity. + /// @param acceleration_x The acceleration's x component. + /// @param acceleration_y The acceleration's y component. + /// @param angular_acceleration The angular acceleration. + /// @param wheel_forces Forces along the wheel directions. + MecanumTrajectorySample(double timestamp, double x, double y, double heading, + double velocity_x, double velocity_y, + double angular_velocity, double acceleration_x, + double acceleration_y, double angular_acceleration, + std::vector wheel_forces) + : timestamp{timestamp}, + x{x}, + y{y}, + heading{heading}, + velocity_x{velocity_x}, + velocity_y{velocity_y}, + angular_velocity{angular_velocity}, + acceleration_x{acceleration_x}, + acceleration_y{acceleration_y}, + angular_acceleration{angular_acceleration}, + wheel_forces{std::move(wheel_forces)} {} +}; + +/// Mecanum trajectory. +class TRAJOPT_DLLEXPORT MecanumTrajectory { + public: + /// Trajectory samples. + std::vector samples; + + MecanumTrajectory() = default; + + /// Construct a MecanumTrajectory from samples. + /// + /// @param samples The samples. + explicit MecanumTrajectory(std::vector samples) + : samples{std::move(samples)} {} + + /// Construct a MecanumTrajectory from a swerve solution. + /// + /// @param solution The swerve solution. + explicit MecanumTrajectory(const MecanumSolution& solution) { + double ts = 0.0; + for (size_t sample = 0; sample < solution.x.size(); ++sample) { + samples.emplace_back( + ts, solution.x[sample], solution.y[sample], + std::atan2(solution.thetasin[sample], solution.thetacos[sample]), + solution.vx[sample], solution.vy[sample], solution.omega[sample], + solution.ax[sample], solution.ay[sample], solution.alpha[sample], + solution.wheel_f[sample]); + ts += solution.dt[sample]; + } + } +}; + +/// A mecanum path. +using MecanumPath = Path; + +/// Builds a mecanum path using information about how the robot must travel +/// through a series of waypoints. This path can be converted to a trajectory +/// using MecanumTrajectoryGenerator. +using MecanumPathBuilder = PathBuilder; + +/// This trajectory generator class contains functions to generate time-optimal +/// trajectories for several drivetrain types. +class TRAJOPT_DLLEXPORT MecanumTrajectoryGenerator { + public: + /// Construct a new swerve trajectory optimization problem. + /// + /// @param path_builder The path builder. + /// @param handle An identifier for state callbacks. + explicit MecanumTrajectoryGenerator(MecanumPathBuilder path_builder, + int64_t handle = 0); + + /// Generates an optimal trajectory. + /// + /// This function may take a long time to complete. + /// + /// @param diagnostics Enables diagnostic prints. + /// @return Returns a holonomic trajectory on success, or the solver's exit + /// status on failure. + std::expected generate( + bool diagnostics = false); + + private: + /// Mecanum path + MecanumPath path; + + /// State Variables + std::vector> x; + std::vector> y; + std::vector> cosθ; + std::vector> sinθ; + std::vector> vx; + std::vector> vy; + std::vector> ω; + std::vector> ax; + std::vector> ay; + std::vector> α; + + /// Input Variables + std::vector>> F; + + /// Time Variables + std::vector> dts; + + /// Discretization Constants + std::vector Ns; + + slp::Problem problem; + + void apply_initial_guess(const MecanumSolution& solution); + + MecanumSolution construct_mecanum_solution(); +}; + +} // namespace trajopt diff --git a/trajoptlib/include/trajopt/swerve_trajectory_generator.hpp b/trajoptlib/include/trajopt/swerve_trajectory_generator.hpp index 9b75e2a022..822bfebd58 100644 --- a/trajoptlib/include/trajopt/swerve_trajectory_generator.hpp +++ b/trajoptlib/include/trajopt/swerve_trajectory_generator.hpp @@ -193,13 +193,13 @@ class TRAJOPT_DLLEXPORT SwerveTrajectory { /// A swerve path. using SwervePath = Path; -/// Builds a swerve path using information about how the robot -/// must travel through a series of waypoints. This path can be converted -/// to a trajectory using SwerveTrajectoryGenerator. +/// Builds a swerve path using information about how the robot must travel +/// through a series of waypoints. This path can be converted to a trajectory +/// using SwerveTrajectoryGenerator. using SwervePathBuilder = PathBuilder; -/// This trajectory generator class contains functions to generate -/// time-optimal trajectories for several drivetrain types. +/// This trajectory generator class contains functions to generate time-optimal +/// trajectories for several drivetrain types. class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator { public: /// Construct a new swerve trajectory optimization problem. diff --git a/trajoptlib/src/lib.rs b/trajoptlib/src/lib.rs index 154d44b74e..473e869fb4 100644 --- a/trajoptlib/src/lib.rs +++ b/trajoptlib/src/lib.rs @@ -45,6 +45,27 @@ mod ffi { /// coordinate system to the center of the module (m). There's /// usually one in each corner. modules: Vec, + trackwidth: f64, + } + + #[derive(Debug, Deserialize, Serialize, Clone)] + struct MecanumDrivetrain { + mass: f64, + moi: f64, + wheel_radius: f64, + wheel_max_angular_velocity: f64, + wheel_max_torque: f64, + wheel_cof: f64, + static_friction_coefficient: f64, + strafe_efficiency: f64, + modules: Vec, + } + + #[derive(Debug, Deserialize, Serialize, Clone)] + struct Pose2d { + x: f64, + y: f64, + heading: f64, } #[derive(Debug, Deserialize, Serialize, Clone)] @@ -138,6 +159,26 @@ mod ffi { samples: Vec, } + #[derive(Debug, Deserialize, Serialize, Clone)] + struct MecanumTrajectorySample { + timestamp: f64, + x: f64, + y: f64, + heading: f64, + velocity_x: f64, + velocity_y: f64, + angular_velocity: f64, + acceleration_x: f64, + acceleration_y: f64, + angular_acceleration: f64, + wheel_forces: Vec, + } + + #[derive(Debug, Deserialize, Serialize, Clone)] + struct MecanumTrajectory { + samples: Vec, + } + unsafe extern "C++" { include!("rust_ffi.hpp"); @@ -561,6 +602,221 @@ mod ffi { // Cancel all generators fn cancel_all(); + + type MecanumTrajectoryGenerator; + + fn mecanum_trajectory_generator_new() -> UniquePtr; + + fn set_drivetrain( + self: Pin<&mut MecanumTrajectoryGenerator>, + drivetrain: &MecanumDrivetrain, + ); + + fn set_bumpers( + self: Pin<&mut MecanumTrajectoryGenerator>, + front: f64, + left: f64, + right: f64, + back: f64, + ); + + fn set_control_interval_counts( + self: Pin<&mut MecanumTrajectoryGenerator>, + counts: Vec, + ); + + // Pose constraints + + fn pose_wpt( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + x: f64, + y: f64, + heading: f64, + ); + + fn translation_wpt( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + x: f64, + y: f64, + heading_guess: f64, + ); + + fn empty_wpt( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + x_guess: f64, + y_guess: f64, + heading_guess: f64, + ); + + // Segment initial guess points setter + + fn sgmt_initial_guess_points( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + guess_points: &Vec, + ); + + // Constraints with waypoint scope + + fn wpt_linear_velocity_direction( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + angle: f64, + ); + + fn wpt_linear_velocity_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + magnitude: f64, + ); + + fn wpt_angular_velocity_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + angular_velocity: f64, + ); + + fn wpt_linear_acceleration_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + magnitude: f64, + ); + + fn wpt_point_at( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + field_point_x: f64, + field_point_y: f64, + heading_tolerance: f64, + flip: bool, + ); + + fn wpt_keep_in_circle( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ); + + fn wpt_keep_in_polygon( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + field_points_x: Vec, + field_points_y: Vec, + ); + + fn wpt_keep_in_lane( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); + + fn wpt_keep_out_circle( + self: Pin<&mut MecanumTrajectoryGenerator>, + index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ); + + // Constraints with segment scope + + fn sgmt_linear_velocity_direction( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + angle: f64, + ); + + fn sgmt_linear_velocity_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + magnitude: f64, + ); + + fn sgmt_angular_velocity_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + angular_velocity: f64, + ); + + fn sgmt_linear_acceleration_max_magnitude( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + magnitude: f64, + ); + + fn sgmt_point_at( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + field_point_x: f64, + field_point_y: f64, + heading_tolerance: f64, + flip: bool, + ); + + fn sgmt_keep_in_circle( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ); + + fn sgmt_keep_in_polygon( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + field_points_x: Vec, + field_points_y: Vec, + ); + + #[allow(clippy::too_many_arguments)] + fn sgmt_keep_in_lane( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ); + + fn sgmt_keep_out_circle( + self: Pin<&mut MecanumTrajectoryGenerator>, + from_index: usize, + to_index: usize, + x: f64, + y: f64, + radius: f64, + ); + + // Trajectory generator functions + + fn add_callback( + self: Pin<&mut MecanumTrajectoryGenerator>, + callback: fn(MecanumTrajectory, i64), + ); + + fn generate( + self: &MecanumTrajectoryGenerator, + diagnostics: bool, + uuid: i64, + ) -> Result; } } @@ -1736,6 +1992,405 @@ impl DifferentialTrajectoryGenerator { } /// Cancels all running generations. +pub struct MecanumTrajectoryGenerator { + generator: cxx::UniquePtr, +} + +impl Default for MecanumTrajectoryGenerator { + fn default() -> Self { + Self::new() + } +} + +impl MecanumTrajectoryGenerator { + pub fn new() -> MecanumTrajectoryGenerator { + MecanumTrajectoryGenerator { + generator: crate::ffi::mecanum_trajectory_generator_new(), + } + } + + pub fn set_drivetrain(&mut self, drivetrain: &crate::ffi::MecanumDrivetrain) { + crate::ffi::MecanumTrajectoryGenerator::set_drivetrain( + self.generator.pin_mut(), + drivetrain, + ); + } + + pub fn set_bumpers(&mut self, front: f64, left: f64, right: f64, back: f64) { + crate::ffi::MecanumTrajectoryGenerator::set_bumpers( + self.generator.pin_mut(), + front, + left, + right, + back, + ); + } + + pub fn set_control_interval_counts(&mut self, counts: Vec) { + crate::ffi::MecanumTrajectoryGenerator::set_control_interval_counts( + self.generator.pin_mut(), + counts, + ); + } + + // Constraints with waypoint scope + + pub fn pose_wpt(&mut self, index: usize, x: f64, y: f64, heading: f64) { + crate::ffi::MecanumTrajectoryGenerator::pose_wpt( + self.generator.pin_mut(), + index, + x, + y, + heading, + ); + } + + pub fn translation_wpt(&mut self, index: usize, x: f64, y: f64, heading_guess: f64) { + crate::ffi::MecanumTrajectoryGenerator::translation_wpt( + self.generator.pin_mut(), + index, + x, + y, + heading_guess, + ); + } + + pub fn empty_wpt(&mut self, index: usize, x_guess: f64, y_guess: f64, heading_guess: f64) { + crate::ffi::MecanumTrajectoryGenerator::empty_wpt( + self.generator.pin_mut(), + index, + x_guess, + y_guess, + heading_guess, + ); + } + + // Segment initial guess points setter + + pub fn sgmt_initial_guess_points( + &mut self, + from_index: usize, + guess_points: &Vec, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_initial_guess_points( + self.generator.pin_mut(), + from_index, + guess_points, + ); + } + + // Constraints with waypoint scope + + pub fn wpt_linear_velocity_direction(&mut self, index: usize, angle: f64) { + crate::ffi::MecanumTrajectoryGenerator::wpt_linear_velocity_direction( + self.generator.pin_mut(), + index, + angle, + ); + } + + pub fn wpt_linear_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) { + crate::ffi::MecanumTrajectoryGenerator::wpt_linear_velocity_max_magnitude( + self.generator.pin_mut(), + index, + magnitude, + ); + } + + pub fn wpt_angular_velocity_max_magnitude(&mut self, index: usize, angular_velocity: f64) { + crate::ffi::MecanumTrajectoryGenerator::wpt_angular_velocity_max_magnitude( + self.generator.pin_mut(), + index, + angular_velocity, + ); + } + + pub fn wpt_linear_acceleration_max_magnitude(&mut self, index: usize, magnitude: f64) { + crate::ffi::MecanumTrajectoryGenerator::wpt_linear_acceleration_max_magnitude( + self.generator.pin_mut(), + index, + magnitude, + ); + } + + pub fn wpt_point_at( + &mut self, + index: usize, + field_point_x: f64, + field_point_y: f64, + heading_tolerance: f64, + flip: bool, + ) { + crate::ffi::MecanumTrajectoryGenerator::wpt_point_at( + self.generator.pin_mut(), + index, + field_point_x, + field_point_y, + heading_tolerance, + flip, + ) + } + + pub fn wpt_keep_in_circle( + &mut self, + index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::wpt_keep_in_circle( + self.generator.pin_mut(), + index, + field_point_x, + field_point_y, + keep_in_radius, + ) + } + + pub fn wpt_keep_in_polygon( + &mut self, + index: usize, + field_points_x: Vec, + field_points_y: Vec, + ) { + crate::ffi::MecanumTrajectoryGenerator::wpt_keep_in_polygon( + self.generator.pin_mut(), + index, + field_points_x, + field_points_y, + ); + } + + pub fn wpt_keep_in_lane( + &mut self, + index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::wpt_keep_in_lane( + self.generator.pin_mut(), + index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ); + } + + pub fn wpt_keep_out_circle( + &mut self, + index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::wpt_keep_out_circle( + self.generator.pin_mut(), + index, + field_point_x, + field_point_y, + keep_in_radius, + ) + } + + // Constraints with segment scope + + pub fn sgmt_linear_velocity_direction( + &mut self, + from_index: usize, + to_index: usize, + angle: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_linear_velocity_direction( + self.generator.pin_mut(), + from_index, + to_index, + angle, + ); + } + + pub fn sgmt_linear_velocity_max_magnitude( + &mut self, + from_index: usize, + to_index: usize, + magnitude: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_linear_velocity_max_magnitude( + self.generator.pin_mut(), + from_index, + to_index, + magnitude, + ); + } + + pub fn sgmt_angular_velocity_max_magnitude( + &mut self, + from_index: usize, + to_index: usize, + angular_velocity: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_angular_velocity_max_magnitude( + self.generator.pin_mut(), + from_index, + to_index, + angular_velocity, + ); + } + + pub fn sgmt_linear_acceleration_max_magnitude( + &mut self, + from_index: usize, + to_index: usize, + magnitude: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_linear_acceleration_max_magnitude( + self.generator.pin_mut(), + from_index, + to_index, + magnitude, + ); + } + + pub fn sgmt_keep_in_circle( + &mut self, + from_index: usize, + to_index: usize, + field_point_x: f64, + field_point_y: f64, + keep_in_radius: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_keep_in_circle( + self.generator.pin_mut(), + from_index, + to_index, + field_point_x, + field_point_y, + keep_in_radius, + ) + } + + pub fn sgmt_keep_in_polygon( + &mut self, + from_index: usize, + to_index: usize, + field_points_x: Vec, + field_points_y: Vec, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_keep_in_polygon( + self.generator.pin_mut(), + from_index, + to_index, + field_points_x, + field_points_y, + ); + } + + #[allow(clippy::too_many_arguments)] + pub fn sgmt_keep_in_lane( + &mut self, + from_index: usize, + to_index: usize, + center_line_start_x: f64, + center_line_start_y: f64, + center_line_end_x: f64, + center_line_end_y: f64, + tolerance: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_keep_in_lane( + self.generator.pin_mut(), + from_index, + to_index, + center_line_start_x, + center_line_start_y, + center_line_end_x, + center_line_end_y, + tolerance, + ) + } + + pub fn sgmt_keep_out_circle( + &mut self, + from_index: usize, + to_index: usize, + x: f64, + y: f64, + radius: f64, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_keep_out_circle( + self.generator.pin_mut(), + from_index, + to_index, + x, + y, + radius, + ); + } + + pub fn sgmt_point_at( + &mut self, + from_index: usize, + to_index: usize, + field_point_x: f64, + field_point_y: f64, + heading_tolerance: f64, + flip: bool, + ) { + crate::ffi::MecanumTrajectoryGenerator::sgmt_point_at( + self.generator.pin_mut(), + from_index, + to_index, + field_point_x, + field_point_y, + heading_tolerance, + flip, + ) + } + + /// + /// Add a callback that will be called on each iteration of the solver. + /// + /// * callback: a `fn` (not a closure) to be executed. The callback's first + /// parameter will be a `trajopt::MecanumTrajectory`, and the second + /// parameter will be an `i64` equal to the handle passed in `generate()` + /// + /// This function can be called multiple times to add multiple callbacks. + pub fn add_callback(&mut self, callback: fn(MecanumTrajectory, i64)) { + crate::ffi::MecanumTrajectoryGenerator::add_callback(self.generator.pin_mut(), callback); + } + + /// + /// Generate the trajectory; + /// + /// * diagnostics: If true, prints per-iteration details of the solver to + /// stdout. + /// * handle: A number used to identify results from this generation in the + /// `add_callback` callback. If `add_callback` has not been called, this + /// value has no significance. + /// + /// Returns a result with either the final `trajopt::MecanumTrajectory`, + /// or a TrajoptError if generation failed. + pub fn generate( + &self, + diagnostics: bool, + handle: i64, + ) -> Result { + match self.generator.generate(diagnostics, handle) { + Ok(trajectory) => Ok(trajectory), + Err(msg) => { + let what = msg.what(); + Err(TrajoptError::from( + what.parse::() + .map_err(|_| TrajoptError::Unparsable(Box::from(what)))?, + )) + } + } + } +} + pub fn cancel_all() { crate::ffi::cancel_all(); } @@ -1744,6 +2399,9 @@ use error::TrajoptError; pub use ffi::DifferentialDrivetrain; pub use ffi::DifferentialTrajectory; pub use ffi::DifferentialTrajectorySample; +pub use ffi::MecanumDrivetrain; +pub use ffi::MecanumTrajectory; +pub use ffi::MecanumTrajectorySample; pub use ffi::Pose2d; pub use ffi::SwerveDrivetrain; pub use ffi::SwerveTrajectory; diff --git a/trajoptlib/src/mecanum_trajectory_generator.cpp b/trajoptlib/src/mecanum_trajectory_generator.cpp new file mode 100644 index 0000000000..4f64750b87 --- /dev/null +++ b/trajoptlib/src/mecanum_trajectory_generator.cpp @@ -0,0 +1,430 @@ +// Copyright (c) TrajoptLib contributors + +#include "trajopt/mecanum_trajectory_generator.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/autodiff/variable.hpp" +#include "trajopt/util/cancellation.hpp" +#include "trajopt/util/trajopt_util.hpp" + +// Physics notation in this file: +// +// x = linear position +// v = linear velocity +// a = linear acceleration +// θ = heading +// ω = angular velocity +// α = angular acceleration +// F = force +// τ = torque +// t = time +// +// A `d` prefix to one of these means delta. + +namespace trajopt { + +MecanumTrajectoryGenerator::MecanumTrajectoryGenerator( + MecanumPathBuilder path_builder, int64_t handle) + : path(path_builder.get_path()), + Ns(path_builder.get_control_interval_counts()) { + auto initial_guess = path_builder.calculate_linear_initial_guess(); + + problem.add_callback( + [this, handle = handle](const slp::IterationInfo&) -> bool { + constexpr int fps = 60; + constexpr std::chrono::duration time_per_frame{1.0 / fps}; + + // FPS limit on sending updates + static auto last_frame_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + if (now - last_frame_time < time_per_frame) { + return trajopt::get_cancellation_flag(); + } + + last_frame_time = now; + + auto soln = construct_mecanum_solution(); + for (auto& callback : this->path.callbacks) { + callback(soln, handle); + } + + return trajopt::get_cancellation_flag(); + }); + + size_t wpt_cnt = path.waypoints.size(); + size_t sgmt_cnt = path.waypoints.size() - 1; + size_t samp_tot = get_index(Ns, wpt_cnt - 1, 0) + 1; + size_t module_cnt = path.drivetrain.wheels.size(); + + x.reserve(samp_tot); + y.reserve(samp_tot); + cosθ.reserve(samp_tot); + sinθ.reserve(samp_tot); + vx.reserve(samp_tot); + vy.reserve(samp_tot); + ω.reserve(samp_tot); + ax.reserve(samp_tot); + ay.reserve(samp_tot); + α.reserve(samp_tot); + + F.reserve(samp_tot); + for (size_t sample_index = 0; sample_index < samp_tot; ++sample_index) { + auto& _F = F.emplace_back(); + _F.reserve(module_cnt); + } + + dts.reserve(samp_tot); + + for (size_t index = 0; index < samp_tot; ++index) { + x.emplace_back(problem.decision_variable()); + y.emplace_back(problem.decision_variable()); + cosθ.emplace_back(problem.decision_variable()); + sinθ.emplace_back(problem.decision_variable()); + vx.emplace_back(problem.decision_variable()); + vy.emplace_back(problem.decision_variable()); + ω.emplace_back(problem.decision_variable()); + ax.emplace_back(problem.decision_variable()); + ay.emplace_back(problem.decision_variable()); + α.emplace_back(problem.decision_variable()); + + for (size_t module_index = 0; module_index < module_cnt; ++module_index) { + F.at(index).emplace_back(problem.decision_variable()); + } + + dts.emplace_back(problem.decision_variable()); + } + + // Minimize total time + constexpr double s_init = 1.0 / std::numbers::sqrt2; + const double max_force = path.drivetrain.wheel_max_torque * 4 / + path.drivetrain.wheel_radius * s_init; + const auto max_accel = max_force / path.drivetrain.mass; + const double max_drivetrain_velocity = + path.drivetrain.wheel_radius * path.drivetrain.wheel_max_angular_velocity; + auto max_wheel_position_radius = 0.0; + for (auto module : path.drivetrain.wheels) { + max_wheel_position_radius = + std::max(max_wheel_position_radius, module.norm()); + } + const auto max_ang_vel = max_drivetrain_velocity / max_wheel_position_radius; + const auto max_ang_accel = max_accel / max_wheel_position_radius; + for (size_t sgmt_index = 0; sgmt_index < Ns.size(); ++sgmt_index) { + auto N_sgmt = Ns.at(sgmt_index); + const auto sgmt_start = get_index(Ns, sgmt_index); + const auto sgmt_end = get_index(Ns, sgmt_index + 1); + + if (N_sgmt == 0) { + for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) { + dts.at(index).set_value(0.0); + } + } else { + // Use initial_guess and Ns to find the dx, dy, dθ between wpts + const auto dx = + initial_guess.x.at(sgmt_end) - initial_guess.x.at(sgmt_start); + const auto dy = + initial_guess.y.at(sgmt_end) - initial_guess.y.at(sgmt_start); + const auto dist = std::hypot(dx, dy); + const auto θ_0 = std::atan2(initial_guess.thetasin.at(sgmt_start), + initial_guess.thetacos.at(sgmt_start)); + const auto θ_1 = std::atan2(initial_guess.thetasin.at(sgmt_end), + initial_guess.thetacos.at(sgmt_end)); + const auto dθ = std::abs(angle_modulus(θ_1 - θ_0)); + + auto max_linear_vel = max_drivetrain_velocity; + + const auto angular_time = + calculate_trapezoidal_time(dθ, max_ang_vel, max_ang_accel); + max_linear_vel = std::min(max_linear_vel, dist / angular_time); + + const auto linear_time = + calculate_trapezoidal_time(dist, max_linear_vel, max_accel); + const double sgmt_time = angular_time + linear_time; + + for (size_t index = sgmt_start; index < sgmt_end + 1; ++index) { + auto& dt = dts.at(index); + problem.subject_to(slp::bounds(0, dt, 3)); + dt.set_value(sgmt_time / N_sgmt); + } + } + } + problem.minimize(std::accumulate(dts.begin(), dts.end(), slp::Variable{0.0})); + + // Apply kinematics constraints + for (size_t wpt_index = 0; wpt_index < wpt_cnt - 1; ++wpt_index) { + size_t N_sgmt = Ns.at(wpt_index); + + for (size_t sample_index = 0; sample_index < N_sgmt; ++sample_index) { + size_t index = get_index(Ns, wpt_index, sample_index); + + Translation2v x_k{x.at(index), y.at(index)}; + Translation2v x_k_1{x.at(index + 1), y.at(index + 1)}; + + Rotation2v θ_k{cosθ.at(index), sinθ.at(index)}; + Rotation2v θ_k_1{cosθ.at(index + 1), sinθ.at(index + 1)}; + + Translation2v v_k{vx.at(index), vy.at(index)}; + Translation2v v_k_1{vx.at(index + 1), vy.at(index + 1)}; + + auto ω_k = ω.at(index); + auto ω_k_1 = ω.at(index + 1); + + Translation2v a_k{ax.at(index), ay.at(index)}; + Translation2v a_k_1{ax.at(index + 1), ay.at(index + 1)}; + + auto α_k = α.at(index); + auto α_k_1 = α.at(index + 1); + + auto dt_k = dts.at(index); + if (sample_index < N_sgmt - 1) { + auto dt_k_1 = dts.at(index + 1); + problem.subject_to(dt_k_1 == dt_k); + } + + // xₖ₊₁ = xₖ + vₖt + 1/2aₖt² + // θₖ₊₁ = θₖ + ωₖt + // vₖ₊₁ = vₖ + aₖt + // ωₖ₊₁ = ωₖ + αₖt + problem.subject_to(x_k_1 == x_k + v_k * dt_k + a_k * 0.5 * dt_k * dt_k); + problem.subject_to((θ_k_1 - θ_k) == Rotation2v{ω_k * dt_k}); + problem.subject_to(v_k_1 == v_k + a_k * dt_k); + problem.subject_to(ω_k_1 == ω_k + α_k * dt_k); + } + } + + constexpr double s = 1.0 / std::numbers::sqrt2; + + const double strafe_eff = path.drivetrain.strafe_efficiency; + std::vector fx_coeff = {s, s, s, s}; + std::vector fy_coeff = {-s * strafe_eff, -s * strafe_eff, + s * strafe_eff, s * strafe_eff}; + + for (size_t index = 0; index < samp_tot; ++index) { + Rotation2v θ_k{cosθ.at(index), sinθ.at(index)}; + Translation2v v_k{vx.at(index), vy.at(index)}; + + // x and y forces in the body frame + std::vector> Fx_body; + Fx_body.reserve(path.drivetrain.wheels.size()); + std::vector> Fy_body; + Fy_body.reserve(path.drivetrain.wheels.size()); + + for (size_t i = 0; i < path.drivetrain.wheels.size(); ++i) { + Fx_body.push_back(fx_coeff[i] * F.at(index)[i]); + Fy_body.push_back(fy_coeff[i] * F.at(index)[i]); + } + + // Solve for net force + auto Fx_body_net = + std::accumulate(Fx_body.begin(), Fx_body.end(), slp::Variable{0.0}); + auto Fy_body_net = + std::accumulate(Fy_body.begin(), Fy_body.end(), slp::Variable{0.0}); + auto F_world_net = + Translation2v{Fx_body_net, Fy_body_net}.rotate_by(θ_k); + + // Solve for net torque + slp::Variable τ_net = 0.0; + for (size_t module_index = 0; module_index < path.drivetrain.wheels.size(); + ++module_index) { + const auto& r_body = path.drivetrain.wheels.at(module_index); + Translation2v F_body{Fx_body.at(module_index), + Fy_body.at(module_index)}; + + τ_net += r_body.cross(F_body); + } + + // Apply module power constraints + auto v_wrt_robot = v_k.rotate_by(-θ_k); + for (size_t module_index = 0; module_index < path.drivetrain.wheels.size(); + ++module_index) { + const auto& translation = path.drivetrain.wheels.at(module_index); + + double sign = (module_index == 2 || module_index == 3) ? 1.0 : -1.0; + + // v_active_trans = v_x cos(45°) - v_y sin(45°) sign + // = (v_x − v_y sign) / √(2) + auto v_active_from_translation = + (v_wrt_robot.x() - v_wrt_robot.y() * sign) * s; + + // v_active_rot = v_rot_x cos(45°) − v_rot_y sin(45°) sign + // = (−ωy) cos(45°) − (ωx) sin(45°) sign + // = −ω(y cos(45°) + x sin(45°) sign) + // = −ω(y + x sign) / √(2) + auto v_active_from_rotation = + -ω.at(index) * (translation.y() + translation.x() * sign) * s; + + auto v_wheel = v_active_from_translation + v_active_from_rotation; + + double max_wheel_velocity = path.drivetrain.wheel_radius * + path.drivetrain.wheel_max_angular_velocity; + + // |v|₂² ≤ vₘₐₓ² + problem.subject_to(v_wheel * v_wheel <= + max_wheel_velocity * max_wheel_velocity); + + // τ = r x F + // F = τ/r + double max_wheel_force = + (path.drivetrain.wheel_max_torque / path.drivetrain.wheel_radius) * s; + + // 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); + + // |F|₂² ≤ Fₘₐₓ² + auto f = F.at(index).at(module_index); + problem.subject_to(f * f <= max_force * max_force); + } + + // Apply dynamics constraints + // + // ΣF_xₖ = ma_xₖ + // ΣF_yₖ = ma_yₖ + // Στₖ = Jαₖ + problem.subject_to(F_world_net.x() == path.drivetrain.mass * ax.at(index)); + problem.subject_to(F_world_net.y() == path.drivetrain.mass * ay.at(index)); + problem.subject_to(τ_net == path.drivetrain.moi * α.at(index)); + } + + for (size_t wpt_index = 0; wpt_index < wpt_cnt; ++wpt_index) { + // First index of next wpt - 1 + size_t index = get_index(Ns, wpt_index, 0); + + Pose2v pose_k{ + x.at(index), y.at(index), {cosθ.at(index), sinθ.at(index)}}; + Translation2v v_k{vx.at(index), vy.at(index)}; + auto ω_k = ω.at(index); + Translation2v a_k{ax.at(index), ay.at(index)}; + auto α_k = α.at(index); + + for (auto& constraint : path.waypoints.at(wpt_index).waypoint_constraints) { + std::visit( + [&](auto&& arg) { arg.apply(problem, pose_k, v_k, ω_k, a_k, α_k); }, + constraint); + } + } + + for (size_t sgmt_index = 0; sgmt_index < sgmt_cnt; ++sgmt_index) { + size_t start_index = get_index(Ns, sgmt_index, 0); + size_t end_index = get_index(Ns, sgmt_index + 1, 0); + + for (size_t index = start_index; index < end_index; ++index) { + Pose2v pose_k{ + x.at(index), y.at(index), {cosθ.at(index), sinθ.at(index)}}; + Translation2v v_k{vx.at(index), vy.at(index)}; + auto ω_k = ω.at(index); + Translation2v a_k{ax.at(index), ay.at(index)}; + auto α_k = α.at(index); + + for (auto& constraint : + path.waypoints.at(sgmt_index + 1).segment_constraints) { + std::visit( + [&](auto&& arg) { arg.apply(problem, pose_k, v_k, ω_k, a_k, α_k); }, + constraint); + } + } + } + + apply_initial_guess(initial_guess); +} + +std::expected +MecanumTrajectoryGenerator::generate(bool diagnostics) { + get_cancellation_flag() = 0; + + // tolerance of 1e-4 is 0.1 mm + auto status = problem.solve({.tolerance = 1e-4, .diagnostics = diagnostics}); + + if (static_cast(status) < 0 || + status == slp::ExitStatus::CALLBACK_REQUESTED_STOP) { + return std::unexpected{status}; + } else { + return construct_mecanum_solution(); + } +} + +void MecanumTrajectoryGenerator::apply_initial_guess( + const MecanumSolution& solution) { + size_t sample_total = x.size(); + for (size_t sample_index = 0; sample_index < sample_total; ++sample_index) { + x[sample_index].set_value(solution.x[sample_index]); + y[sample_index].set_value(solution.y[sample_index]); + cosθ[sample_index].set_value(solution.thetacos[sample_index]); + sinθ[sample_index].set_value(solution.thetasin[sample_index]); + } + + vx[0].set_value(0.0); + vy[0].set_value(0.0); + ω[0].set_value(0.0); + ax[0].set_value(0.0); + ay[0].set_value(0.0); + α[0].set_value(0.0); + + for (size_t sample_index = 1; sample_index < sample_total; ++sample_index) { + vx[sample_index].set_value( + (solution.x[sample_index] - solution.x[sample_index - 1]) / + solution.dt[sample_index]); + vy[sample_index].set_value( + (solution.y[sample_index] - solution.y[sample_index - 1]) / + solution.dt[sample_index]); + + double cosθ = solution.thetacos[sample_index]; + double sinθ = solution.thetasin[sample_index]; + double last_cosθ = solution.thetacos[sample_index - 1]; + double last_sinθ = solution.thetasin[sample_index - 1]; + + ω[sample_index].set_value(Rotation2d{cosθ, sinθ} + .rotate_by(-Rotation2d{last_cosθ, last_sinθ}) + .radians() / + solution.dt[sample_index]); + + ax[sample_index].set_value( + (vx[sample_index].value() - vx[sample_index - 1].value()) / + solution.dt[sample_index]); + ay[sample_index].set_value( + (vy[sample_index].value() - vy[sample_index - 1].value()) / + solution.dt[sample_index]); + α[sample_index].set_value( + (ω[sample_index].value() - ω[sample_index - 1].value()) / + solution.dt[sample_index]); + } +} + +MecanumSolution MecanumTrajectoryGenerator::construct_mecanum_solution() { + auto get_value = [](auto& var) { return var.value(); }; + + auto vector_value = [&](std::vector>& row) { + return row | std::views::transform(get_value) | + std::ranges::to(); + }; + + auto matrix_value = + [&](std::vector>>& mat) { + return mat | std::views::transform([&](auto& v) { + return v | std::views::transform(get_value) | + std::ranges::to(); + }) | + std::ranges::to(); + }; + + return MecanumSolution{ + vector_value(dts), vector_value(x), vector_value(y), + vector_value(cosθ), vector_value(sinθ), vector_value(vx), + vector_value(vy), vector_value(ω), vector_value(ax), + vector_value(ay), vector_value(α), matrix_value(F), + }; +} + +} // namespace trajopt diff --git a/trajoptlib/src/rust_ffi.cpp b/trajoptlib/src/rust_ffi.cpp index 3b6ce65b9d..2a8aedbb4e 100644 --- a/trajoptlib/src/rust_ffi.cpp +++ b/trajoptlib/src/rust_ffi.cpp @@ -696,6 +696,352 @@ differential_trajectory_generator_new() { return std::make_unique(); } +void MecanumTrajectoryGenerator::set_drivetrain( + const MecanumDrivetrain& drivetrain) { + std::vector cpp_modules; + for (const auto& module : drivetrain.modules) { + cpp_modules.emplace_back(module.x, module.y); + } + + path_builder.set_drivetrain(trajopt::MecanumDrivetrain{ + drivetrain.mass, drivetrain.moi, drivetrain.wheel_radius, + drivetrain.wheel_max_angular_velocity, drivetrain.wheel_max_torque, + drivetrain.wheel_cof, drivetrain.strafe_efficiency, + std::move(cpp_modules)}); +} + +void MecanumTrajectoryGenerator::set_bumpers(double front, double left, + double right, double back) { + path_builder.set_bumpers(front, left, right, back); +} + +void MecanumTrajectoryGenerator::set_control_interval_counts( + const rust::Vec counts) { + std::vector cpp_counts; + for (const auto& count : counts) { + cpp_counts.emplace_back(count); + } + + path_builder.set_control_interval_counts(std::move(cpp_counts)); +} + +void MecanumTrajectoryGenerator::sgmt_initial_guess_points( + size_t from_index, const rust::Vec& guess_points) { + std::vector cpp_guess_points; + for (const auto& guess_point : guess_points) { + cpp_guess_points.emplace_back(guess_point.x, guess_point.y, + guess_point.heading); + } + + path_builder.sgmt_initial_guess_points(from_index, + std::move(cpp_guess_points)); +} + +void MecanumTrajectoryGenerator::pose_wpt(size_t index, double x, double y, + double heading) { + path_builder.pose_wpt(index, x, y, heading); +} + +void MecanumTrajectoryGenerator::translation_wpt(size_t index, double x, + double y, + double heading_guess) { + path_builder.translation_wpt(index, x, y, heading_guess); +} + +void MecanumTrajectoryGenerator::empty_wpt(size_t index, double x_guess, + double y_guess, + double heading_guess) { + path_builder.wpt_initial_guess_point(index, + {x_guess, y_guess, heading_guess}); +} + +void MecanumTrajectoryGenerator::wpt_linear_velocity_direction(size_t index, + double angle) { + path_builder.wpt_constraint( + index, trajopt::LinearVelocityDirectionConstraint{angle}); +} + +void MecanumTrajectoryGenerator::wpt_linear_velocity_max_magnitude( + size_t index, double magnitude) { + path_builder.wpt_constraint( + index, trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); +} + +void MecanumTrajectoryGenerator::wpt_angular_velocity_max_magnitude( + size_t index, double angular_velocity) { + path_builder.wpt_constraint( + index, trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity}); +} + +void MecanumTrajectoryGenerator::wpt_linear_acceleration_max_magnitude( + size_t index, double magnitude) { + path_builder.wpt_constraint( + index, trajopt::LinearAccelerationMaxMagnitudeConstraint{magnitude}); +} + +void MecanumTrajectoryGenerator::wpt_point_at(size_t index, + double field_point_x, + double field_point_y, + double heading_tolerance, + bool flip) { + path_builder.wpt_constraint( + index, trajopt::PointAtConstraint{ + trajopt::Translation2d{field_point_x, field_point_y}, + heading_tolerance, flip}); +} + +void MecanumTrajectoryGenerator::wpt_keep_in_circle(size_t index, + double field_point_x, + double field_point_y, + double keep_in_radius) { + for (size_t bumper = 0; bumper < path_builder.get_bumpers().size(); + bumper++) { + for (size_t i = 0; i < path_builder.get_bumpers().at(bumper).points.size(); + i++) { + path_builder.wpt_constraint( + index, trajopt::PointPointMaxConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + {field_point_x, field_point_y}, + keep_in_radius}); + } + } + path_builder.wpt_constraint( + index, trajopt::PointPointMaxConstraint{ + {0.0, 0.0}, {field_point_x, field_point_y}, keep_in_radius}); +} + +void MecanumTrajectoryGenerator::wpt_keep_in_polygon( + size_t index, rust::Vec field_points_x, + rust::Vec field_points_y) { + if (field_points_x.size() != field_points_y.size()) { + return; + } + for (size_t i = 0; i < field_points_x.size(); i++) { + auto j = (i + 1) % field_points_x.size(); + path_builder.wpt_constraint(index, + trajopt::PointLineRegionConstraint{ + {0.0, 0.0}, + {field_points_x[i], field_points_y[i]}, + {field_points_x[j], field_points_y[j]}, + Side::ABOVE}); + for (const auto& bumper : path_builder.get_bumpers()) { + for (const auto& corner : bumper.points) { + path_builder.wpt_constraint(index, + trajopt::PointLineRegionConstraint{ + corner, + {field_points_x[i], field_points_y[i]}, + {field_points_x[j], field_points_y[j]}, + Side::ABOVE}); + } + } + } +} + +void MecanumTrajectoryGenerator::wpt_keep_in_lane( + size_t index, double center_line_start_x, double center_line_start_y, + double center_line_end_x, double center_line_end_y, double tolerance) { + path_builder.wpt_constraint( + index, trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); +} + +void MecanumTrajectoryGenerator::wpt_keep_out_circle(size_t index, double x, + double y, double radius) { + for (size_t bumper = 0; bumper < path_builder.get_bumpers().size(); + bumper++) { + for (size_t i = 0; i < path_builder.get_bumpers().at(bumper).points.size(); + i++) { + path_builder.wpt_constraint( + index, trajopt::PointPointMinConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + {x, y}, + radius}); + path_builder.wpt_constraint( + index, trajopt::LinePointConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + path_builder.get_bumpers().at(bumper).points.at( + (i + 1) % + path_builder.get_bumpers().at(bumper).points.size()), + {x, y}, + radius}); + } + } +} + +void MecanumTrajectoryGenerator::sgmt_linear_velocity_direction( + size_t from_index, size_t to_index, double angle) { + path_builder.sgmt_constraint( + from_index, to_index, trajopt::LinearVelocityDirectionConstraint{angle}); +} + +void MecanumTrajectoryGenerator::sgmt_linear_velocity_max_magnitude( + size_t from_index, size_t to_index, double magnitude) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); +} + +void MecanumTrajectoryGenerator::sgmt_angular_velocity_max_magnitude( + size_t from_index, size_t to_index, double angular_velocity) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity}); +} + +void MecanumTrajectoryGenerator::sgmt_linear_acceleration_max_magnitude( + size_t from_index, size_t to_index, double magnitude) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::LinearAccelerationMaxMagnitudeConstraint{magnitude}); +} + +void MecanumTrajectoryGenerator::sgmt_point_at( + size_t from_index, size_t to_index, double field_point_x, + double field_point_y, double heading_tolerance, bool flip) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::PointAtConstraint{ + {field_point_x, field_point_y}, heading_tolerance, flip}); +} + +void MecanumTrajectoryGenerator::sgmt_keep_in_circle(size_t from_index, + size_t to_index, + double field_point_x, + double field_point_y, + double keep_in_radius) { + for (size_t bumper = 0; bumper < path_builder.get_bumpers().size(); + bumper++) { + for (size_t i = 0; i < path_builder.get_bumpers().at(bumper).points.size(); + i++) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::PointPointMaxConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + {field_point_x, field_point_y}, + keep_in_radius}); + } + } +} + +void MecanumTrajectoryGenerator::sgmt_keep_in_polygon( + size_t from_index, size_t to_index, rust::Vec field_points_x, + rust::Vec field_points_y) { + if (field_points_x.size() != field_points_y.size()) { + return; + } + for (size_t i = 0; i < field_points_x.size(); i++) { + auto j = (i + 1) % field_points_x.size(); + path_builder.sgmt_constraint(from_index, to_index, + trajopt::PointLineRegionConstraint{ + {0.0, 0.0}, + {field_points_x[i], field_points_y[i]}, + {field_points_x[j], field_points_y[j]}, + Side::ABOVE}); + for (const auto& bumper : path_builder.get_bumpers()) { + for (const auto& corner : bumper.points) { + path_builder.sgmt_constraint(from_index, to_index, + trajopt::PointLineRegionConstraint{ + corner, + {field_points_x[i], field_points_y[i]}, + {field_points_x[j], field_points_y[j]}, + Side::ABOVE}); + } + } + } +} + +void MecanumTrajectoryGenerator::sgmt_keep_in_lane( + size_t from_index, size_t to_index, double center_line_start_x, + double center_line_start_y, double center_line_end_x, + double center_line_end_y, double tolerance) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::LaneConstraint{{center_line_start_x, center_line_start_y}, + {center_line_end_x, center_line_end_y}, + tolerance}); +} + +void MecanumTrajectoryGenerator::sgmt_keep_out_circle(size_t from_index, + size_t to_index, double x, + double y, double radius) { + for (size_t bumper = 0; bumper < path_builder.get_bumpers().size(); + bumper++) { + for (size_t i = 0; i < path_builder.get_bumpers().at(bumper).points.size(); + i++) { + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::PointPointMinConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + {x, y}, + radius}); + path_builder.sgmt_constraint( + from_index, to_index, + trajopt::LinePointConstraint{ + path_builder.get_bumpers().at(bumper).points.at(i), + path_builder.get_bumpers().at(bumper).points.at( + (i + 1) % + path_builder.get_bumpers().at(bumper).points.size()), + {x, y}, + radius}); + } + } +} + +void MecanumTrajectoryGenerator::add_callback( + rust::Fn callback) { + path_builder.add_callback( + [=](const trajopt::MecanumSolution& solution, int64_t handle) { + trajopt::MecanumTrajectory cpp_trajectory{solution}; + + rust::Vec rust_samples; + for (const auto& cpp_sample : cpp_trajectory.samples) { + rust::Vec fs; + std::copy(cpp_sample.wheel_forces.begin(), + cpp_sample.wheel_forces.end(), std::back_inserter(fs)); + + rust_samples.push_back(MecanumTrajectorySample{ + cpp_sample.timestamp, cpp_sample.x, cpp_sample.y, + cpp_sample.heading, cpp_sample.velocity_x, cpp_sample.velocity_y, + cpp_sample.angular_velocity, cpp_sample.acceleration_x, + cpp_sample.acceleration_y, cpp_sample.angular_acceleration, + std::move(fs)}); + } + + callback(MecanumTrajectory{rust_samples}, handle); + }); +} + +MecanumTrajectory MecanumTrajectoryGenerator::generate(bool diagnostics, + int64_t handle) const { + trajopt::MecanumTrajectoryGenerator generator{path_builder, handle}; + if (auto sol = generator.generate(diagnostics); sol.has_value()) { + trajopt::MecanumTrajectory cpp_trajectory{sol.value()}; + + rust::Vec rust_samples; + for (const auto& cpp_sample : cpp_trajectory.samples) { + rust::Vec fs; + std::copy(cpp_sample.wheel_forces.begin(), cpp_sample.wheel_forces.end(), + std::back_inserter(fs)); + + rust_samples.push_back(MecanumTrajectorySample{ + cpp_sample.timestamp, cpp_sample.x, cpp_sample.y, cpp_sample.heading, + cpp_sample.velocity_x, cpp_sample.velocity_y, + cpp_sample.angular_velocity, cpp_sample.acceleration_x, + cpp_sample.acceleration_y, cpp_sample.angular_acceleration, + std::move(fs)}); + } + + return MecanumTrajectory{std::move(rust_samples)}; + } else { + throw sol.error(); + } +} + +std::unique_ptr mecanum_trajectory_generator_new() { + return std::make_unique(); +} + void cancel_all() { trajopt::get_cancellation_flag() = 1; } diff --git a/trajoptlib/src/rust_ffi.hpp b/trajoptlib/src/rust_ffi.hpp index 5d5cdae7d2..2358a53115 100644 --- a/trajoptlib/src/rust_ffi.hpp +++ b/trajoptlib/src/rust_ffi.hpp @@ -11,6 +11,7 @@ #include #include "trajopt/differential_trajectory_generator.hpp" +#include "trajopt/mecanum_trajectory_generator.hpp" #include "trajopt/swerve_trajectory_generator.hpp" // override cxx try/catch so it catches thrown integers/exit conditions @@ -29,9 +30,11 @@ namespace trajopt::rsffi { struct SwerveTrajectory; struct DifferentialTrajectory; +struct MecanumTrajectory; struct Pose2d; struct SwerveDrivetrain; struct DifferentialDrivetrain; +struct MecanumDrivetrain; class SwerveTrajectoryGenerator { public: @@ -186,11 +189,90 @@ class DifferentialTrajectoryGenerator { trajopt::DifferentialPathBuilder path_builder; }; +class MecanumTrajectoryGenerator { + public: + MecanumTrajectoryGenerator() = default; + + void set_drivetrain(const MecanumDrivetrain& drivetrain); + void set_bumpers(double front, double left, double right, double back); + void set_control_interval_counts(const rust::Vec counts); + void sgmt_initial_guess_points(size_t from_index, + const rust::Vec& guess_points); + + void pose_wpt(size_t index, double x, double y, double heading); + void translation_wpt(size_t index, double x, double y, double heading_guess); + void empty_wpt(size_t index, double x_guess, double y_guess, + double heading_guess); + + void wpt_linear_velocity_direction(size_t index, double angle); + void wpt_linear_velocity_max_magnitude(size_t index, double magnitude); + + void wpt_angular_velocity_max_magnitude(size_t index, + double angular_velocity); + void wpt_linear_acceleration_max_magnitude(size_t index, double magnitude); + void wpt_point_at(size_t index, double field_point_x, double field_point_y, + double heading_tolerance, bool flip); + void wpt_keep_in_circle(size_t index, double field_point_x, + double field_point_y, double keep_in_radius); + void wpt_keep_in_polygon(size_t index, rust::Vec field_points_x, + rust::Vec field_points_y); + void wpt_keep_in_lane(size_t index, double center_line_start_x, + double center_line_start_y, double center_line_end_x, + double center_line_end_y, double tolerance); + void wpt_keep_out_circle(size_t index, double field_point_x, + double field_point_y, double keep_in_radius); + + void sgmt_linear_velocity_direction(size_t from_index, size_t to_index, + double angle); + void sgmt_linear_velocity_max_magnitude(size_t from_index, size_t to_index, + double magnitude); + void sgmt_angular_velocity_max_magnitude(size_t from_index, size_t to_index, + double angular_velocity); + void sgmt_linear_acceleration_max_magnitude(size_t from_index, + size_t to_index, + double magnitude); + void sgmt_point_at(size_t from_index, size_t to_index, double field_point_x, + double field_point_y, double heading_tolerance, bool flip); + void sgmt_keep_in_circle(size_t from_index, size_t to_index, + double field_point_x, double field_point_y, + double keep_in_radius); + void sgmt_keep_in_polygon(size_t from_index, size_t to_index, + rust::Vec field_points_x, + rust::Vec field_points_y); + void sgmt_keep_in_lane(size_t from_index, size_t to_index, + double center_line_start_x, double center_line_start_y, + double center_line_end_x, double center_line_end_y, + double tolerance); + void sgmt_keep_out_circle(size_t from_index, size_t to_index, double x, + double y, double radius); + + /// Add a callback that will be called on each iteration of the solver. + /// + /// This function can be called multiple times to add multiple callbacks. + /// + /// @param callback A `fn` (not a closure) to be executed. The callback's + /// first parameter will be a `trajopt::MecanumTrajectory`, and the second + /// parameter will be an `i64` equal to the handle passed in `generate()`. + void add_callback(rust::Fn callback); + + // TODO: Return std::expected + // instead of throwing exception, once cxx supports it + // + // https://github.com/dtolnay/cxx/issues/1052 + MecanumTrajectory generate(bool diagnostics = false, + int64_t handle = 0) const; + + private: + trajopt::MecanumPathBuilder path_builder; +}; + std::unique_ptr swerve_trajectory_generator_new(); std::unique_ptr differential_trajectory_generator_new(); +std::unique_ptr mecanum_trajectory_generator_new(); + void cancel_all(); } // namespace trajopt::rsffi diff --git a/trajoptlib/test/src/mecanum_path_builder_test.cpp b/trajoptlib/test/src/mecanum_path_builder_test.cpp new file mode 100644 index 0000000000..60ddbc6b50 --- /dev/null +++ b/trajoptlib/test/src/mecanum_path_builder_test.cpp @@ -0,0 +1 @@ +// Copyright (c) TrajoptLib contributors