symtropy_physics/
articulation.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: AGPL-3.0-or-later
3//! Articulated chain builder for serial kinematic chains.
4//!
5//! Constructs a series of rigid bodies connected by hinge joints with
6//! optional motor drives. Used by robotics platforms (manipulator arm,
7//! quadruped legs, exoskeleton limbs).
8
9use nalgebra::SVector;
10use symtropy_math::Point;
11
12use crate::body::{BodyHandle, RigidBody};
13use crate::joints::{HingeJoint, MotorDrive};
14use crate::world::PhysicsWorld;
15
16/// Specification for a single link in an articulated chain.
17#[derive(Clone, Debug)]
18pub struct LinkSpec {
19    pub mass: f64,
20    pub length: f64,
21    pub radius: f64,
22    pub plane_a: usize,
23    pub plane_b: usize,
24    pub angle_limits: Option<(f64, f64)>,
25    pub motor_max_force: Option<f64>,
26    pub motor_damping: Option<f64>,
27}
28
29impl Default for LinkSpec {
30    fn default() -> Self {
31        Self {
32            mass: 1.0, length: 0.3, radius: 0.03,
33            plane_a: 0, plane_b: 2,
34            angle_limits: Some((-2.9, 2.9)),
35            motor_max_force: Some(50.0),
36            motor_damping: None,
37        }
38    }
39}
40
41/// An articulated chain in a physics world.
42#[derive(Debug)]
43pub struct ArticulatedChain {
44    pub base: BodyHandle,
45    pub links: Vec<BodyHandle>,
46    pub num_joints: usize,
47}
48
49impl ArticulatedChain {
50    /// Read approximate joint angles from relative body positions.
51    pub fn read_joint_states(&self, world: &PhysicsWorld<3>) -> Vec<(f64, f64)> {
52        let mut states = Vec::with_capacity(self.num_joints);
53        let mut prev = self.base;
54        for &link in &self.links {
55            let (angle, vel) = match (world.body(prev), world.body(link)) {
56                (Some(a), Some(b)) => {
57                    let d = b.transform.translation.0 - a.transform.translation.0;
58                    (d[2].atan2(d[0]), b.angular_velocity.get(0, 2) - a.angular_velocity.get(0, 2))
59                }
60                _ => (0.0, 0.0),
61            };
62            states.push((angle, vel));
63            prev = link;
64        }
65        states
66    }
67
68    /// Get the tip (last link) body handle.
69    pub fn tip(&self) -> BodyHandle {
70        *self.links.last().unwrap_or(&self.base)
71    }
72}
73
74/// Builder for articulated chains.
75pub struct ChainBuilder {
76    base_pos: Point<3>,
77    links: Vec<LinkSpec>,
78}
79
80impl ChainBuilder {
81    pub fn new() -> Self {
82        Self { base_pos: Point::origin(), links: Vec::new() }
83    }
84
85    pub fn base_position(mut self, pos: Point<3>) -> Self {
86        self.base_pos = pos;
87        self
88    }
89
90    pub fn add_link(mut self, spec: LinkSpec) -> Self {
91        self.links.push(spec);
92        self
93    }
94
95    /// Build the chain in the physics world.
96    pub fn build(self, world: &mut PhysicsWorld<3>) -> ArticulatedChain {
97        // Static base (small sphere)
98        let base = world.add_body(RigidBody::static_body(
99            BodyHandle(0), // overwritten by add_body
100            self.base_pos.clone(),
101            Box::new(symtropy_math::Sphere::new(Point::origin(), 0.05)),
102        ));
103
104        let mut links = Vec::with_capacity(self.links.len());
105        let mut prev = base;
106        let mut pos = self.base_pos.0;
107
108        for spec in &self.links {
109            pos[2] -= spec.length;
110            let link_pos = Point::new([pos[0], pos[1], pos[2]]);
111            let handle = world.add_body(RigidBody::dynamic_sphere(
112                BodyHandle(0), link_pos, spec.radius.max(0.01), spec.mass,
113            ));
114
115            let anchor_a: SVector<f64, 3> = SVector::from([0.0, 0.0, -spec.length * 0.5]);
116            let anchor_b: SVector<f64, 3> = SVector::from([0.0, 0.0, spec.length * 0.5]);
117
118            let mut hinge = HingeJoint::with_anchors(
119                prev, handle, anchor_a, anchor_b, spec.plane_a, spec.plane_b,
120            );
121            if let Some((min, max)) = spec.angle_limits {
122                hinge = hinge.with_limits(min, max);
123            }
124            if let Some(max_force) = spec.motor_max_force {
125                let mut motor = MotorDrive::new(0.0, max_force);
126                if let Some(d) = spec.motor_damping { motor.damping = d; }
127                hinge = hinge.with_motor(motor);
128            }
129
130            world.add_constraint(Box::new(hinge));
131            links.push(handle);
132            prev = handle;
133        }
134
135        ArticulatedChain { base, links, num_joints: self.links.len() }
136    }
137}
138
139impl Default for ChainBuilder {
140    fn default() -> Self { Self::new() }
141}
142
143#[cfg(test)]
144mod tests {
145    use super::*;
146
147    fn world_with_gravity() -> PhysicsWorld<3> {
148        PhysicsWorld::new(SVector::from([0.0, 0.0, -9.81]))
149    }
150
151    #[test]
152    fn test_single_link_pendulum() {
153        let mut world = world_with_gravity();
154        let chain = ChainBuilder::new()
155            .base_position(Point::new([0.0, 0.0, 2.0]))
156            .add_link(LinkSpec { mass: 1.0, length: 0.5, ..Default::default() })
157            .build(&mut world);
158
159        assert_eq!(chain.num_joints, 1);
160        // After gravity steps, the tip should have moved
161        for _ in 0..500 { world.step(0.002); }
162        let tip = world.body(chain.tip()).unwrap();
163        let pos = &tip.transform.translation.0;
164        assert!(pos[0].is_finite() && pos[2].is_finite(),
165            "Tip position should be finite: {pos:?}");
166        // The constraint keeps it connected to base; verify it hasn't exploded
167        let dist_from_base = ((pos[0]).powi(2) + (pos[1]).powi(2) + (pos[2] - 2.0).powi(2)).sqrt();
168        assert!(dist_from_base < 2.0, "Tip should stay near base: dist={dist_from_base}");
169    }
170
171    #[test]
172    fn test_three_link_chain_finite() {
173        let mut world = world_with_gravity();
174        let chain = ChainBuilder::new()
175            .base_position(Point::new([0.0, 0.0, 3.0]))
176            .add_link(LinkSpec { mass: 2.0, length: 0.5, ..Default::default() })
177            .add_link(LinkSpec { mass: 1.5, length: 0.4, ..Default::default() })
178            .add_link(LinkSpec { mass: 1.0, length: 0.3, ..Default::default() })
179            .build(&mut world);
180
181        assert_eq!(chain.num_joints, 3);
182        for _ in 0..200 { world.step(0.001); }
183        for &h in &chain.links {
184            let p = &world.body(h).unwrap().transform.translation.0;
185            assert!(p[0].is_finite() && p[1].is_finite() && p[2].is_finite());
186        }
187    }
188
189    #[test]
190    fn test_joint_readback() {
191        let mut world = world_with_gravity();
192        let chain = ChainBuilder::new()
193            .base_position(Point::new([0.0, 0.0, 2.0]))
194            .add_link(LinkSpec::default())
195            .add_link(LinkSpec::default())
196            .build(&mut world);
197
198        let states = chain.read_joint_states(&world);
199        assert_eq!(states.len(), 2);
200        for (a, v) in &states {
201            assert!(a.is_finite());
202            assert!(v.is_finite());
203        }
204    }
205
206    #[test]
207    fn test_tip_is_last_link() {
208        let mut world = PhysicsWorld::new(SVector::from([0.0, 0.0, 0.0]));
209        let chain = ChainBuilder::new()
210            .add_link(LinkSpec::default())
211            .add_link(LinkSpec::default())
212            .build(&mut world);
213        assert_eq!(chain.tip(), chain.links[1]);
214    }
215}
symtropy_physics/articulation.rs

symtropy_physics/
articulation.rs
