symtropy_physics/joints/
mod.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
4//! Joint types for articulated bodies.
5//!
6//! All joints implement the `Constraint<D>` trait, using the same iterative
7//! position + velocity solve pipeline as other constraints. Joints are
8//! dimension-agnostic (const-generic `<const D: usize>`).
9
10pub mod ball;
11pub mod fixed;
12pub mod hinge;
13pub mod prismatic;
14
15pub use ball::BallJoint;
16pub use fixed::FixedJoint;
17pub use hinge::HingeJoint;
18pub use prismatic::PrismaticJoint;
19
20/// Motor drive for actuated joints (hinge + prismatic).
21///
22/// Uses a PD (Proportional-Derivative) controller to reach a target:
23/// `force = kp * (target - current_pos) + kd * (target_vel - current_vel)`
24#[derive(Clone, Debug)]
25pub struct MotorDrive {
26    /// Target position (units) or angle (radians).
27    pub target_pos: f64,
28    /// Target velocity.
29    pub target_vel: f64,
30    /// Proportional gain (stiffness).
31    pub kp: f64,
32    /// Derivative gain (damping).
33    pub kd: f64,
34    /// Maximum force/torque the motor can apply.
35    pub max_force: f64,
36}
37
38impl MotorDrive {
39    /// Create a motor with default gains.
40    pub fn new(target_pos: f64, max_force: f64) -> Self {
41        Self {
42            target_pos,
43            target_vel: 0.0,
44            kp: max_force,
45            kd: max_force * 0.1,
46            max_force,
47        }
48    }
49
50    /// Set PD gains.
51    pub fn with_gains(mut self, kp: f64, kd: f64) -> Self {
52        self.kp = kp;
53        self.kd = kd;
54        self
55    }
56
57    /// Calculate motor impulse.
58    pub fn calculate_impulse(&self, current_pos: f64, current_vel: f64, dt: f64) -> f64 {
59        let error_pos = self.target_pos - current_pos;
60        let error_vel = self.target_vel - current_vel;
61        let force =
62            (self.kp * error_pos + self.kd * error_vel).clamp(-self.max_force, self.max_force);
63        force * dt
64    }
65}
symtropy_physics/joints/mod.rs

symtropy_physics/joints/
mod.rs
