1#![allow(
2 dead_code,
3 clippy::needless_borrows_for_generic_args,
4 clippy::new_without_default
5)]
6
7#[cfg(feature = "viz")]
15use gnuplot::{AxesCommon, Caption, Color, Figure, PointSymbol};
16
17use std::f64::consts::PI;
18
19const KP: f64 = 15.0;
21const DT: f64 = 0.01;
22
23const L1: f64 = 1.0;
25const L2: f64 = 1.0;
26
27#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub enum ElbowBranch {
30 Positive,
31 Negative,
32}
33
34pub struct TwoJointArm {
35 pub theta1: f64,
36 pub theta2: f64,
37 pub target_x: f64,
38 pub target_y: f64,
39 pub trajectory: Vec<(f64, f64, f64, f64)>, }
41
42impl TwoJointArm {
43 pub const LINK_LENGTHS: [f64; 2] = [L1, L2];
45
46 pub fn new() -> Self {
47 TwoJointArm {
48 theta1: 0.0,
49 theta2: 0.0,
50 target_x: 2.0,
51 target_y: 0.0,
52 trajectory: Vec::new(),
53 }
54 }
55
56 pub fn set_target(&mut self, x: f64, y: f64) {
57 self.target_x = x;
58 self.target_y = y;
59 }
60
61 pub fn solve_inverse_kinematics(&mut self, goal_threshold: f64) -> bool {
62 let mut iterations = 0;
63 const MAX_ITERATIONS: usize = 1000;
64
65 self.trajectory.clear();
66
67 if !goal_threshold.is_finite()
68 || goal_threshold <= 0.0
69 || !self.theta1.is_finite()
70 || !self.theta2.is_finite()
71 {
72 return false;
73 }
74
75 while iterations < MAX_ITERATIONS {
76 let (end_x, end_y) = self.forward_kinematics();
77 self.trajectory
78 .push((self.theta1, self.theta2, end_x, end_y));
79
80 let (theta1_goal, theta2_goal) = match self.calculate_target_angles() {
82 Some(angles) => angles,
83 None => return false,
84 };
85
86 let distance_to_goal =
88 ((end_x - self.target_x).powi(2) + (end_y - self.target_y).powi(2)).sqrt();
89 if distance_to_goal < goal_threshold {
90 return true;
91 }
92
93 self.theta1 += KP * self.angle_diff(theta1_goal, self.theta1) * DT;
95 self.theta2 += KP * self.angle_diff(theta2_goal, self.theta2) * DT;
96
97 iterations += 1;
98 }
99
100 false }
102
103 pub fn inverse_kinematics(x: f64, y: f64, branch: ElbowBranch) -> Option<(f64, f64)> {
109 if !x.is_finite() || !y.is_finite() || x.hypot(y) > L1 + L2 {
110 return None;
111 }
112 let cos_theta2 = (x.powi(2) + y.powi(2) - L1.powi(2) - L2.powi(2)) / (2.0 * L1 * L2);
113 let sign = if branch == ElbowBranch::Positive {
114 1.0
115 } else {
116 -1.0
117 };
118 let theta2 = sign * cos_theta2.clamp(-1.0, 1.0).acos();
119 let theta1 = if x == 0.0 && y == 0.0 {
120 0.0
121 } else {
122 y.atan2(x) - (L2 * theta2.sin()).atan2(L1 + L2 * theta2.cos())
123 };
124 Some((theta1, theta2))
125 }
126
127 fn calculate_target_angles(&self) -> Option<(f64, f64)> {
128 let positive =
129 Self::inverse_kinematics(self.target_x, self.target_y, ElbowBranch::Positive)?;
130 if positive.0 < 0.0 {
131 Self::inverse_kinematics(self.target_x, self.target_y, ElbowBranch::Negative)
132 } else {
133 Some(positive)
134 }
135 }
136
137 pub fn forward_kinematics(&self) -> (f64, f64) {
138 let elbow_x = L1 * self.theta1.cos();
139 let elbow_y = L1 * self.theta1.sin();
140
141 let end_x = elbow_x + L2 * (self.theta1 + self.theta2).cos();
142 let end_y = elbow_y + L2 * (self.theta1 + self.theta2).sin();
143
144 (end_x, end_y)
145 }
146
147 pub fn jacobian(&self) -> [[f64; 2]; 2] {
150 let angle = self.theta1 + self.theta2;
151 [
152 [
153 -L1 * self.theta1.sin() - L2 * angle.sin(),
154 -L2 * angle.sin(),
155 ],
156 [L1 * self.theta1.cos() + L2 * angle.cos(), L2 * angle.cos()],
157 ]
158 }
159
160 pub fn damped_joint_velocity(&self, velocity: [f64; 2], damping: f64) -> Option<[f64; 2]> {
167 if !damping.is_finite() || damping <= 0.0 || velocity.iter().any(|v| !v.is_finite()) {
168 return None;
169 }
170 let j = self.jacobian();
171 if j.iter().flatten().any(|v| !v.is_finite()) {
172 return None;
173 }
174 let a = j[0][0] * j[0][0] + j[0][1] * j[0][1] + damping * damping;
175 let b = j[0][0] * j[1][0] + j[0][1] * j[1][1];
176 let c = j[1][0] * j[1][0] + j[1][1] * j[1][1] + damping * damping;
177 let det = a * c - b * b;
178 if !det.is_finite() || det <= 0.0 {
179 return None;
180 }
181 let x = (c * velocity[0] - b * velocity[1]) / det;
182 let y = (a * velocity[1] - b * velocity[0]) / det;
183 let rates = [j[0][0] * x + j[1][0] * y, j[0][1] * x + j[1][1] * y];
184 rates.iter().all(|v| v.is_finite()).then_some(rates)
185 }
186
187 fn angle_diff(&self, theta1: f64, theta2: f64) -> f64 {
188 let mut diff = theta1 - theta2;
189 while diff > PI {
190 diff -= 2.0 * PI;
191 }
192 while diff < -PI {
193 diff += 2.0 * PI;
194 }
195 diff
196 }
197}
198
199#[cfg(feature = "viz")]
201impl TwoJointArm {
202 pub fn save_animation_frames(&self, target_name: &str) {
203 let output_dir = format!("img/arm_navigation/{}", target_name);
204 std::fs::create_dir_all(&output_dir).unwrap();
205
206 for (frame, &(theta1, theta2, end_x, end_y)) in self.trajectory.iter().enumerate() {
207 if frame % 5 == 0 {
208 self.save_frame(frame / 5, theta1, theta2, end_x, end_y, &output_dir);
210 }
211 }
212
213 println!("Animation frames saved in {}/frame_*.png", output_dir);
214 println!("Create video with: ffmpeg -r 10 -i {}/frame_%04d.png -c:v libx264 -pix_fmt yuv420p {}_animation.mp4", output_dir, target_name);
215 }
216
217 fn save_frame(
218 &self,
219 frame: usize,
220 theta1: f64,
221 _theta2: f64,
222 end_x: f64,
223 end_y: f64,
224 output_dir: &str,
225 ) {
226 let mut fg = Figure::new();
227 {
228 let axes = fg
229 .axes2d()
230 .set_title(
231 &format!(
232 "Two Joint Arm Control - Frame {} (Target: {:.2}, {:.2})",
233 frame, self.target_x, self.target_y
234 ),
235 &[],
236 )
237 .set_x_label("X [m]", &[])
238 .set_y_label("Y [m]", &[])
239 .set_x_range(gnuplot::Fix(-2.5), gnuplot::Fix(2.5))
240 .set_y_range(gnuplot::Fix(-2.5), gnuplot::Fix(2.5))
241 .set_aspect_ratio(gnuplot::Fix(1.0));
242
243 let shoulder = (0.0, 0.0);
245 let elbow = (L1 * theta1.cos(), L1 * theta1.sin());
246 let wrist = (end_x, end_y);
247
248 axes.lines(
250 &[shoulder.0, elbow.0],
251 &[shoulder.1, elbow.1],
252 &[Color("black")],
253 );
254 axes.lines(&[elbow.0, wrist.0], &[elbow.1, wrist.1], &[Color("black")]);
255
256 axes.points(
258 &[shoulder.0, elbow.0, wrist.0],
259 &[shoulder.1, elbow.1, wrist.1],
260 &[Color("red"), PointSymbol('O')],
261 );
262
263 axes.points(
265 &[self.target_x],
266 &[self.target_y],
267 &[Color("green"), PointSymbol('*')],
268 );
269
270 axes.lines(
272 &[wrist.0, self.target_x],
273 &[wrist.1, self.target_y],
274 &[Color("green")],
275 );
276 }
277
278 let output_path = format!("{}/frame_{:04}.png", output_dir, frame);
279 fg.set_terminal("pngcairo", &output_path);
280 fg.show().unwrap();
281 }
282
283 pub fn save_arm_plot(&self, output_path: &str) {
285 let (end_x, end_y) = self.forward_kinematics();
286
287 let mut fg = Figure::new();
288 {
289 let axes = fg
290 .axes2d()
291 .set_title("Two Joint Arm to Point Control", &[])
292 .set_x_label("X [m]", &[])
293 .set_y_label("Y [m]", &[])
294 .set_x_range(gnuplot::Fix(-2.5), gnuplot::Fix(2.5))
295 .set_y_range(gnuplot::Fix(-2.5), gnuplot::Fix(2.5))
296 .set_aspect_ratio(gnuplot::Fix(1.0));
297
298 let shoulder = (0.0, 0.0);
300 let elbow = (L1 * self.theta1.cos(), L1 * self.theta1.sin());
301 let wrist = (end_x, end_y);
302
303 axes.lines(
305 &[shoulder.0, elbow.0],
306 &[shoulder.1, elbow.1],
307 &[Color("black")],
308 );
309 axes.lines(&[elbow.0, wrist.0], &[elbow.1, wrist.1], &[Color("black")]);
310
311 axes.points(
313 &[shoulder.0, elbow.0, wrist.0],
314 &[shoulder.1, elbow.1, wrist.1],
315 &[Color("red"), PointSymbol('O')],
316 );
317
318 axes.points(
320 &[self.target_x],
321 &[self.target_y],
322 &[Color("green"), PointSymbol('*')],
323 );
324
325 axes.lines(
327 &[wrist.0, self.target_x],
328 &[wrist.1, self.target_y],
329 &[Color("green")],
330 );
331 }
332
333 fg.set_terminal("pngcairo", output_path);
334 fg.show().unwrap();
335 println!("Arm plot saved to: {}", output_path);
336 }
337
338 pub fn create_summary_plot(&self, target_name: &str) {
339 let mut fg = Figure::new();
340 {
341 let axes = fg
342 .axes2d()
343 .set_title("Two Joint Arm - Joint Angles vs Time", &[])
344 .set_x_label("Iteration", &[])
345 .set_y_label("Angle [rad]", &[]);
346
347 let iterations: Vec<f64> = (0..self.trajectory.len()).map(|i| i as f64).collect();
348 let theta1_data: Vec<f64> = self.trajectory.iter().map(|(t1, _, _, _)| *t1).collect();
349 let theta2_data: Vec<f64> = self.trajectory.iter().map(|(_, t2, _, _)| *t2).collect();
350
351 axes.lines(
352 &iterations,
353 &theta1_data,
354 &[Caption("Theta1 [rad]"), Color("blue")],
355 );
356 axes.lines(
357 &iterations,
358 &theta2_data,
359 &[Caption("Theta2 [rad]"), Color("red")],
360 );
361 }
362
363 let output_path = format!("img/arm_navigation/{}_summary.png", target_name);
364 std::fs::create_dir_all("img/arm_navigation").unwrap();
365 fg.set_terminal("pngcairo", &output_path);
366 fg.show().unwrap();
367 println!("Summary plot saved to: {}", output_path);
368 }
369
370 pub fn run_random_targets_demo(&mut self, num_targets: usize) {
371 use rand::Rng;
372 let mut rng = rand::rng();
373
374 println!("Starting Two Joint Arm random targets demo...");
375
376 for i in 0..num_targets {
377 let angle = rng.random::<f64>() * 2.0 * PI;
379 let radius = rng.random::<f64>() * (L1 + L2 - 0.1); let target_x = radius * angle.cos();
381 let target_y = radius * angle.sin();
382
383 self.set_target(target_x, target_y);
384
385 println!("Target {}: ({:.2}, {:.2})", i + 1, target_x, target_y);
386
387 if self.solve_inverse_kinematics(0.01) {
388 println!(" Reached target in {} iterations", self.trajectory.len());
389 let target_name = format!("target_{:02}", i + 1);
390 self.save_animation_frames(&target_name);
391 } else {
392 println!(" Failed to reach target");
393 }
394 }
395
396 self.create_summary_plot("random_demo");
397 println!("Two Joint Arm demo complete!");
398 }
399}
400
401#[cfg(test)]
402mod tests {
403 use super::*;
404
405 #[test]
406 fn test_forward_kinematics_at_origin() {
407 let arm = TwoJointArm::new();
408 let (x, y) = arm.forward_kinematics();
409 assert!((x - 2.0).abs() < 1e-10); assert!(y.abs() < 1e-10);
411 }
412
413 #[test]
414 fn test_solve_ik_reaches_target() {
415 let mut arm = TwoJointArm::new();
416 arm.set_target(1.5, 0.5);
417 assert!(arm.solve_inverse_kinematics(0.01));
418
419 let (end_x, end_y) = arm.forward_kinematics();
420 let distance = ((end_x - 1.5).powi(2) + (end_y - 0.5).powi(2)).sqrt();
421 assert!(distance < 0.01);
422 }
423
424 #[test]
425 fn both_analytic_branches_reach_targets_in_every_quadrant() {
426 for radius in [0.0, 0.001, 0.5, 1.3, 1.999, 2.0] {
427 for direction in 0..32 {
428 let angle = f64::from(direction) * 2.0 * PI / 32.0;
429 let (x, y) = (radius * angle.cos(), radius * angle.sin());
430 let scale = if x.hypot(y) > 2.0 {
432 2.0 / x.hypot(y)
433 } else {
434 1.0
435 };
436 for branch in [ElbowBranch::Positive, ElbowBranch::Negative] {
437 let (theta1, theta2) =
438 TwoJointArm::inverse_kinematics(x * scale, y * scale, branch).unwrap();
439 let mut arm = TwoJointArm::new();
440 arm.theta1 = theta1;
441 arm.theta2 = theta2;
442 let (px, py) = arm.forward_kinematics();
443 assert!((px - x * scale).hypot(py - y * scale) < 1e-9);
444 assert_eq!(theta2.is_sign_positive(), branch == ElbowBranch::Positive);
445 }
446 }
447 }
448 }
449
450 #[test]
451 fn unreachable_or_invalid_targets_terminate_without_moving() {
452 for (x, y) in [
453 (2.01, 0.0),
454 (0.0, -3.0),
455 (f64::NAN, 1.0),
456 (f64::INFINITY, 0.0),
457 ] {
458 assert!(TwoJointArm::inverse_kinematics(x, y, ElbowBranch::Positive).is_none());
459 assert!(TwoJointArm::inverse_kinematics(x, y, ElbowBranch::Negative).is_none());
460 let mut arm = TwoJointArm::new();
461 arm.set_target(x, y);
462 assert!(!arm.solve_inverse_kinematics(0.01));
463 assert_eq!((arm.theta1, arm.theta2), (0.0, 0.0));
464 assert_eq!(arm.trajectory.len(), 1);
465 }
466 for threshold in [0.0, -1.0, f64::NAN, f64::INFINITY] {
467 assert!(!TwoJointArm::new().solve_inverse_kinematics(threshold));
468 }
469 for (a, b) in [
470 (f64::INFINITY, 0.0),
471 (0.0, f64::NEG_INFINITY),
472 (f64::NAN, 1.0),
473 ] {
474 let mut arm = TwoJointArm::new();
475 arm.theta1 = a;
476 arm.theta2 = b;
477 assert!(!arm.solve_inverse_kinematics(0.01));
478 assert!(arm.trajectory.is_empty());
479 }
480 }
481
482 #[test]
483 fn jacobian_matches_central_differences_and_dls_normal_equations() {
484 for i in -8..=8 {
485 for k in -8..=8 {
486 let mut arm = TwoJointArm::new();
487 arm.theta1 = f64::from(i) * PI / 8.0;
488 arm.theta2 = f64::from(k) * PI / 8.0;
489 let j = arm.jacobian();
490 let eps = 1e-6;
491 for (column, (&jx, &jy)) in j[0].iter().zip(&j[1]).enumerate() {
492 if column == 0 {
493 arm.theta1 += eps;
494 } else {
495 arm.theta2 += eps;
496 }
497 let plus = arm.forward_kinematics();
498 if column == 0 {
499 arm.theta1 -= 2.0 * eps;
500 } else {
501 arm.theta2 -= 2.0 * eps;
502 }
503 let minus = arm.forward_kinematics();
504 if column == 0 {
505 arm.theta1 += eps;
506 } else {
507 arm.theta2 += eps;
508 }
509 assert!(((plus.0 - minus.0) / (2.0 * eps) - jx).abs() < 2e-9);
510 assert!(((plus.1 - minus.1) / (2.0 * eps) - jy).abs() < 2e-9);
511 }
512 for damping in [0.02, 0.2, 0.6, 1.0] {
513 let v = [0.6, -0.4];
514 let q = arm.damped_joint_velocity(v, damping).unwrap();
515 let actual = [
516 j[0][0] * q[0] + j[0][1] * q[1],
517 j[1][0] * q[0] + j[1][1] * q[1],
518 ];
519 for (column, rate) in q.iter().enumerate() {
520 let residual = j[0][column] * (actual[0] - v[0])
521 + j[1][column] * (actual[1] - v[1])
522 + damping * damping * rate;
523 assert!(residual.abs() < 1e-10);
524 }
525 }
526 }
527 }
528 }
529
530 #[test]
531 fn damping_keeps_singular_velocity_finite_but_does_not_invent_radial_motion() {
532 let arm = TwoJointArm::new();
533 assert_eq!(arm.damped_joint_velocity([1.0, 0.0], 0.2), Some([0.0, 0.0]));
534 let rates = arm.damped_joint_velocity([0.0, 1.0], 0.2).unwrap();
535 assert!((rates[0] - 2.0 / 5.04).abs() < 1e-12);
536 assert!((rates[1] - 1.0 / 5.04).abs() < 1e-12);
537 for damping in [0.0, -0.2, f64::NAN, f64::INFINITY] {
538 assert!(arm.damped_joint_velocity([0.0, 1.0], damping).is_none());
539 }
540 assert!(arm.damped_joint_velocity([f64::NAN, 0.0], 0.2).is_none());
541 }
542}