Skip to main content

rust_robotics_control/
n_joint_arm_control.rs

1#![allow(
2    dead_code,
3    clippy::needless_borrows_for_generic_args,
4    clippy::new_without_default
5)]
6
7//! Generalized N-joint planar arm inverse kinematics and control.
8//!
9//! Uses the Jacobian pseudo-inverse method to solve inverse kinematics
10//! for a planar arm with an arbitrary number of revolute joints.
11//!
12//! Reference: PythonRobotics ArmNavigation/n\_joint\_arm\_to\_point\_control
13
14/// Simulation parameters
15const KP: f64 = 2.0;
16const DT: f64 = 0.1;
17const DEFAULT_N_ITERATIONS: usize = 10000;
18const DEFAULT_GOAL_THRESHOLD: f64 = 0.1;
19
20/// A planar N-link arm with forward and inverse kinematics.
21#[derive(Debug, Clone)]
22pub struct NLinkArm {
23    /// Length of each link
24    pub link_lengths: Vec<f64>,
25    /// Current joint angles \[rad\]
26    pub joint_angles: Vec<f64>,
27    /// Computed joint positions (n\_links + 1 points, including the base at origin)
28    pub points: Vec<[f64; 2]>,
29    /// End-effector position
30    pub end_effector: [f64; 2],
31    /// Number of links
32    pub n_links: usize,
33}
34
35impl NLinkArm {
36    /// Creates a new N-link arm.
37    ///
38    /// # Panics
39    ///
40    /// Panics if `link_lengths` and `joint_angles` have different lengths.
41    pub fn new(link_lengths: Vec<f64>, joint_angles: Vec<f64>) -> Self {
42        assert_eq!(
43            link_lengths.len(),
44            joint_angles.len(),
45            "link_lengths and joint_angles must have the same length"
46        );
47        let n_links = link_lengths.len();
48        let mut arm = NLinkArm {
49            link_lengths,
50            joint_angles,
51            points: vec![[0.0, 0.0]; n_links + 1],
52            end_effector: [0.0, 0.0],
53            n_links,
54        };
55        arm.update_points();
56        arm
57    }
58
59    /// Updates joint angles and recomputes link positions.
60    pub fn update_joints(&mut self, joint_angles: &[f64]) {
61        assert_eq!(joint_angles.len(), self.n_links);
62        self.joint_angles = joint_angles.to_vec();
63        self.update_points();
64    }
65
66    /// Recomputes all joint positions from current angles.
67    pub fn update_points(&mut self) {
68        self.points[0] = [0.0, 0.0];
69        for i in 1..=self.n_links {
70            let cumulative_angle: f64 = self.joint_angles[..i].iter().sum();
71            self.points[i][0] =
72                self.points[i - 1][0] + self.link_lengths[i - 1] * cumulative_angle.cos();
73            self.points[i][1] =
74                self.points[i - 1][1] + self.link_lengths[i - 1] * cumulative_angle.sin();
75        }
76        self.end_effector = self.points[self.n_links];
77    }
78
79    /// Total workspace radius (sum of all link lengths).
80    pub fn workspace_radius(&self) -> f64 {
81        self.link_lengths.iter().sum()
82    }
83}
84
85/// Computes the forward kinematics for given link lengths and joint angles.
86///
87/// Returns the end-effector position \[x, y\].
88pub fn forward_kinematics(link_lengths: &[f64], joint_angles: &[f64]) -> [f64; 2] {
89    let n = link_lengths.len();
90    let mut x = 0.0;
91    let mut y = 0.0;
92    for i in 1..=n {
93        let cumulative_angle: f64 = joint_angles[..i].iter().sum();
94        x += link_lengths[i - 1] * cumulative_angle.cos();
95        y += link_lengths[i - 1] * cumulative_angle.sin();
96    }
97    [x, y]
98}
99
100/// Computes the Jacobian matrix for the planar arm.
101///
102/// Returns a 2-by-n matrix stored as `[row0, row1]` where each row has `n` elements.
103/// The Jacobian relates joint velocities to end-effector velocity.
104fn jacobian(link_lengths: &[f64], joint_angles: &[f64]) -> Vec<Vec<f64>> {
105    let n = link_lengths.len();
106    let mut j0 = vec![0.0; n];
107    let mut j1 = vec![0.0; n];
108    for (i, (j0_val, j1_val)) in j0.iter_mut().zip(j1.iter_mut()).enumerate() {
109        for k in i..n {
110            let cumulative_angle: f64 = joint_angles[..=k].iter().sum();
111            *j0_val -= link_lengths[k] * cumulative_angle.sin();
112            *j1_val += link_lengths[k] * cumulative_angle.cos();
113        }
114    }
115    vec![j0, j1]
116}
117
118/// Computes the pseudo-inverse of a 2-by-n matrix using the formula:
119/// `J^+ = J^T (J J^T)^{-1}`
120fn pseudo_inverse_2xn(j: &[Vec<f64>]) -> Vec<Vec<f64>> {
121    let n = j[0].len();
122
123    // Compute J * J^T (2x2 matrix)
124    let mut jjt = [[0.0f64; 2]; 2];
125    for row in 0..2 {
126        for col in 0..2 {
127            for (jr, jc) in j[row].iter().zip(j[col].iter()) {
128                jjt[row][col] += jr * jc;
129            }
130        }
131    }
132
133    // Invert the 2x2 matrix
134    let det = jjt[0][0] * jjt[1][1] - jjt[0][1] * jjt[1][0];
135    if det.abs() < 1e-12 {
136        // Near-singular: return zero matrix (damped least squares fallback)
137        return vec![vec![0.0; 2]; n];
138    }
139    let inv_det = 1.0 / det;
140    let inv = [
141        [jjt[1][1] * inv_det, -jjt[0][1] * inv_det],
142        [-jjt[1][0] * inv_det, jjt[0][0] * inv_det],
143    ];
144
145    // J^T * (J J^T)^{-1}  => n x 2
146    let mut result = vec![vec![0.0; 2]; n];
147    for (i, result_row) in result.iter_mut().enumerate().take(n) {
148        for (col, result_cell) in result_row.iter_mut().enumerate().take(2) {
149            for k in 0..2 {
150                *result_cell += j[k][i] * inv[k][col];
151            }
152        }
153    }
154    result
155}
156
157/// Computes the distance and error vector from current position to goal.
158pub fn distance_to_goal(current: &[f64; 2], goal: &[f64; 2]) -> ([f64; 2], f64) {
159    let dx = goal[0] - current[0];
160    let dy = goal[1] - current[1];
161    ([dx, dy], (dx * dx + dy * dy).sqrt())
162}
163
164/// Normalizes an angle to the range \[-pi, pi\].
165pub fn angle_mod(angle: f64) -> f64 {
166    rust_robotics_core::normalize_angle(angle)
167}
168
169/// Computes element-wise angle difference, each wrapped to \[-pi, pi\].
170fn ang_diff(theta1: &[f64], theta2: &[f64]) -> Vec<f64> {
171    theta1
172        .iter()
173        .zip(theta2.iter())
174        .map(|(a, b)| angle_mod(a - b))
175        .collect()
176}
177
178/// Solves inverse kinematics using the Jacobian pseudo-inverse method.
179///
180/// Returns `Some(joint_angles)` if a solution is found within tolerance,
181/// or `None` if the maximum number of iterations is exceeded.
182pub fn inverse_kinematics(
183    link_lengths: &[f64],
184    initial_joint_angles: &[f64],
185    goal_pos: &[f64; 2],
186) -> Option<Vec<f64>> {
187    inverse_kinematics_with_params(
188        link_lengths,
189        initial_joint_angles,
190        goal_pos,
191        DEFAULT_N_ITERATIONS,
192        DEFAULT_GOAL_THRESHOLD,
193    )
194}
195
196/// Solves inverse kinematics with configurable iteration limit and threshold.
197pub fn inverse_kinematics_with_params(
198    link_lengths: &[f64],
199    initial_joint_angles: &[f64],
200    goal_pos: &[f64; 2],
201    max_iterations: usize,
202    goal_threshold: f64,
203) -> Option<Vec<f64>> {
204    let mut joint_angles = initial_joint_angles.to_vec();
205
206    for _iteration in 0..max_iterations {
207        let current_pos = forward_kinematics(link_lengths, &joint_angles);
208        let (errors, distance) = distance_to_goal(&current_pos, goal_pos);
209        if distance < goal_threshold {
210            return Some(joint_angles);
211        }
212
213        let j = jacobian(link_lengths, &joint_angles);
214        let j_pinv = pseudo_inverse_2xn(&j);
215
216        // joint_angles += J_pinv * errors
217        for i in 0..joint_angles.len() {
218            joint_angles[i] += j_pinv[i][0] * errors[0] + j_pinv[i][1] * errors[1];
219        }
220    }
221    None
222}
223
224/// Runs the proportional-control IK loop: smoothly moves from current angles
225/// toward goal angles.
226///
227/// Returns the trajectory of joint angle snapshots.
228pub fn move_to_goal(
229    arm: &mut NLinkArm,
230    goal_angles: &[f64],
231    goal_pos: &[f64; 2],
232    goal_threshold: f64,
233) -> Vec<Vec<f64>> {
234    let mut trajectory = Vec::new();
235
236    for _ in 0..10000 {
237        let (_, distance) = distance_to_goal(&arm.end_effector, goal_pos);
238        if distance < goal_threshold {
239            break;
240        }
241        let diff = ang_diff(goal_angles, &arm.joint_angles);
242        let new_angles: Vec<f64> = arm
243            .joint_angles
244            .iter()
245            .zip(diff.iter())
246            .map(|(a, d)| a + KP * d * DT)
247            .collect();
248        arm.update_joints(&new_angles);
249        trajectory.push(arm.joint_angles.clone());
250    }
251    trajectory
252}
253
254#[cfg(test)]
255mod tests {
256    use super::*;
257    use std::f64::consts::FRAC_PI_4;
258    use std::f64::consts::PI;
259
260    #[test]
261    fn test_forward_kinematics_straight() {
262        let links = vec![1.0, 1.0, 1.0];
263        let angles = vec![0.0, 0.0, 0.0];
264        let [x, y] = forward_kinematics(&links, &angles);
265        assert!((x - 3.0).abs() < 1e-10);
266        assert!(y.abs() < 1e-10);
267    }
268
269    #[test]
270    fn test_forward_kinematics_bent() {
271        let links = vec![1.0, 1.0];
272        let angles = vec![0.0, FRAC_PI_4];
273        let [x, y] = forward_kinematics(&links, &angles);
274        // link1 at angle 0: (1, 0)
275        // link2 at cumulative angle pi/4: (1 + cos(pi/4), sin(pi/4))
276        let expected_x = 1.0 + (FRAC_PI_4).cos();
277        let expected_y = (FRAC_PI_4).sin();
278        assert!((x - expected_x).abs() < 1e-10);
279        assert!((y - expected_y).abs() < 1e-10);
280    }
281
282    #[test]
283    fn test_nlink_arm_new() {
284        let arm = NLinkArm::new(vec![1.0, 1.0], vec![0.0, 0.0]);
285        assert_eq!(arm.n_links, 2);
286        assert!((arm.end_effector[0] - 2.0).abs() < 1e-10);
287        assert!(arm.end_effector[1].abs() < 1e-10);
288        assert_eq!(arm.points.len(), 3);
289    }
290
291    #[test]
292    fn test_nlink_arm_update_joints() {
293        let mut arm = NLinkArm::new(vec![1.0, 1.0], vec![0.0, 0.0]);
294        arm.update_joints(&[PI / 2.0, 0.0]);
295        // Both links point up
296        assert!(arm.end_effector[0].abs() < 1e-10);
297        assert!((arm.end_effector[1] - 2.0).abs() < 1e-10);
298    }
299
300    #[test]
301    fn test_inverse_kinematics_reachable() {
302        // Use a 2-link arm with initial angles that break symmetry
303        let links = vec![1.0, 1.0];
304        let initial = vec![0.3, 0.3]; // avoid singular straight config
305        let goal = [1.0, 1.0]; // well within reach (max reach = 2.0)
306
307        let result = inverse_kinematics(&links, &initial, &goal);
308        assert!(result.is_some(), "IK should converge for reachable target");
309
310        let angles = result.unwrap();
311        let pos = forward_kinematics(&links, &angles);
312        let dist = ((pos[0] - goal[0]).powi(2) + (pos[1] - goal[1]).powi(2)).sqrt();
313        assert!(dist < DEFAULT_GOAL_THRESHOLD);
314    }
315
316    #[test]
317    fn test_inverse_kinematics_unreachable() {
318        let links = vec![1.0, 1.0];
319        let initial = vec![0.0, 0.0];
320        // Way outside workspace radius of 2.0
321        let goal = [10.0, 10.0];
322
323        let result = inverse_kinematics_with_params(&links, &initial, &goal, 500, 0.01);
324        assert!(result.is_none());
325    }
326
327    #[test]
328    fn test_angle_mod() {
329        assert!((angle_mod(0.0)).abs() < 1e-10);
330        assert!((angle_mod(2.0 * PI) - 0.0).abs() < 1e-10);
331        assert!((angle_mod(PI + 0.1) - (-PI + 0.1)).abs() < 1e-10);
332        assert!((angle_mod(-PI - 0.1) - (PI - 0.1)).abs() < 1e-10);
333    }
334
335    #[test]
336    fn test_distance_to_goal() {
337        let (errors, dist) = distance_to_goal(&[0.0, 0.0], &[3.0, 4.0]);
338        assert!((errors[0] - 3.0).abs() < 1e-10);
339        assert!((errors[1] - 4.0).abs() < 1e-10);
340        assert!((dist - 5.0).abs() < 1e-10);
341    }
342
343    #[test]
344    fn test_jacobian_shape() {
345        let links = vec![1.0, 1.0, 1.0];
346        let angles = vec![0.0, 0.0, 0.0];
347        let j = jacobian(&links, &angles);
348        assert_eq!(j.len(), 2);
349        assert_eq!(j[0].len(), 3);
350        assert_eq!(j[1].len(), 3);
351    }
352
353    #[test]
354    fn test_move_to_goal_converges() {
355        let mut arm = NLinkArm::new(vec![1.0, 1.0], vec![0.5, 0.5]);
356        let goal = [1.5, 0.5];
357        let goal_angles =
358            inverse_kinematics(&arm.link_lengths.clone(), &arm.joint_angles.clone(), &goal)
359                .unwrap();
360        let traj = move_to_goal(&mut arm, &goal_angles, &goal, 0.1);
361        assert!(!traj.is_empty());
362        let (_, dist) = distance_to_goal(&arm.end_effector, &goal);
363        assert!(dist < 0.2);
364    }
365
366    #[test]
367    fn test_10_link_arm() {
368        let n = 10;
369        let links = vec![0.5; n]; // total reach = 5.0
370        let initial = vec![0.1; n]; // slight spread to avoid singular configuration
371        let goal = [3.0, 1.0]; // well within reach
372
373        let result = inverse_kinematics(&links, &initial, &goal);
374        assert!(result.is_some(), "10-link IK should converge");
375
376        let angles = result.unwrap();
377        let pos = forward_kinematics(&links, &angles);
378        let dist = ((pos[0] - goal[0]).powi(2) + (pos[1] - goal[1]).powi(2)).sqrt();
379        assert!(dist < DEFAULT_GOAL_THRESHOLD);
380    }
381
382    #[test]
383    fn test_workspace_radius() {
384        let arm = NLinkArm::new(vec![1.0, 2.0, 0.5], vec![0.0, 0.0, 0.0]);
385        assert!((arm.workspace_radius() - 3.5).abs() < 1e-10);
386    }
387
388    #[test]
389    fn test_pseudo_inverse_identity_like() {
390        // For a 2x2 non-singular Jacobian, pinv should be close to the true inverse
391        let j = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
392        let pinv = pseudo_inverse_2xn(&j);
393        assert!((pinv[0][0] - 1.0).abs() < 1e-10);
394        assert!((pinv[0][1] - 0.0).abs() < 1e-10);
395        assert!((pinv[1][0] - 0.0).abs() < 1e-10);
396        assert!((pinv[1][1] - 1.0).abs() < 1e-10);
397    }
398}