Add joint motors for revolute and prismatic joints

[pierre-l]

Draft status

This is a work in progress.
While this solution appears to work as-is, I am still self-reviewing, refining, and testing it.
Early feedback is still welcome though :)

Objective

Supports velocity and position control with optional timestep-independent spring-damper parameters (frequency, damping_ratio). Includes warm starting and motor force feedback via JointForces.

Partially fixes #465
This implementation only works for joints with a single axis (PrismaticJoint, RevoluteJoint).
Additional work is needed to support multiple axes (SphericalJoint).

Solution

Three MotorModel were added: ForceBased and AccelerationBased were directly inspired from the Rapier eponymous models, the SpringDamper model doesn't exists in Rapier but seems much easier to use to me. The latter was made the default.

The motor are solved through XPBD like any other joint constraint.

Relevant Rapier sources:

• src/dynamics/joint/motor_model.rs
• src/dynamics/joint/generic_joint.rs
• dynamics/solver/joint_constraint/joint_constraint_builder.rs
• src/dynamics/joint/revolute_joint.rs
• src/dynamics/joint/prismatic_joint.rs

Breaking change

The current implementation minimizes breaking changes, however, users who directly add the XPBD solver systems instead of using the plugin will have to make changes for motors to work:

Similarly, users who manually implemented XpbdConstraint for custom joints may need to implement XpbdMotorConstraint instead if they want motors to work.

Testing

• A reasonable series of unit tests is included in the PR,
• The motor_joints_2d and motor_joint_3d examples provide an interactive way to test these,
• I couldn't find proper test vectors so there currently is no test scenario where it is verified that a specific motor configuration reaches a specific state in a precise time interval.

---

Showcase

Concise showcase

// Position-controlled motor (e.g., servo arm)
RevoluteJoint::new(anchor, arm)
    .with_motor(AngularMotor {
        target_position: PI / 4.0,  // 45 degrees
        max_torque: 100.0,
        motor_model: MotorModel::SpringDamper {
            frequency: 2.0,
            damping_ratio: 1.0,  // critically damped
        },
        ..default()
    })

See the examples included in this PR for more detailed examples.

Screenshot of the motor3d example:

[Jondolf]

Thanks for the PR! This looks very nice and impressive already, based on a quick look :)

Just as a heads-up: we have some plans to potentially replace the joint XPBD solver with something else, most likely a more traditional impulse-based solver like the one used by most other engines and our contacts, or mayybe AVBD if we can get it working well enough. That's one of the main reasons why I haven't personally prioritized implementing joint motors yet.

Still, that may take a while, and this work is very useful regardless; motors are something that a lot of people have been asking for, and the API should remain mostly the same even if the solver implementation were to change in the future.

---

Now, some general comments based on a first glance at the implementation.

I like having LinearJointMotor and AngularJointMotor as components from an API perspective. However, it does have a few problems:

• some joints don't support motors at all (ex: fixed joints)
• some joints only support linear motors (ex: prismatic joint) or angular motors (ex: revolute joint), but not both
• some joints can have multiple linear and/or angular motors, and need to support multiple axes, as you pointed out

Ultimately, it seems to me like we need some joint-specific motor APIs, both for correctness and flexibility. There's roughly two approaches I can think of:

1. Similar to Box2D and Jolt: Store motor settings on the joints themselves, in a JointMotor or MotorSettings property.

// Example: revolute joint motor
#[derive(Component)]
pub struct RevoluteJoint {
    pub body1: Entity,
    pub body2: Entity,
    pub frame1: JointFrame,
    pub frame2: JointFrame,
    #[cfg(feature = "3d")]
    pub hinge_axis: Vector,
    pub angle_limit: Option<AngleLimit>,
    // `MotorSettings` is like this PR's `LinearJointMotor`
    // or `AngularJointMotor`, but as just a single type;
    // in this case, it's used as an angular motor
    pub motor: MotorSettings,
    pub point_compliance: Scalar,
    pub align_compliance: Scalar,
    pub limit_compliance: Scalar,
}

2. Similar to Bepu: Have many different motor types, like RevoluteMotor, SphericalMotor, PrismaticMotor, and so on.

// Example: revolute joint motor
#[derive(Component, Deref, DerefMut)]
pub struct RevoluteMotor(MotorSettings);

// Example: spherical joint motor
// Alternatively, we could have a separate `SwingMotor` and `TwistMotor`
#[cfg(feature = "3d")]
#[derive(Component)]
pub struct SphericalMotor {
    pub swing: MotorSettings,
    pub twist: MotorSettings,
}

With either approach, each joint can store only the motors that are relevant for it, and can even support multiple axes. For example, Jolt's SwingTwistConstraint has separate mSwingMotorSettings and mTwistMotorSettings.

Option 2 looks interesting and does maybe feel more natural from a composition and ECS standpoint. I've been thinking that joint limits are optional in a similar way as motors, so we could even land on a design where the base joint type, its limit(s), and its motor(s) are all separate components:

commands.spawn((
    // The base joint type
    RevoluteJoint::new(entity1, entity2),
    // Optional limit
    RevoluteLimit::new(-FRAC_PI_2, FRAC_PI_2),
    // Optional motor
    RevoluteMotor::from_target_velocity(1.0),
));

This would lead to a rather nice conceptual split:

• Joint: locks some degrees of freedom, leaving others free
• Limit: limits relative movement along the remaining free degrees of freedom
• Motor: provides position and/or velocity actuation for the remaining free degrees of freedom

However I think the gains are still somewhat dubious, and it does add some complexity, since you need to query for multiple different components, and users could technically create invalid or unsupported configurations, like a spherical joint motor on a prismatic joint.

So for now, I think I would just go with Option 1, since it's more common and probably the most straightforward to implement. I also do kinda like that e.g. all revolute joint configuration and features are on the actual RevoluteJoint type and not scattered across many components.

Curious if you have ideas or thoughts here though :)

[pierre-l]

@Jondolf I greatly appreciate your words, thoughts, and the attention you already put into this.

This PR's API is an aspect I'm particularly unsatisfied with yet. There is too much freedom to shoot oneself in the foot in the constructor variants, and then there's the syntax point you mentioned. I am fine with both options. I agree with the nice conceptual separation from option 2, but joint limits and motors make no sense without a joint to attach to, so it's more natural to me to have these as attributes rather than components. We agree on which is the best option then. Worst case scenario: we're both wrong on this and adjust. No big deal, the adjustment effort looks similar either way.

I should be able to bring this ready for a proper review in the next few days.