1#![allow(
2 dead_code,
3 clippy::needless_borrows_for_generic_args,
4 clippy::new_without_default
5)]
6
7const KP: f64 = 2.0;
16const DT: f64 = 0.1;
17const DEFAULT_N_ITERATIONS: usize = 10000;
18const DEFAULT_GOAL_THRESHOLD: f64 = 0.1;
19
20#[derive(Debug, Clone)]
22pub struct NLinkArm {
23 pub link_lengths: Vec<f64>,
25 pub joint_angles: Vec<f64>,
27 pub points: Vec<[f64; 2]>,
29 pub end_effector: [f64; 2],
31 pub n_links: usize,
33}
34
35impl NLinkArm {
36 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 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 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 pub fn workspace_radius(&self) -> f64 {
81 self.link_lengths.iter().sum()
82 }
83}
84
85pub 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
100fn 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
118fn pseudo_inverse_2xn(j: &[Vec<f64>]) -> Vec<Vec<f64>> {
121 let n = j[0].len();
122
123 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 let det = jjt[0][0] * jjt[1][1] - jjt[0][1] * jjt[1][0];
135 if det.abs() < 1e-12 {
136 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 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
157pub 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
164pub fn angle_mod(angle: f64) -> f64 {
166 rust_robotics_core::normalize_angle(angle)
167}
168
169fn 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
178pub 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
196pub 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(¤t_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 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
224pub 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 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 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 let links = vec![1.0, 1.0];
304 let initial = vec![0.3, 0.3]; let goal = [1.0, 1.0]; 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 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]; let initial = vec![0.1; n]; let goal = [3.0, 1.0]; 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 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}