Skip to main content

rust_robotics_control/
two_joint_arm_control.rs

1#![allow(
2    dead_code,
3    clippy::needless_borrows_for_generic_args,
4    clippy::new_without_default
5)]
6
7//
8// Inverse kinematics of a two-joint arm
9// Author: Daniel Ingram (daniel-s-ingram)
10//         Atsushi Sakai (@Atsushi_twi)
11// Ported to Rust by: rust_robotics team
12//
13
14#[cfg(feature = "viz")]
15use gnuplot::{AxesCommon, Caption, Color, Figure, PointSymbol};
16
17use std::f64::consts::PI;
18
19// Simulation parameters
20const KP: f64 = 15.0;
21const DT: f64 = 0.01;
22
23// Link lengths
24const L1: f64 = 1.0;
25const L2: f64 = 1.0;
26
27/// The sign of the elbow angle in the two analytic IK solutions.
28#[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)>, // (theta1, theta2, end_x, end_y)
40}
41
42impl TwoJointArm {
43    /// Link lengths in metres; this planar arm has unrestricted revolute joints.
44    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            // Validate reachability before applying the numerical goal tolerance.
81            let (theta1_goal, theta2_goal) = match self.calculate_target_angles() {
82                Some(angles) => angles,
83                None => return false,
84            };
85
86            // Check if goal is reached
87            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            // Update joint angles with proportional control
94            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 // Failed to reach target within max iterations
101    }
102
103    /// Solve one analytic IK branch without moving the arm.
104    ///
105    /// Returns `None` for nonfinite targets or targets outside the reachable
106    /// disk. At the origin both unit links fold; the shoulder angle is
107    /// underdetermined, so zero is returned as a deterministic representative.
108    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    /// Cartesian tip velocity is `J * [shoulder_rate, elbow_rate]`.
148    /// Rows are x/y and columns are shoulder/elbow; link lengths are metres.
149    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    /// Damped least-squares differential IK: `dq = J^T (J J^T + λ² I)^-1 v`.
161    ///
162    /// Rates are rad/s for a requested velocity in m/s. No speed limit is
163    /// applied. Returns `None` for nonfinite inputs, nonpositive damping, or
164    /// an unrepresentable solve. Damping regularizes the matrix but cannot
165    /// create motion in a Cartesian direction absent from the Jacobian.
166    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// Visualization methods gated behind the "viz" feature
200#[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                // Save every 5th frame
209                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            // Calculate joint positions
244            let shoulder = (0.0, 0.0);
245            let elbow = (L1 * theta1.cos(), L1 * theta1.sin());
246            let wrist = (end_x, end_y);
247
248            // Draw arm links (black lines like Python version)
249            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            // Draw joints (red circles like Python version)
257            axes.points(
258                &[shoulder.0, elbow.0, wrist.0],
259                &[shoulder.1, elbow.1, wrist.1],
260                &[Color("red"), PointSymbol('O')],
261            );
262
263            // Draw target (green star like Python version)
264            axes.points(
265                &[self.target_x],
266                &[self.target_y],
267                &[Color("green"), PointSymbol('*')],
268            );
269
270            // Draw line to target (green dashed line like Python version)
271            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    /// Save a single arm visualization image (like PythonRobotics)
284    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            // Calculate joint positions
299            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            // Draw arm links (black lines like Python version)
304            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            // Draw joints (red circles like Python version)
312            axes.points(
313                &[shoulder.0, elbow.0, wrist.0],
314                &[shoulder.1, elbow.1, wrist.1],
315                &[Color("red"), PointSymbol('O')],
316            );
317
318            // Draw target (green star like Python version)
319            axes.points(
320                &[self.target_x],
321                &[self.target_y],
322                &[Color("green"), PointSymbol('*')],
323            );
324
325            // Draw line to target (green dashed line like Python version)
326            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            // Generate random target within workspace
378            let angle = rng.random::<f64>() * 2.0 * PI;
379            let radius = rng.random::<f64>() * (L1 + L2 - 0.1); // Slightly inside workspace
380            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); // L1 + L2
410        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                // Keep exact boundary targets within the disk despite roundoff.
431                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}