diff --git a/crates/avian2d/Cargo.toml b/crates/avian2d/Cargo.toml index 6022176ad..53785dc19 100644 --- a/crates/avian2d/Cargo.toml +++ b/crates/avian2d/Cargo.toml @@ -194,6 +194,10 @@ required-features = ["2d", "default-collider"] name = "revolute_joint_2d" required-features = ["2d", "default-collider"] +[[example]] +name = "joint_motors_2d" +required-features = ["2d", "default-collider"] + [[example]] name = "sensor" required-features = ["2d", "default-collider"] diff --git a/crates/avian2d/examples/joint_motors_2d.rs b/crates/avian2d/examples/joint_motors_2d.rs new file mode 100644 index 000000000..11b41257b --- /dev/null +++ b/crates/avian2d/examples/joint_motors_2d.rs @@ -0,0 +1,271 @@ +//! Demonstrates motor joints in 2D. +//! +//! - Left side: Revolute joint with velocity-controlled angular motor (spinning wheel) +//! - Center: Revolute joint with position-controlled angular motor (servo) +//! - Right side: Prismatic joint with linear motor (piston) +//! +//! Controls: +//! - Arrow Up/Down: Adjust left motor target velocity +//! - A/D: Adjust center motor target angle +//! - W/S: Adjust right motor target position +//! - Space: Toggle motors on/off + +use avian2d::{math::*, prelude::*}; +use bevy::prelude::*; +use examples_common_2d::ExampleCommonPlugin; + +fn main() { + App::new() + .add_plugins(( + DefaultPlugins, + ExampleCommonPlugin, + PhysicsPlugins::default(), + )) + .insert_resource(ClearColor(Color::srgb(0.05, 0.05, 0.1))) + .insert_resource(Gravity(Vector::NEG_Y * 1000.0)) + .add_systems(Startup, setup) + .add_systems(Update, (control_motors, update_ui)) + .run(); +} + +#[derive(Component)] +struct VelocityMotorJoint; + +#[derive(Component)] +struct PositionMotorJoint; + +#[derive(Component)] +struct PrismaticMotorJoint; + +#[derive(Component)] +struct UiText; + +fn setup(mut commands: Commands) { + commands.spawn(Camera2d); + + // === Velocity-Controlled Revolute Joint (left side) === + // Static anchor for the wheel + let velocity_anchor = commands + .spawn(( + Sprite { + color: Color::srgb(0.5, 0.5, 0.5), + custom_size: Some(Vec2::splat(20.0)), + ..default() + }, + Transform::from_xyz(-200.0, 0.0, 0.0), + RigidBody::Static, + )) + .id(); + + // Spinning wheel + let velocity_wheel = commands + .spawn(( + Sprite { + color: Color::srgb(0.9, 0.3, 0.3), + custom_size: Some(Vec2::splat(80.0)), + ..default() + }, + Transform::from_xyz(-200.0, 0.0, 0.0), + RigidBody::Dynamic, + Mass(1.0), + AngularInertia(1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + )) + .id(); + + // Revolute joint with velocity-controlled motor + // Default anchors are at body centers (Vector::ZERO) + commands.spawn(( + RevoluteJoint::new(velocity_anchor, velocity_wheel).with_motor(AngularMotor { + target_velocity: 5.0, + max_torque: 1000.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 1.0, + }, + ..default() + }), + VelocityMotorJoint, + )); + + // === Position-Controlled Revolute Joint (center) === + // Static anchor for the servo + let position_anchor = commands + .spawn(( + Sprite { + color: Color::srgb(0.5, 0.5, 0.5), + custom_size: Some(Vec2::splat(20.0)), + ..default() + }, + Transform::from_xyz(0.0, 0.0, 0.0), + RigidBody::Static, + )) + .id(); + + // Servo arm - also positioned at anchor, rotates around its center + let servo_arm = commands + .spawn(( + Sprite { + color: Color::srgb(0.3, 0.5, 0.9), + custom_size: Some(Vec2::new(100.0, 20.0)), + ..default() + }, + Transform::from_xyz(0.0, 0.0, 0.0), + RigidBody::Dynamic, + Mass(1.0), + AngularInertia(1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + )) + .id(); + + // Revolute joint with position-controlled motor (servo behavior) + // + // Using spring parameters (frequency, damping_ratio) for stable behavior. + // This provides predictable spring-damper dynamics across different configurations. + // - frequency: 5 Hz = fairly stiff spring + // - damping_ratio: 1.0 = critically damped (fastest approach without overshoot) + commands.spawn(( + RevoluteJoint::new(position_anchor, servo_arm).with_motor( + AngularMotor::new(MotorModel::SpringDamper { + frequency: 5.0, + damping_ratio: 1.0, + }) + .with_target_position(0.0) + .with_max_torque(Scalar::MAX), + ), + PositionMotorJoint, + )); + + // === Prismatic Joint with Linear Motor (right side) === + let piston_base_sprite = Sprite { + color: Color::srgb(0.5, 0.5, 0.5), + custom_size: Some(Vec2::new(40.0, 200.0)), + ..default() + }; + + let piston_sprite = Sprite { + color: Color::srgb(0.3, 0.9, 0.3), + custom_size: Some(Vec2::new(60.0, 40.0)), + ..default() + }; + + // Static base for the piston + let piston_base = commands + .spawn(( + piston_base_sprite, + Transform::from_xyz(200.0, 0.0, 0.0), + RigidBody::Static, + Position(Vector::new(200.0, 0.0)), + )) + .id(); + + // Moving piston + let piston = commands + .spawn(( + piston_sprite, + Transform::from_xyz(200.0, 0.0, 0.0), + RigidBody::Dynamic, + Mass(1.0), + AngularInertia(1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + Position(Vector::new(200.0, 0.0)), + )) + .id(); + + // frequency = 20 Hz, damping_ratio = 1.0 (critically damped) + commands.spawn(( + PrismaticJoint::new(piston_base, piston) + .with_slider_axis(Vector::Y) + .with_limits(-100.0, 100.0) + .with_motor( + LinearMotor::new(MotorModel::SpringDamper { + frequency: 20.0, + damping_ratio: 1.0, + }) + .with_target_position(50.0) + .with_max_force(Scalar::MAX), + ), + PrismaticMotorJoint, + )); + + commands.spawn(( + Text::new("Motor Joints Demo\n\nArrow Up/Down: Velocity motor speed\nA/D: Position motor angle\nW/S: Prismatic motor position\nSpace: Reset motors\n\nVelocity: 5.0 rad/s\nPosition: 0.00 rad\nPrismatic: 50.0 units"), + TextFont { + font_size: 18.0, + ..default() + }, + TextColor(Color::WHITE), + Node { + position_type: PositionType::Absolute, + top: Val::Px(10.0), + left: Val::Px(10.0), + ..default() + }, + UiText, + )); +} + +fn control_motors( + keyboard: Res>, + mut velocity_motors: Single<&mut RevoluteJoint, With>, + mut position_motors: Single< + &mut RevoluteJoint, + (With, Without), + >, + mut prismatic_motors: Single<&mut PrismaticJoint, With>, +) { + // Velocity-controlled revolute joint motor + if keyboard.just_pressed(KeyCode::ArrowUp) { + velocity_motors.motor.target_velocity += 1.0; + } + if keyboard.just_pressed(KeyCode::ArrowDown) { + velocity_motors.motor.target_velocity -= 1.0; + } + + // Position-controlled revolute joint motor + if keyboard.just_pressed(KeyCode::KeyA) { + position_motors.motor.target_position += 0.5; + } + if keyboard.just_pressed(KeyCode::KeyD) { + position_motors.motor.target_position -= 0.5; + } + + // Position-controlled prismatic joint motor + if keyboard.just_pressed(KeyCode::KeyW) { + prismatic_motors.motor.target_position += 25.0; + } + if keyboard.just_pressed(KeyCode::KeyS) { + prismatic_motors.motor.target_position -= 25.0; + } + + // Toggle motors on/off + if keyboard.just_pressed(KeyCode::Space) { + velocity_motors.motor.enabled = !velocity_motors.motor.enabled; + position_motors.motor.enabled = !position_motors.motor.enabled; + prismatic_motors.motor.enabled = !prismatic_motors.motor.enabled; + } +} + +fn update_ui( + velocity_motor: Single<&RevoluteJoint, With>, + position_motor: Single<&RevoluteJoint, With>, + prismatic_motor: Single<&PrismaticJoint, With>, + mut ui_text: Single<&mut Text, With>, +) { + ui_text.0 = format!( + "Motor Joints Demo\n\n\ + Arrow Up/Down: Velocity motor speed\n\ + A/D: Position motor angle\n\ + W/S: Prismatic motor position\n\ + Space: Toggle motors\n\n\ + Velocity: {:.1} rad/s\n\ + Position: {:.2} rad\n\ + Prismatic: {:.1} units\n\ + Enabled: {}", + velocity_motor.motor.target_velocity, + position_motor.motor.target_position, + prismatic_motor.motor.target_position, + // We can pick any of the motors here since they are toggled together + velocity_motor.motor.enabled + ); +} diff --git a/crates/avian3d/Cargo.toml b/crates/avian3d/Cargo.toml index ae234b547..ccd56e3d5 100644 --- a/crates/avian3d/Cargo.toml +++ b/crates/avian3d/Cargo.toml @@ -173,6 +173,10 @@ required-features = ["3d", "default-collider"] name = "revolute_joint_3d" required-features = ["3d", "default-collider"] +[[example]] +name = "joint_motors_3d" +required-features = ["3d", "default-collider"] + [[example]] name = "gyroscopic_motion" required-features = ["3d", "default-collider"] diff --git a/crates/avian3d/examples/joint_motors_3d.rs b/crates/avian3d/examples/joint_motors_3d.rs new file mode 100644 index 000000000..40825335c --- /dev/null +++ b/crates/avian3d/examples/joint_motors_3d.rs @@ -0,0 +1,274 @@ +//! Demonstrates motor joints in 3D. +//! +//! - Left side: Revolute joint with velocity-controlled angular motor (spinning wheel) +//! - Center: Revolute joint with position-controlled angular motor (servo) +//! - Right side: Prismatic joint with linear motor (piston) +//! +//! Controls: +//! - Arrow Up/Down: Adjust left motor target velocity +//! - A/D: Adjust center motor target angle +//! - W/S: Adjust right motor target position +//! - Space: Toggle motors on/off + +use avian3d::{math::*, prelude::*}; +use bevy::prelude::*; +use examples_common_3d::ExampleCommonPlugin; + +fn main() { + App::new() + .add_plugins(( + DefaultPlugins, + ExampleCommonPlugin, + PhysicsPlugins::default(), + )) + .insert_resource(ClearColor(Color::srgb(0.05, 0.05, 0.1))) + .add_systems(Startup, setup) + .add_systems(Update, (control_motors, update_ui)) + .run(); +} + +#[derive(Component)] +struct VelocityMotorJoint; + +#[derive(Component)] +struct PositionMotorJoint; + +#[derive(Component)] +struct PrismaticMotorJoint; + +#[derive(Component)] +struct UiText; + +fn setup( + mut commands: Commands, + mut materials: ResMut>, + mut meshes: ResMut>, +) { + let anchor_mesh = meshes.add(Cuboid::from_size(Vec3::splat(0.3))); + let anchor_material = materials.add(Color::srgb(0.5, 0.5, 0.5)); + + let wheel_mesh = meshes.add(Cylinder::new(1.0, 0.3)); + let wheel_material = materials.add(Color::srgb(0.9, 0.3, 0.3)); + + let arm_mesh = meshes.add(Cuboid::from_size(Vec3::new(2.0, 0.3, 0.3))); + let arm_material = materials.add(Color::srgb(0.3, 0.5, 0.9)); + + let piston_base_mesh = meshes.add(Cuboid::from_size(Vec3::new(0.5, 3.0, 0.5))); + let piston_base_material = materials.add(Color::srgb(0.5, 0.5, 0.5)); + + let piston_mesh = meshes.add(Cuboid::from_size(Vec3::new(0.8, 0.5, 0.8))); + let piston_material = materials.add(Color::srgb(0.3, 0.9, 0.3)); + + // === Velocity-Controlled Revolute Joint (left side) === + // Static anchor for the wheel + let velocity_anchor = commands + .spawn(( + Mesh3d(anchor_mesh.clone()), + MeshMaterial3d(anchor_material.clone()), + Transform::from_xyz(-3.0, 0.0, 0.0), + RigidBody::Static, + )) + .id(); + + // Spinning wheel + let velocity_wheel = commands + .spawn(( + Mesh3d(wheel_mesh), + MeshMaterial3d(wheel_material), + Transform::from_xyz(-3.0, 0.0, 0.0), + RigidBody::Dynamic, + MassPropertiesBundle::from_shape(&Cylinder::new(1.0, 0.3), 1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + )) + .id(); + + // Revolute joint with velocity-controlled motor + commands.spawn(( + RevoluteJoint::new(velocity_anchor, velocity_wheel) + .with_hinge_axis(Vector::Z) + .with_motor(AngularMotor { + target_velocity: 5.0, + max_torque: 1000.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 1.0, + }, + ..default() + }), + VelocityMotorJoint, + )); + + // === Position-Controlled Revolute Joint (center) === + // Static anchor for the servo + let position_anchor = commands + .spawn(( + Mesh3d(anchor_mesh.clone()), + MeshMaterial3d(anchor_material.clone()), + Transform::from_xyz(0.0, 0.0, 0.0), + RigidBody::Static, + )) + .id(); + + // Servo arm - rotates around its center + let servo_arm = commands + .spawn(( + Mesh3d(arm_mesh), + MeshMaterial3d(arm_material), + Transform::from_xyz(0.0, 0.0, 0.0), + RigidBody::Dynamic, + MassPropertiesBundle::from_shape(&Cuboid::from_size(Vec3::new(2.0, 0.3, 0.3)), 1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + )) + .id(); + + // Revolute joint with position-controlled motor (servo behavior) + commands.spawn(( + RevoluteJoint::new(position_anchor, servo_arm) + .with_hinge_axis(Vector::Z) + .with_motor( + AngularMotor::new(MotorModel::SpringDamper { + frequency: 5.0, + damping_ratio: 1.0, + }) + .with_target_position(0.0) + .with_max_torque(Scalar::MAX), + ), + PositionMotorJoint, + )); + + // === Prismatic Joint with Linear Motor (right side) === + // Static base for the piston + let piston_base = commands + .spawn(( + Mesh3d(piston_base_mesh), + MeshMaterial3d(piston_base_material), + Transform::from_xyz(3.0, 0.0, 0.0), + RigidBody::Static, + )) + .id(); + + // Moving piston + let piston = commands + .spawn(( + Mesh3d(piston_mesh), + MeshMaterial3d(piston_material), + Transform::from_xyz(3.0, 0.0, 0.0), + RigidBody::Dynamic, + MassPropertiesBundle::from_shape(&Cuboid::from_size(Vec3::new(0.8, 0.5, 0.8)), 1.0), + SleepingDisabled, // Prevent sleeping so motor can always control it + )) + .id(); + + // Prismatic joint with linear motor + commands.spawn(( + PrismaticJoint::new(piston_base, piston) + .with_slider_axis(Vector::Y) + .with_limits(-1.0, 1.0) + .with_motor( + LinearMotor::new(MotorModel::SpringDamper { + frequency: 20.0, + damping_ratio: 1.0, + }) + .with_target_position(1.0) + .with_max_force(Scalar::MAX), + ), + PrismaticMotorJoint, + )); + + // UI text + commands.spawn(( + Text::new("Motor Joints Demo\n\nArrow Up/Down: Velocity motor speed\nA/D: Position motor angle\nW/S: Prismatic motor position\nSpace: Reset motors\n\nVelocity: 5.0 rad/s\nPosition: 0.00 rad\nPrismatic: 1.0 units"), + TextFont { + font_size: 18.0, + ..default() + }, + TextColor(Color::WHITE), + Node { + position_type: PositionType::Absolute, + top: Val::Px(10.0), + left: Val::Px(10.0), + ..default() + }, + UiText, + )); + + // Directional light + commands.spawn(( + DirectionalLight { + illuminance: 2000.0, + shadows_enabled: true, + ..default() + }, + Transform::default().looking_at(Vec3::new(-1.0, -2.5, -1.5), Vec3::Y), + )); + + // Camera + commands.spawn(( + Camera3d::default(), + Transform::from_translation(Vec3::new(0.0, 2.0, 10.0)).looking_at(Vec3::ZERO, Vec3::Y), + )); +} + +fn control_motors( + keyboard: Res>, + mut velocity_motors: Single<&mut RevoluteJoint, With>, + mut position_motors: Single< + &mut RevoluteJoint, + (With, Without), + >, + mut prismatic_motors: Single<&mut PrismaticJoint, With>, +) { + // Velocity-controlled revolute joint motor + if keyboard.just_pressed(KeyCode::ArrowUp) { + velocity_motors.motor.target_velocity += 1.0; + } + if keyboard.just_pressed(KeyCode::ArrowDown) { + velocity_motors.motor.target_velocity -= 1.0; + } + + // Position-controlled revolute joint motor + if keyboard.just_pressed(KeyCode::KeyA) { + position_motors.motor.target_position += 0.5; + } + if keyboard.just_pressed(KeyCode::KeyD) { + position_motors.motor.target_position -= 0.5; + } + + // Position-controlled prismatic joint motor + if keyboard.just_pressed(KeyCode::KeyW) { + prismatic_motors.motor.target_position += 0.5; + } + if keyboard.just_pressed(KeyCode::KeyS) { + prismatic_motors.motor.target_position -= 0.5; + } + + // Toggle motors on/off + if keyboard.just_pressed(KeyCode::Space) { + velocity_motors.motor.enabled = !velocity_motors.motor.enabled; + position_motors.motor.enabled = !position_motors.motor.enabled; + prismatic_motors.motor.enabled = !prismatic_motors.motor.enabled; + } +} + +fn update_ui( + velocity_motor: Single<&RevoluteJoint, With>, + position_motor: Single<&RevoluteJoint, With>, + prismatic_motor: Single<&PrismaticJoint, With>, + mut ui_text: Single<&mut Text, With>, +) { + ui_text.0 = format!( + "Motor Joints Demo\n\n\ + Arrow Up/Down: Velocity motor speed\n\ + A/D: Position motor angle\n\ + W/S: Prismatic motor position\n\ + Space: Toggle motors\n\n\ + Velocity: {:.1} rad/s\n\ + Position: {:.2} rad\n\ + Prismatic: {:.1} units\n\ + Enabled: {}", + velocity_motor.motor.target_velocity, + position_motor.motor.target_position, + prismatic_motor.motor.target_position, + velocity_motor.motor.enabled, + ); +} diff --git a/migration-guides/0.5-to-main.md b/migration-guides/0.5-to-main.md index 0965c96a7..761da8411 100644 --- a/migration-guides/0.5-to-main.md +++ b/migration-guides/0.5-to-main.md @@ -6,6 +6,11 @@ since the latest release. These guides are evolving and may not be polished yet. See [migration-guides/README.md](./README.md) and existing entries for information about Avian's migration guide process and what to put here. +## Joimt Motors + +`RevoluteJoint` and `PrismaticJoimt` now store motors for driving movement. +By default, they are disabled, so this should not be a breaking change for most applications. + ## `ReadRigidBodyForces` and `WriteRigidBodyForces` PR [#908](https://github.com/avianphysics/avian/pull/908) introduced two new traits: `ReadRigidBodyForces` and `WriteRigidBodyForces`, and `RigidyBodyForces` is now defined as: diff --git a/src/dynamics/joints/mod.rs b/src/dynamics/joints/mod.rs index 9355d67ff..036982b7b 100644 --- a/src/dynamics/joints/mod.rs +++ b/src/dynamics/joints/mod.rs @@ -208,19 +208,23 @@ Therefore, they have 3 translational DOF and 3 rotational DOF, a total of 6 DOF. //! ## Other Configuration //! //! Different joints may have different configuration options. They may allow you to change the axis of allowed -//! translation or rotation, and can have distance or angle limits for those axes. +//! translation or rotation, and apply limits or motors for those axes. //! //! Take a look at the documentation and methods of each joint to see all the different configuration options. mod distance; mod fixed; +mod motor; mod prismatic; mod revolute; #[cfg(feature = "3d")] mod spherical; +#[cfg(test)] +mod tests; pub use distance::DistanceJoint; pub use fixed::FixedJoint; +pub use motor::{AngularMotor, LinearMotor, MotorModel}; pub use prismatic::PrismaticJoint; pub use revolute::RevoluteJoint; #[cfg(feature = "3d")] @@ -660,6 +664,7 @@ pub struct JointDamping { pub struct JointForces { force: Vector, torque: AngularVector, + motor_force: Scalar, } impl JointForces { @@ -669,6 +674,7 @@ impl JointForces { Self { force: Vector::ZERO, torque: AngularVector::ZERO, + motor_force: 0.0, } } @@ -684,9 +690,18 @@ impl JointForces { self.torque } + /// Returns the force or torque applied by the motor, if any. + /// + /// For angular motors ([`AngularMotor`]), this is the torque in N·m. + /// For linear motors ([`LinearMotor`]), this is the force in N. + #[inline] + pub const fn motor_force(&self) -> Scalar { + self.motor_force + } + /// Sets the force applied by the joint. /// - /// This should be done automatically by the joint solver, + /// This should be done automatically by the joint solver. #[inline] pub const fn set_force(&mut self, force: Vector) { self.force = force; @@ -694,11 +709,19 @@ impl JointForces { /// Sets the torque applied by the joint. /// - /// This should be done automatically by the joint solver, + /// This should be done automatically by the joint solver. #[inline] pub const fn set_torque(&mut self, torque: AngularVector) { self.torque = torque; } + + /// Sets the motor force or torque. + /// + /// This should be done automatically by the joint solver. + #[inline] + pub const fn set_motor_force(&mut self, motor_force: Scalar) { + self.motor_force = motor_force; + } } /// The [reference frame] of a body that is being constrained by a [joint](self). diff --git a/src/dynamics/joints/motor.rs b/src/dynamics/joints/motor.rs new file mode 100644 index 000000000..9345b20c4 --- /dev/null +++ b/src/dynamics/joints/motor.rs @@ -0,0 +1,305 @@ +use crate::prelude::*; +use bevy::prelude::*; + +/// Determines how the joint motor force/torque is computed. +/// +/// Different models offer trade-offs between ease of tuning and physical accuracy. +/// The default is a [`SpringDamper`](MotorModel::SpringDamper) model that provides +/// stable, predictable behavior across different configurations. +#[derive(Clone, Copy, Debug, PartialEq, Reflect)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, PartialEq)] +pub enum MotorModel { + /// A spring-damper model using implicit Euler integration. + /// + /// Unlike the other models, this is unconditionally stable: the implicit formulation + /// naturally limits the response as frequency increases, preventing overshoot and + /// oscillation even with aggressive parameters. This makes it easier to tune than + /// the other models, which can become unstable with high stiffness values. + /// + /// This is the recommended model for most use cases. + /// + /// # Parameters + /// + /// - `frequency`: The natural frequency of the spring in Hz. Higher values create stiffer springs. + /// - `damping_ratio`: The damping ratio. + /// - 0.0 = no damping (oscillates forever) + /// - 1.0 = critically damped (fastest approach without overshoot) + /// - \> 1.0 = overdamped (slower approach without overshoot) + /// - < 1.0 = underdamped (overshoots and oscillates) + SpringDamper { + /// The natural frequency of the spring in Hz. + frequency: Scalar, + /// The damping ratio. + damping_ratio: Scalar, + }, + + /// The motor force/torque is computed directly from the stiffness and damping parameters. + /// + /// The model can be described by the following formula: + /// + /// ```text + /// force = (stiffness * position_error) + (damping * velocity_error) + /// ``` + /// + /// This produces physically accurate forces/torques, but requires careful tuning of the + /// stiffness and damping parameters based on the masses of the connected bodies. + /// High stiffness values can cause instability (overshoot, oscillation, or divergence), + /// so parameters must be chosen appropriately for your timestep and mass configuration. + /// + /// # Parameters + /// + /// - `stiffness`: The stiffness coefficient for position control. Set to zero for pure velocity control. + /// - `damping`: The damping coefficient for velocity control. + ForceBased { + /// The stiffness coefficient for position control. + stiffness: Scalar, + /// The damping coefficient for velocity control. + damping: Scalar, + }, + + /// The motor force/torque is computed based on the acceleration required to reach the target. + /// + /// The model can be described by the following formula: + /// + /// ```text + /// acceleration = (stiffness * position_error) + (damping * velocity_error) + /// ``` + /// + /// This automatically scales the motor force/torque based on the masses of the bodies, + /// resulting in consistent behavior across different mass configurations. + /// It is therefore easier to tune compared to the [`ForceBased`](MotorModel::ForceBased) model, + /// which requires manual adjustment of stiffness and damping based on mass. + /// + /// Note that high stiffness values can still cause instability. For unconditionally + /// stable behavior, use the [`SpringDamper`](MotorModel::SpringDamper) model instead. + /// + /// # Parameters + /// + /// - `stiffness`: The stiffness coefficient for position control. Set to zero for pure velocity control. + /// - `damping`: The damping coefficient for velocity control. + AccelerationBased { + /// The stiffness coefficient for position control. + stiffness: Scalar, + /// The damping coefficient for velocity control. + damping: Scalar, + }, +} + +impl Default for MotorModel { + /// The default motor model: a critically damped spring-damper with 5 Hz frequency. + fn default() -> Self { + Self::DEFAULT + } +} + +impl MotorModel { + /// The default motor model: a critically damped spring-damper with 5 Hz frequency. + pub const DEFAULT: Self = Self::SpringDamper { + frequency: 5.0, + damping_ratio: 1.0, + }; +} + +/// A motor for driving the angular motion of a [`RevoluteJoint`]. +/// +/// Motors are configured as part of a joint, applying torque to drive +/// the joint towards a target velocity and/or position. +/// +/// ```ignore +/// RevoluteJoint::new(entity1, entity2) +/// .with_motor( +/// AngularMotor::new(MotorModel::SpringDamper { +/// frequency: 2.0, +/// damping_ratio: 1.0, +/// }) +/// .with_target_position(target_angle) +/// ) +/// ``` +#[derive(Clone, Copy, Debug, PartialEq, Reflect)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, PartialEq)] +pub struct AngularMotor { + /// Whether the motor is enabled. + pub enabled: bool, + /// The target angular velocity (rad/s). + pub target_velocity: Scalar, + /// The target angle (rad) for position control. + pub target_position: Scalar, + /// The maximum torque the motor can apply (N·m). + pub max_torque: Scalar, + /// The motor model used for computing the motor torque. + pub motor_model: MotorModel, +} + +impl Default for AngularMotor { + fn default() -> Self { + Self::new(MotorModel::DEFAULT) + } +} + +impl AngularMotor { + /// Creates a new angular motor with the given motor model. + #[inline] + pub const fn new(motor_model: MotorModel) -> Self { + Self { + enabled: true, + target_velocity: 0.0, + target_position: 0.0, + max_torque: Scalar::MAX, + motor_model, + } + } + + /// Creates a new disabled angular motor with the given motor model. + /// + /// To enable the motor later, use [`set_enabled`](Self::set_enabled). + #[inline] + pub const fn new_disabled(motor_model: MotorModel) -> Self { + Self { + enabled: false, + ..Self::new(motor_model) + } + } + + /// Enables or disables the motor. + #[inline] + pub const fn set_enabled(&mut self, enabled: bool) -> &mut Self { + self.enabled = enabled; + self + } + + /// Sets the target angular velocity in radians per second. + #[inline] + pub const fn with_target_velocity(mut self, velocity: Scalar) -> Self { + self.target_velocity = velocity; + self + } + + /// Sets the target position. + #[inline] + pub const fn with_target_position(mut self, target_position: Scalar) -> Self { + self.target_position = target_position; + self + } + + /// Sets the maximum torque the motor can apply. + #[inline] + pub const fn with_max_torque(mut self, max_torque: Scalar) -> Self { + self.max_torque = max_torque; + self + } + + /// Sets the motor model used for computing the motor torque. + #[inline] + pub const fn with_motor_model(mut self, motor_model: MotorModel) -> Self { + self.motor_model = motor_model; + self + } +} + +/// A motor for driving the linear motion of a [`PrismaticJoint`]. +/// +/// Motors are configured as part of a joint, applying force to drive +/// the joint towards a target velocity and/or position. +/// +/// # Spring-Damper Model +/// +/// For stable position control that behaves consistently across different configurations, +/// use [`MotorModel::SpringDamper`]. This uses implicit Euler integration for +/// unconditional stability. +/// +/// ```ignore +/// PrismaticJoint::new(entity1, entity2) +/// .with_motor( +/// LinearMotor::new(MotorModel::SpringDamper { +/// frequency: 2.0, +/// damping_ratio: 1.0, +/// }) +/// .with_target_position(target_position) +/// ) +/// ``` +#[derive(Clone, Copy, Debug, PartialEq, Reflect)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, PartialEq)] +pub struct LinearMotor { + /// Whether the motor is enabled. + pub enabled: bool, + /// The target linear velocity (m/s). + pub target_velocity: Scalar, + /// The target position (m) for position control. + pub target_position: Scalar, + /// The maximum force the motor can apply (N). + pub max_force: Scalar, + /// The motor model used for computing the motor force. + pub motor_model: MotorModel, +} + +impl Default for LinearMotor { + fn default() -> Self { + Self::new(MotorModel::DEFAULT) + } +} + +impl LinearMotor { + /// Creates a new linear motor with the given motor model. + #[inline] + pub const fn new(motor_model: MotorModel) -> Self { + Self { + enabled: true, + target_velocity: 0.0, + target_position: 0.0, + max_force: Scalar::MAX, + motor_model, + } + } + + /// Creates a new disabled linear motor with the given motor model. + /// + /// To enable the motor later, use [`set_enabled`](Self::set_enabled). + #[inline] + pub const fn new_disabled(motor_model: MotorModel) -> Self { + Self { + enabled: false, + ..Self::new(motor_model) + } + } + + /// Enables or disables the motor. + #[inline] + pub const fn set_enabled(&mut self, enabled: bool) -> &mut Self { + self.enabled = enabled; + self + } + + /// Sets the target linear velocity in meters per second. + #[inline] + pub const fn with_target_velocity(mut self, velocity: Scalar) -> Self { + self.target_velocity = velocity; + self + } + + /// Sets the target position. + #[inline] + pub const fn with_target_position(mut self, target_position: Scalar) -> Self { + self.target_position = target_position; + self + } + + /// Sets the maximum force the motor can apply. + #[inline] + pub const fn with_max_force(mut self, max_force: Scalar) -> Self { + self.max_force = max_force; + self + } + + /// Sets the motor model used for computing the motor force. + #[inline] + pub const fn with_motor_model(mut self, motor_model: MotorModel) -> Self { + self.motor_model = motor_model; + self + } +} diff --git a/src/dynamics/joints/prismatic.rs b/src/dynamics/joints/prismatic.rs index 59fed1823..0b78e3f51 100644 --- a/src/dynamics/joints/prismatic.rs +++ b/src/dynamics/joints/prismatic.rs @@ -1,5 +1,5 @@ use crate::{ - dynamics::joints::{EntityConstraint, JointSystems}, + dynamics::joints::{EntityConstraint, JointSystems, motor::LinearMotor}, prelude::*, }; use bevy::{ @@ -16,9 +16,13 @@ use bevy::{ /// This can be useful for things like elevators, pistons, sliding doors and moving platforms. /// /// Each prismatic joint is defined by a [`JointFrame`] on each body, a [`slider_axis`](Self::slider_axis) -/// along which the bodies can translate, and an optional [`DistanceLimit`] that defines the extents of the allowed translation. +/// along which the bodies can translate, and an optional [`DistanceLimit`] that defines the extents +/// of the allowed translation. /// #[doc = include_str!("./images/prismatic_joint.svg")] +/// +/// The joint can also include a [`LinearMotor`] for driving the translation along the [`slider_axis`](Self::slider_axis). +/// Use this to create pistons, elevators, or other linear motion mechanisms. #[derive(Component, Clone, Debug, PartialEq, Reflect)] #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] @@ -47,6 +51,8 @@ pub struct PrismaticJoint { pub angle_compliance: Scalar, /// The compliance of the distance limit (inverse of stiffness, m / N). pub limit_compliance: Scalar, + /// A motor for driving the joint. + pub motor: LinearMotor, } impl EntityConstraint<2> for PrismaticJoint { @@ -72,6 +78,7 @@ impl PrismaticJoint { align_compliance: 0.0, angle_compliance: 0.0, limit_compliance: 0.0, + motor: LinearMotor::new_disabled(MotorModel::DEFAULT), } } @@ -306,6 +313,13 @@ impl PrismaticJoint { self.limit_compliance = compliance; self } + + /// Sets the motor for the joint. + #[inline] + pub const fn with_motor(mut self, motor: LinearMotor) -> Self { + self.motor = motor; + self + } } impl MapEntities for PrismaticJoint { diff --git a/src/dynamics/joints/revolute.rs b/src/dynamics/joints/revolute.rs index 9ecd0641b..9bf4a6aab 100644 --- a/src/dynamics/joints/revolute.rs +++ b/src/dynamics/joints/revolute.rs @@ -1,5 +1,5 @@ use crate::{ - dynamics::joints::{EntityConstraint, JointSystems}, + dynamics::joints::{EntityConstraint, JointSystems, motor::AngularMotor}, prelude::*, }; use bevy::{ @@ -37,6 +37,9 @@ use bevy::{ )] /// #[doc = include_str!("./images/revolute_joint.svg")] +/// +/// The joint can also include an [`AngularMotor`] for driving the rotation about the pivot point. +/// Use this to create wheels, fans, servos, or other rotating mechanisms. #[derive(Component, Clone, Debug, PartialEq, Reflect)] #[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] #[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] @@ -67,6 +70,8 @@ pub struct RevoluteJoint { pub align_compliance: Scalar, /// The compliance of the angle limit (inverse of stiffness, N * m / rad). pub limit_compliance: Scalar, + /// A motor for driving the joint. + pub motor: AngularMotor, } impl EntityConstraint<2> for RevoluteJoint { @@ -95,6 +100,7 @@ impl RevoluteJoint { #[cfg(feature = "3d")] align_compliance: 0.0, limit_compliance: 0.0, + motor: AngularMotor::new_disabled(MotorModel::DEFAULT), } } @@ -337,6 +343,13 @@ impl RevoluteJoint { self.limit_compliance = compliance; self } + + /// Sets the motor for the joint. + #[inline] + pub const fn with_motor(mut self, motor: AngularMotor) -> Self { + self.motor = motor; + self + } } impl MapEntities for RevoluteJoint { diff --git a/src/dynamics/joints/tests.rs b/src/dynamics/joints/tests.rs new file mode 100644 index 000000000..6aaaf03f0 --- /dev/null +++ b/src/dynamics/joints/tests.rs @@ -0,0 +1,853 @@ +use core::time::Duration; + +#[cfg(feature = "2d")] +use approx::assert_relative_eq; +use bevy::{mesh::MeshPlugin, prelude::*, time::TimeUpdateStrategy}; + +use crate::prelude::*; + +const TIMESTEP: f32 = 1.0 / 64.0; + +fn create_app() -> App { + let mut app = App::new(); + app.add_plugins(( + MinimalPlugins, + PhysicsPlugins::default(), + TransformPlugin, + #[cfg(feature = "bevy_scene")] + AssetPlugin::default(), + #[cfg(feature = "bevy_scene")] + bevy::scene::ScenePlugin, + MeshPlugin, + )); + + app.insert_resource(SubstepCount(20)); + + app.insert_resource(Gravity(Vector::ZERO)); + + app.insert_resource(Time::::from_duration(Duration::from_secs_f32( + TIMESTEP, + ))); + app.insert_resource(TimeUpdateStrategy::ManualDuration(Duration::from_secs_f32( + TIMESTEP, + ))); + + app +} + +/// Tests that an angular motor on a revolute joint spins the attached body. +#[test] +fn revolute_motor_spins_body() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + target_velocity: 2.0, + max_torque: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 10.0, + }, + ..default() + }), + ); + + // Initialize the app. + app.update(); + + // Run simulation for 1 second. + let duration = 1.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let angular_velocity = body_ref.get::().unwrap(); + + #[cfg(feature = "2d")] + { + assert!( + angular_velocity.0.abs() > 1.0, + "Angular velocity should be significant" + ); + assert_relative_eq!(angular_velocity.0, 2.0, epsilon = 0.5); + } + #[cfg(feature = "3d")] + { + let speed = angular_velocity.0.length(); + assert!(speed > 1.0, "Angular velocity should be significant"); + } +} + +/// Tests that a linear motor on a prismatic joint moves the attached body. +#[test] +fn prismatic_motor_moves_body() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + app.world_mut().spawn( + PrismaticJoint::new(anchor, dynamic) + .with_local_anchor1(Vector::X * 2.0) + .with_motor(LinearMotor { + target_velocity: 1.0, + max_force: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 10.0, + }, + ..default() + }), + ); + + // Initialize the app. + app.update(); + + let initial_x = app.world().entity(dynamic).get::().unwrap().0.x; + + // Run simulation for 1 second. + let duration = 1.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let final_x = body_ref.get::().unwrap().0.x; + + let displacement = final_x - initial_x; + assert!( + displacement > 0.5, + "Body should have moved: {}", + displacement + ); +} + +/// Tests that an angular motor with max torque limit respects the limit. +#[test] +fn revolute_motor_respects_max_torque() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(100.0), // Heavy body to test torque limiting + #[cfg(feature = "2d")] + AngularInertia(100.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(100.0)), + )) + .id(); + + // Create a revolute joint with a very limited motor torque. + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + target_velocity: 10.0, + max_torque: 0.1, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 1.0, + }, + ..default() + }), + ); + + // Initialize the app. + app.update(); + + // Run simulation for 1 second. + let duration = 1.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let angular_velocity = body_ref.get::().unwrap(); + + #[cfg(feature = "2d")] + { + assert!( + angular_velocity.0.abs() < 5.0, + "Velocity should be limited by max torque" + ); + } + #[cfg(feature = "3d")] + { + let speed = angular_velocity.0.length(); + assert!(speed < 5.0, "Velocity should be limited by max torque"); + } +} + +/// Tests that a position-targeting motor moves the joint towards the target position. +#[test] +fn revolute_motor_position_target() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Create a revolute joint with a position-targeting motor. + let target_angle = 1.0; + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + target_position: target_angle, + max_torque: Scalar::MAX, + motor_model: MotorModel::AccelerationBased { + stiffness: 50.0, + damping: 20.0, + }, + ..default() + }), + ); + + // Initialize the app. + app.update(); + + // Run simulation for 3 seconds to let it settle. + let duration = 3.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let rotation = body_ref.get::().unwrap(); + + // The body should have rotated towards the target angle (allow some tolerance). + #[cfg(feature = "2d")] + { + let angle = rotation.as_radians(); + assert!( + angle.abs() > 0.3, + "Motor should have rotated the body: {}", + angle + ); + } + #[cfg(feature = "3d")] + { + let (axis, angle) = rotation.to_axis_angle(); + let signed_angle = angle * axis.z.signum(); + assert!( + signed_angle.abs() > 0.3, + "Motor should have rotated the body: {}", + signed_angle + ); + } +} + +/// Tests that a linear position-targeting motor moves the joint towards the target position. +#[test] +fn prismatic_motor_position_target() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Create a prismatic joint with a position-targeting motor. + // Use AccelerationBased model for stable position targeting. + let target_position = 1.0; // Target is 1 meter along the slider axis + app.world_mut().spawn( + PrismaticJoint::new(anchor, dynamic) + .with_local_anchor1(Vector::X * 2.0) + .with_motor(LinearMotor { + target_position, + max_force: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 10.0, + damping: 5.0, + }, + ..default() + }), + ); + + // Initialize the app. + app.update(); + + let initial_pos = app.world().entity(dynamic).get::().unwrap().0; + + // Run simulation for 3 seconds to let it settle. + let duration = 3.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let final_pos = body_ref.get::().unwrap().0; + + assert!(!final_pos.x.is_nan(), "Final position should not be NaN"); + assert!(!final_pos.y.is_nan(), "Final position should not be NaN"); + + let displacement = final_pos.x - initial_pos.x; + + assert!( + displacement.abs() > 0.1 || final_pos.x.abs() > 0.1, + "Body should have moved: displacement={}, final_x={}", + displacement, + final_pos.x + ); +} + +/// Tests that a velocity motor on a revolute joint respects angle limits. +/// +/// The motor drives with constant velocity, but the joint should stop +/// when it reaches the angle limit. +#[test] +fn revolute_motor_respects_angle_limits() { + use crate::math::PI; + + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + let angle_limit = PI / 4.0; + + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic) + .with_angle_limits(-angle_limit, angle_limit) + .with_motor(AngularMotor { + target_velocity: 5.0, + max_torque: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 10.0, + }, + ..default() + }), + ); + + app.update(); + + // Run for 2 seconds - enough time for motor to hit the limit. + let duration = 2.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let rotation = body_ref.get::().unwrap(); + + #[cfg(feature = "2d")] + { + let angle = rotation.as_radians(); + assert!( + angle <= angle_limit + 0.1, + "Angle {} should not exceed limit {}", + angle, + angle_limit + ); + assert!( + angle > angle_limit - 0.3, + "Angle {} should be near the limit {}", + angle, + angle_limit + ); + } + #[cfg(feature = "3d")] + { + let (axis, angle) = rotation.to_axis_angle(); + let signed_angle = angle * axis.z.signum(); + assert!( + signed_angle <= angle_limit + 0.1, + "Angle {} should not exceed limit {}", + signed_angle, + angle_limit + ); + assert!( + signed_angle > angle_limit - 0.3, + "Angle {} should be near the limit {}", + signed_angle, + angle_limit + ); + } +} + +/// Tests that a velocity motor on a prismatic joint respects distance limits. +/// +/// The motor drives with constant velocity, but the joint should stop +/// when it reaches the distance limit. +#[test] +fn prismatic_motor_respects_limits() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + // Start at origin so we can measure displacement clearly. + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::ZERO), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Limit translation to [0, 1] meters along the slide axis. + let distance_limit = 1.0; + + app.world_mut().spawn( + PrismaticJoint::new(anchor, dynamic) + .with_limits(0.0, distance_limit) + .with_motor(LinearMotor { + target_velocity: 5.0, // High velocity to ensure we hit the limit + max_force: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 0.0, + damping: 10.0, + }, + ..default() + }), + ); + + app.update(); + + // Make sure the motor is not near the limit from the start. + { + let body_ref = app.world().entity(dynamic); + let position = body_ref.get::().unwrap(); + assert!( + (position.0.x - distance_limit).abs() > 0.1, + "Displacement {} should not be near the limit {} at the start of the test", + position.0.x, + distance_limit + ); + } + + // Run for 2 seconds - enough time for motor to hit the limit. + let duration = 2.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let position = body_ref.get::().unwrap(); + + // The displacement along the slide axis (X) should be at or near the limit. + let displacement = position.0.x; + assert!( + displacement <= distance_limit + 0.001, + "Displacement {} should not exceed limit {}", + displacement, + distance_limit + ); + assert!( + (displacement - distance_limit).abs() < 0.1, + "Displacement {} should be near the limit {}", + displacement, + distance_limit + ); +} + +/// Tests that `ForceBased` motor model works for revolute joints. +/// +/// This is the physically accurate motor model that takes mass into account. +#[test] +fn revolute_motor_force_based() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Use ForceBased motor model with velocity control. + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + target_velocity: 2.0, + max_torque: 100.0, + motor_model: MotorModel::ForceBased { + stiffness: 0.0, + damping: 10.0, + }, + ..default() + }), + ); + + app.update(); + + let body_ref = app.world().entity(dynamic); + let angular_velocity = body_ref.get::().unwrap(); + #[cfg(feature = "2d")] + let initial_speed = angular_velocity.0.abs(); + #[cfg(feature = "3d")] + let initial_speed = angular_velocity.0.length(); + + assert!( + initial_speed.abs() < 0.001, + "ForceBased motor should be initiall still, speed: {}", + initial_speed + ); + + // Run simulation for 1 second. + let duration = 1.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let angular_velocity = body_ref.get::().unwrap(); + + // The body should have gained angular velocity. + #[cfg(feature = "2d")] + { + assert!( + angular_velocity.0.abs() > 0.5, + "ForceBased motor should spin the body: {}", + angular_velocity.0 + ); + } + #[cfg(feature = "3d")] + { + let speed = angular_velocity.0.length(); + assert!( + speed > 0.5, + "ForceBased motor should spin the body: {}", + speed + ); + } +} + +/// Tests that the default `SpringDamper` motor model works. +/// +/// `SpringDamper` is unconditionally stable and uses `frequency`/`damping_ratio`. +#[test] +fn revolute_motor_spring_damper() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + let target_angle = 1.0; + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + target_position: target_angle, + max_torque: Scalar::MAX, + motor_model: MotorModel::SpringDamper { + frequency: 2.0, + damping_ratio: 1.0, + }, + ..default() + }), + ); + + app.update(); + + // TODO Assert the body is initially not near the target. + + // Run simulation for 3 seconds to let the spring settle. + let duration = 3.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let rotation = body_ref.get::().unwrap(); + + // The body should have rotated towards the target. + #[cfg(feature = "2d")] + { + let angle = rotation.as_radians(); + assert!( + angle.abs() > 0.3, + "SpringDamper motor should rotate towards target: {}", + angle + ); + } + #[cfg(feature = "3d")] + { + let (axis, angle) = rotation.to_axis_angle(); + let signed_angle = angle * axis.z.signum(); + assert!( + signed_angle.abs() > 0.3, + "SpringDamper motor should rotate towards target: {}", + signed_angle + ); + } +} + +/// Tests that a motor with both velocity and position targeting works. +/// +/// Combined spring-damper behavior: position targeting with velocity damping. +#[test] +fn revolute_motor_combined_position_velocity() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Motor with both position and velocity targeting. + // The velocity adds a constant offset to the spring behavior. + let target_angle = 0.5; + app.world_mut().spawn( + RevoluteJoint::new(anchor, dynamic).with_motor(AngularMotor { + enabled: true, + target_position: target_angle, + target_velocity: 0.5, + max_torque: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 30.0, + damping: 15.0, + }, + }), + ); + + app.update(); + + // Run for 2 seconds. + let duration = 2.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let rotation = body_ref.get::().unwrap(); + + // The body should have rotated in the positive direction. + #[cfg(feature = "2d")] + { + let angle = rotation.as_radians(); + assert!( + angle > 0.2, + "Combined motor should rotate body positively: {}", + angle + ); + } + #[cfg(feature = "3d")] + { + let (axis, angle) = rotation.to_axis_angle(); + let signed_angle = angle * axis.z.signum(); + assert!( + signed_angle > 0.2, + "Combined motor should rotate body positively: {}", + signed_angle + ); + } +} + +/// Tests that a prismatic motor with both velocity and position targeting works. +#[test] +fn prismatic_motor_combined_position_velocity() { + let mut app = create_app(); + app.finish(); + + let anchor = app + .world_mut() + .spawn((RigidBody::Static, Position(Vector::ZERO))) + .id(); + + let dynamic = app + .world_mut() + .spawn(( + RigidBody::Dynamic, + Position(Vector::X * 2.0), + Mass(1.0), + #[cfg(feature = "2d")] + AngularInertia(1.0), + #[cfg(feature = "3d")] + AngularInertia::new(Vec3::splat(1.0)), + )) + .id(); + + // Motor with both position and velocity targeting. + app.world_mut().spawn( + PrismaticJoint::new(anchor, dynamic) + .with_local_anchor1(Vector::X * 2.0) + .with_motor(LinearMotor { + enabled: true, + target_position: 1.0, + target_velocity: 0.5, + max_force: 100.0, + motor_model: MotorModel::AccelerationBased { + stiffness: 20.0, + damping: 10.0, + }, + }), + ); + + app.update(); + + let initial_x = app.world().entity(dynamic).get::().unwrap().0.x; + + // Run for 2 seconds. + let duration = 2.0; + let steps = (duration / TIMESTEP) as usize; + + for _ in 0..steps { + app.update(); + } + + let body_ref = app.world().entity(dynamic); + let final_x = body_ref.get::().unwrap().0.x; + + // The body should have moved. + let displacement = final_x - initial_x; + assert!( + displacement.abs() > 0.1, + "Combined motor should move the body: {}", + displacement + ); +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 27b47af49..8a91ff256 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -79,9 +79,9 @@ pub mod prelude { CustomPositionIntegration, CustomVelocityIntegration, Gravity, IntegratorPlugin, }, joints::{ - AngleLimit, DistanceJoint, DistanceLimit, FixedJoint, JointAnchor, JointBasis, - JointCollisionDisabled, JointDamping, JointDisabled, JointForces, JointFrame, - JointPlugin, PrismaticJoint, RevoluteJoint, + AngleLimit, AngularMotor, DistanceJoint, DistanceLimit, FixedJoint, JointAnchor, + JointBasis, JointCollisionDisabled, JointDamping, JointDisabled, JointForces, + JointFrame, JointPlugin, LinearMotor, MotorModel, PrismaticJoint, RevoluteJoint, }, rigid_body::{ forces::{ diff --git a/src/dynamics/solver/xpbd/joints/prismatic.rs b/src/dynamics/solver/xpbd/joints/prismatic.rs index a30b1c868..2c9255dd4 100644 --- a/src/dynamics/solver/xpbd/joints/prismatic.rs +++ b/src/dynamics/solver/xpbd/joints/prismatic.rs @@ -1,8 +1,11 @@ use super::FixedAngleConstraintShared; use crate::{ - dynamics::solver::{ - solver_body::{SolverBody, SolverBodyInertia}, - xpbd::*, + dynamics::{ + joints::MotorModel, + solver::{ + solver_body::{SolverBody, SolverBodyInertia}, + xpbd::*, + }, }, prelude::*, }; @@ -20,12 +23,20 @@ pub struct PrismaticJointSolverData { pub(super) free_axis1: Vector, pub(super) total_position_lagrange: Vector, pub(super) angle_constraint: FixedAngleConstraintShared, + /// Accumulated motor Lagrange multiplier for this frame. + pub(super) total_motor_lagrange: Scalar, + /// Motor Lagrange multiplier from the previous frame, used for warm starting. + /// This is zeroed after being applied in the first substep. + pub(super) warm_start_motor_lagrange: Scalar, } impl XpbdConstraintSolverData for PrismaticJointSolverData { fn clear_lagrange_multipliers(&mut self) { self.total_position_lagrange = Vector::ZERO; self.angle_constraint.clear_lagrange_multipliers(); + // Save motor lagrange for warm starting before clearing. + self.warm_start_motor_lagrange = self.total_motor_lagrange; + self.total_motor_lagrange = 0.0; } fn total_position_lagrange(&self) -> Vector { @@ -35,6 +46,10 @@ impl XpbdConstraintSolverData for PrismaticJointSolverData { fn total_rotation_lagrange(&self) -> AngularVector { self.angle_constraint.total_rotation_lagrange() } + + fn total_motor_lagrange(&self) -> Scalar { + self.total_motor_lagrange + } } impl XpbdConstraint<2> for PrismaticJoint { @@ -90,9 +105,46 @@ impl XpbdConstraint<2> for PrismaticJoint { .angle_constraint .solve([body1, body2], inertias, self.angle_compliance, dt); + // Solve motors before limits to give limits higher priority. + self.apply_motor(body1, body2, inertias[0], inertias[1], solver_data, dt); + // Constrain the relative positions of the bodies, only allowing translation along one free axis. self.constrain_positions(body1, body2, inertias[0], inertias[1], solver_data, dt); } + + fn warm_start_motors( + &self, + bodies: [&mut SolverBody; 2], + inertias: [&SolverBodyInertia; 2], + solver_data: &mut PrismaticJointSolverData, + _dt: Scalar, + warm_start_coefficient: Scalar, + ) { + if !self.motor.enabled { + return; + } + + let [body1, body2] = bodies; + let [inertia1, inertia2] = inertias; + + let inv_mass1 = inertia1.effective_inv_mass(); + let inv_mass2 = inertia2.effective_inv_mass(); + let inv_angular_inertia1 = inertia1.effective_inv_angular_inertia(); + let inv_angular_inertia2 = inertia2.effective_inv_angular_inertia(); + + let axis = body1.delta_rotation * solver_data.free_axis1; + let world_r1 = body1.delta_rotation * solver_data.world_r1; + let world_r2 = body2.delta_rotation * solver_data.world_r2; + + let impulse = warm_start_coefficient * solver_data.warm_start_motor_lagrange * axis; + + body1.linear_velocity -= impulse * inv_mass1; + body2.linear_velocity += impulse * inv_mass2; + body1.angular_velocity -= inv_angular_inertia1 * cross(world_r1, impulse); + body2.angular_velocity += inv_angular_inertia2 * cross(world_r2, impulse); + + solver_data.warm_start_motor_lagrange = 0.0; + } } impl PrismaticJoint { @@ -191,6 +243,109 @@ impl PrismaticJoint { } } +impl PrismaticJoint { + /// Applies motor forces to drive the joint towards the target velocity and/or position. + fn apply_motor( + &self, + body1: &mut SolverBody, + body2: &mut SolverBody, + inertia1: &SolverBodyInertia, + inertia2: &SolverBodyInertia, + solver_data: &mut PrismaticJointSolverData, + dt: Scalar, + ) { + let motor = &self.motor; + + if !motor.enabled { + return; + } + + let axis1 = body1.delta_rotation * solver_data.free_axis1; + let world_r1 = body1.delta_rotation * solver_data.world_r1; + let world_r2 = body2.delta_rotation * solver_data.world_r2; + + let separation = (body2.delta_position - body1.delta_position) + + (world_r2 - world_r1) + + solver_data.center_difference; + let current_position = separation.dot(axis1); + let current_velocity = (body2.linear_velocity - body1.linear_velocity).dot(axis1); + + let inv_mass1 = inertia1.effective_inv_mass(); + let inv_mass2 = inertia2.effective_inv_mass(); + let inv_angular_inertia1 = inertia1.effective_inv_angular_inertia(); + let inv_angular_inertia2 = inertia2.effective_inv_angular_inertia(); + + let w1 = PositionConstraint::compute_generalized_inverse_mass( + self, + inv_mass1.max_element(), + inv_angular_inertia1, + world_r1, + axis1, + ); + let w2 = PositionConstraint::compute_generalized_inverse_mass( + self, + inv_mass2.max_element(), + inv_angular_inertia2, + world_r2, + axis1, + ); + + let w_sum = w1 + w2; + if w_sum <= Scalar::EPSILON { + return; + } + + let velocity_error = motor.target_velocity - current_velocity; + let position_error = motor.target_position - current_position; + + let target_velocity_change = match motor.motor_model { + MotorModel::SpringDamper { + frequency, + damping_ratio, + } => { + // Implicit Euler formulation for stable spring-damper behavior. + let omega = TAU * frequency; + let omega_sq = omega * omega; + let two_zeta_omega = 2.0 * damping_ratio * omega; + let inv_denominator = 1.0 / (1.0 + two_zeta_omega * dt + omega_sq * dt * dt); + (omega_sq * position_error + two_zeta_omega * velocity_error) * dt * inv_denominator + } + MotorModel::AccelerationBased { stiffness, damping } => { + damping * velocity_error + stiffness * position_error * dt + } + MotorModel::ForceBased { stiffness, damping } => { + // Velocity change = (stiffness * pos_error + damping * vel_error) * inv_mass + (stiffness * position_error + damping * velocity_error) * w_sum + } + }; + + let correction = target_velocity_change * dt; + if correction.abs() <= Scalar::EPSILON { + return; + } + + let delta_lagrange = correction / w_sum; + + // Clamp to limit instantaneous force per substep. + let delta_lagrange = if motor.max_force < Scalar::MAX && motor.max_force > 0.0 { + let max_delta = motor.max_force * dt * dt; + delta_lagrange.clamp(-max_delta, max_delta) + } else { + delta_lagrange + }; + + solver_data.total_motor_lagrange += delta_lagrange; + + let impulse = delta_lagrange * axis1; + solver_data.total_position_lagrange += impulse; + + // Negate impulse: apply_positional_impulse convention is opposite to motor direction. + self.apply_positional_impulse( + body1, body2, inertia1, inertia2, -impulse, world_r1, world_r2, + ); + } +} + impl PositionConstraint for PrismaticJoint {} impl AngularConstraint for PrismaticJoint {} diff --git a/src/dynamics/solver/xpbd/joints/revolute.rs b/src/dynamics/solver/xpbd/joints/revolute.rs index 49c7c3f26..ead3066bb 100644 --- a/src/dynamics/solver/xpbd/joints/revolute.rs +++ b/src/dynamics/solver/xpbd/joints/revolute.rs @@ -1,8 +1,11 @@ use super::PointConstraintShared; use crate::{ - dynamics::solver::{ - solver_body::{SolverBody, SolverBodyInertia}, - xpbd::*, + dynamics::{ + joints::{AngularMotor, MotorModel}, + solver::{ + solver_body::{SolverBody, SolverBodyInertia}, + xpbd::*, + }, }, prelude::*, }; @@ -27,6 +30,11 @@ pub struct RevoluteJointSolverData { pub(super) b2: Vector, pub(super) total_align_lagrange: AngularVector, pub(super) total_limit_lagrange: AngularVector, + /// Accumulated motor Lagrange multiplier for this frame. + pub(super) total_motor_lagrange: AngularVector, + /// Motor Lagrange multiplier from the previous frame, used for warm starting. + /// This is zeroed after being applied in the first substep. + pub(super) warm_start_motor_lagrange: AngularVector, } impl XpbdConstraintSolverData for RevoluteJointSolverData { @@ -34,6 +42,20 @@ impl XpbdConstraintSolverData for RevoluteJointSolverData { self.point_constraint.clear_lagrange_multipliers(); self.total_align_lagrange = AngularVector::ZERO; self.total_limit_lagrange = AngularVector::ZERO; + // Save motor lagrange for warm starting before clearing. + self.warm_start_motor_lagrange = self.total_motor_lagrange; + self.total_motor_lagrange = AngularVector::ZERO; + } + + fn total_motor_lagrange(&self) -> Scalar { + #[cfg(feature = "2d")] + { + self.total_motor_lagrange + } + #[cfg(feature = "3d")] + { + self.total_motor_lagrange.length() + } } fn total_position_lagrange(&self) -> Vector { @@ -41,7 +63,7 @@ impl XpbdConstraintSolverData for RevoluteJointSolverData { } fn total_rotation_lagrange(&self) -> AngularVector { - self.total_align_lagrange + self.total_limit_lagrange + self.total_align_lagrange + self.total_limit_lagrange + self.total_motor_lagrange } } @@ -122,6 +144,16 @@ impl XpbdConstraint<2> for RevoluteJoint { ); } + // Solve motors before limits to give limits higher priority. + self.apply_motor( + body1, + body2, + inv_angular_inertia1, + inv_angular_inertia2, + solver_data, + dt, + ); + // Apply angle limits when rotating around the free axis self.apply_angle_limits( body1, @@ -137,6 +169,32 @@ impl XpbdConstraint<2> for RevoluteJoint { .point_constraint .solve([body1, body2], inertias, self.point_compliance, dt); } + + fn warm_start_motors( + &self, + bodies: [&mut SolverBody; 2], + inertias: [&SolverBodyInertia; 2], + solver_data: &mut RevoluteJointSolverData, + _dt: Scalar, + warm_start_coefficient: Scalar, + ) { + if !self.motor.enabled { + return; + } + + let [body1, body2] = bodies; + let [inertia1, inertia2] = inertias; + + let inv_angular_inertia1 = inertia1.effective_inv_angular_inertia(); + let inv_angular_inertia2 = inertia2.effective_inv_angular_inertia(); + + let impulse = warm_start_coefficient * solver_data.warm_start_motor_lagrange; + + body1.angular_velocity -= inv_angular_inertia1 * impulse; + body2.angular_velocity += inv_angular_inertia2 * impulse; + + solver_data.warm_start_motor_lagrange = AngularVector::ZERO; + } } impl RevoluteJoint { @@ -180,6 +238,148 @@ impl RevoluteJoint { dt, ); } + + /// Applies motor forces to drive the joint towards the target velocity and/or position. + /// + /// Uses a PD controller approach with optional implicit Euler integration improved stability. + fn apply_motor( + &self, + body1: &mut SolverBody, + body2: &mut SolverBody, + inv_angular_inertia1: SymmetricTensor, + inv_angular_inertia2: SymmetricTensor, + solver_data: &mut RevoluteJointSolverData, + dt: Scalar, + ) { + let motor = &self.motor; + + if !motor.enabled { + return; + } + + #[cfg(feature = "2d")] + let current_angle = solver_data.rotation_difference + + body1.delta_rotation.angle_between(body2.delta_rotation); + #[cfg(feature = "3d")] + let a1 = body1.delta_rotation * solver_data.a1; + #[cfg(feature = "3d")] + let current_angle = { + let b1 = body1.delta_rotation * solver_data.b1; + let b2 = body2.delta_rotation * solver_data.b2; + let sin_angle = b1.cross(b2).dot(a1); + let cos_angle = b1.dot(b2); + sin_angle.atan2(cos_angle) + }; + + #[cfg(feature = "2d")] + let relative_angular_velocity = body2.angular_velocity - body1.angular_velocity; + #[cfg(feature = "3d")] + let relative_angular_velocity = (body2.angular_velocity - body1.angular_velocity).dot(a1); + + #[cfg(feature = "2d")] + let w_sum = inv_angular_inertia1 + inv_angular_inertia2; + #[cfg(feature = "3d")] + let w_sum = + AngularConstraint::compute_generalized_inverse_mass(self, inv_angular_inertia1, a1) + + AngularConstraint::compute_generalized_inverse_mass( + self, + inv_angular_inertia2, + a1, + ); + + if w_sum <= Scalar::EPSILON { + return; + } + + let velocity_error = motor.target_velocity - relative_angular_velocity; + + // Wrap position error to [-PI, PI] for shortest path rotation. + let raw_error = motor.target_position - current_angle; + let position_error = (raw_error + PI).rem_euclid(TAU) - PI; + + let Some(delta_lagrange) = + Self::compute_angular_motor_lagrange(velocity_error, position_error, w_sum, motor, dt) + else { + return; + }; + + #[cfg(feature = "2d")] + { + solver_data.total_motor_lagrange += delta_lagrange; + } + #[cfg(feature = "3d")] + { + solver_data.total_motor_lagrange += delta_lagrange * a1; + } + + // Positive delta_lagrange increases body2's angular velocity relative to body1. + #[cfg(feature = "2d")] + self.apply_angular_lagrange_update( + body1, + body2, + inv_angular_inertia1, + inv_angular_inertia2, + delta_lagrange, + ); + #[cfg(feature = "3d")] + self.apply_angular_lagrange_update( + body1, + body2, + inv_angular_inertia1, + inv_angular_inertia2, + delta_lagrange, + a1, + ); + } + + /// Computes the Lagrange multiplier delta for an angular motor. + /// + /// Returns `None` if the correction is negligible. + fn compute_angular_motor_lagrange( + velocity_error: Scalar, + position_error: Scalar, + w_sum: Scalar, + motor: &AngularMotor, + dt: Scalar, + ) -> Option { + let target_velocity_change = match motor.motor_model { + MotorModel::SpringDamper { + frequency, + damping_ratio, + } => { + // Implicit Euler formulation for stable spring-damper behavior. + let omega = TAU * frequency; + let omega_sq = omega * omega; + let two_zeta_omega = 2.0 * damping_ratio * omega; + let inv_denominator = 1.0 / (1.0 + two_zeta_omega * dt + omega_sq * dt * dt); + (omega_sq * position_error + two_zeta_omega * velocity_error) * dt * inv_denominator + } + MotorModel::AccelerationBased { stiffness, damping } => { + damping * velocity_error + stiffness * position_error * dt + } + MotorModel::ForceBased { stiffness, damping } => { + // Velocity change = (stiffness * pos_error + damping * vel_error) * inv_inertia + (stiffness * position_error + damping * velocity_error) * w_sum + } + }; + + let correction = target_velocity_change * dt; + if correction.abs() <= Scalar::EPSILON { + return None; + } + + let delta_lagrange = correction / w_sum; + + // Clamp to limit instantaneous torque per substep. + let delta_lagrange = if motor.max_torque < Scalar::MAX && motor.max_torque > 0.0 { + let max_delta = motor.max_torque * dt * dt; + delta_lagrange.clamp(-max_delta, max_delta) + } else { + delta_lagrange + }; + + Some(delta_lagrange) + } } impl PositionConstraint for RevoluteJoint {} diff --git a/src/dynamics/solver/xpbd/mod.rs b/src/dynamics/solver/xpbd/mod.rs index cae589422..020b4a32a 100644 --- a/src/dynamics/solver/xpbd/mod.rs +++ b/src/dynamics/solver/xpbd/mod.rs @@ -279,7 +279,10 @@ //! attachment position. mod plugin; -pub use plugin::{XpbdSolverPlugin, XpbdSolverSystems, prepare_xpbd_joint, solve_xpbd_joint}; +pub use plugin::{ + XpbdSolverPlugin, XpbdSolverSystems, prepare_xpbd_joint, solve_xpbd_joint, + warm_start_xpbd_motors, +}; pub mod joints; @@ -307,6 +310,11 @@ pub trait XpbdConstraintSolverData { fn total_rotation_lagrange(&self) -> AngularVector { AngularVector::ZERO } + + /// Returns the total Lagrange multiplier accumulated by the motor, if any. + fn total_motor_lagrange(&self) -> Scalar { + 0.0 + } } /// A trait for all XPBD [constraints](self#constraints). @@ -346,6 +354,26 @@ pub trait XpbdConstraint { solver_data: &mut Self::SolverData, dt: Scalar, ); + + /// Warm starts the motor constraints by applying impulses from the previous frame. + /// + /// This is called once at the beginning of the first substep. After applying, + /// the stored warm start impulses should be zeroed to prevent re-application + /// in subsequent substeps. + /// + /// The `warm_start_coefficient` scales the applied impulse (typically 1.0). + /// + /// The default implementation does nothing. + #[allow(unused_variables)] + fn warm_start_motors( + &self, + bodies: [&mut SolverBody; ENTITY_COUNT], + inertias: [&SolverBodyInertia; ENTITY_COUNT], + solver_data: &mut Self::SolverData, + dt: Scalar, + warm_start_coefficient: Scalar, + ) { + } } /// Computes how much a constraint's [Lagrange multiplier](self#lagrange-multipliers) changes when projecting diff --git a/src/dynamics/solver/xpbd/plugin.rs b/src/dynamics/solver/xpbd/plugin.rs index 5f35d9a0f..ea0f35b0b 100644 --- a/src/dynamics/solver/xpbd/plugin.rs +++ b/src/dynamics/solver/xpbd/plugin.rs @@ -5,6 +5,7 @@ use crate::{ dynamics::{ joints::EntityConstraint, solver::{ + SolverConfig, schedule::SubstepSolverSystems, solver_body::{SolverBody, SolverBodyInertia}, xpbd::{XpbdConstraint, XpbdConstraintSolverData}, @@ -54,6 +55,21 @@ impl Plugin for XpbdSolverPlugin { .in_set(SolverSystems::PrepareJoints), ); + // Warm start motor constraints. + // These are chained to avoid ambiguity, and marked ambiguous_with the contact + // warm start since motor and contact warm starting are independent operations + // that both add to body velocities. + app.add_systems( + SubstepSchedule, + ( + warm_start_xpbd_motors::, + warm_start_xpbd_motors::, + ) + .chain() + .ambiguous_with_all() + .in_set(SubstepSolverSystems::WarmStart), + ); + // Solve joints with XPBD. app.add_systems( SubstepSchedule, @@ -188,6 +204,57 @@ pub fn solve_xpbd_joint< } } +/// Warm starts the motor constraints for joints of a given type. +/// +/// This applies the motor impulses from the previous frame as velocity changes, +/// improving convergence for motors that need to maintain steady forces. +pub fn warm_start_xpbd_motors< + C: Component + EntityConstraint<2> + XpbdConstraint<2>, +>( + bodies: Query<(&mut SolverBody, &SolverBodyInertia), Without>, + mut joints: Query<(&C, &mut C::SolverData), (Without, Without)>, + time: Res