Fix torque generation for the prismatic joint motor

examples3d/all_examples3.rs

@@ -23,6 +23,7 @@ mod debug_cylinder3;
 mod debug_dynamic_collider_add3;
 mod debug_friction3;
 mod debug_infinite_fall3;
+mod debug_prismatic3;
 mod debug_rollback3;
 mod debug_triangle3;
 mod debug_trimesh3;
@@ -109,6 +110,7 @@ pub fn main() {
         ("(Debug) trimesh", debug_trimesh3::init_world),
         ("(Debug) cylinder", debug_cylinder3::init_world),
         ("(Debug) infinite fall", debug_infinite_fall3::init_world),
+        ("(Debug) prismatic", debug_prismatic3::init_world),
         ("(Debug) rollback", debug_rollback3::init_world),
     ];
 

examples3d/debug_prismatic3.rs

@@ -0,0 +1,111 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn prismatic_repro(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    box_center: Point3<f32>,
+) {
+    use rapier3d::{dynamics::RigidBodyBuilder, geometry::ColliderBuilder};
+    let box_rb = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(box_center.x, box_center.y, box_center.z)
+            .build(),
+    );
+    colliders.insert(
+        ColliderBuilder::cuboid(1.0, 0.25, 1.0).build(),
+        box_rb,
+        bodies,
+    );
+
+    let wheel_y = -1.0;
+    let wheel_positions = vec![
+        Vector3::new(1.0, wheel_y, -1.0),
+        Vector3::new(-1.0, wheel_y, -1.0),
+        Vector3::new(1.0, wheel_y, 1.0),
+        Vector3::new(-1.0, wheel_y, 1.0),
+    ];
+
+    for pos in wheel_positions {
+        let wheel_pos_in_world = box_center + pos;
+        let wheel_rb = bodies.insert(
+            RigidBodyBuilder::new_dynamic()
+                .translation(
+                    wheel_pos_in_world.x,
+                    wheel_pos_in_world.y,
+                    wheel_pos_in_world.z,
+                )
+                .build(),
+        );
+        colliders.insert(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies);
+
+        let mut prismatic = rapier3d::dynamics::PrismaticJoint::new(
+            Point3::new(pos.x, pos.y, pos.z),
+            Vector3::y_axis(),
+            Vector3::default(),
+            Point3::new(0.0, 0.0, 0.0),
+            Vector3::y_axis(),
+            Vector3::default(),
+        );
+        prismatic.configure_motor_model(rapier3d::dynamics::SpringModel::VelocityBased);
+        let (stiffness, damping) = (0.05, 0.2);
+        prismatic.configure_motor_position(0.0, stiffness, damping);
+
+        joints.insert(bodies, box_rb, wheel_rb, prismatic);
+    }
+
+    // put a small box under one of the wheels
+    let gravel = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(box_center.x + 1.0, box_center.y - 2.4, -1.0)
+            .build(),
+    );
+    colliders.insert(
+        ColliderBuilder::cuboid(0.5, 0.1, 0.5).build(),
+        gravel,
+        bodies,
+    );
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    prismatic_repro(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(0.0, 5.0, 0.0),
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/joints3.rs

@@ -107,12 +107,12 @@ fn create_actuated_prismatic_joints(
             // the limits with large forces.
             prism.motor_max_impulse = 1.0;
         } else if i > 1 {
-            prism.configure_motor_position(2.0, 0.2, 1.0);
+            prism.configure_motor_position(2.0, 0.01, 1.0);
         } else {
             prism.configure_motor_velocity(1.0, 1.0);
             // We set a max impulse so that the motor doesn't fight
             // the limits with large forces.
-            prism.motor_max_impulse = 1.0;
+            prism.motor_max_impulse = 0.7;
             prism.limits_enabled = true;
             prism.limits[0] = -2.0;
             prism.limits[1] = 5.0;

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -83,6 +83,7 @@ impl PrismaticVelocityConstraint {
         let anchor2 = rb2.position * joint.local_anchor2;
         let axis1 = rb1.position * joint.local_axis1;
         let axis2 = rb2.position * joint.local_axis2;
+
         #[cfg(feature = "dim2")]
         let basis1 = rb1.position * joint.basis1[0];
         #[cfg(feature = "dim3")]
@@ -179,6 +180,8 @@ impl PrismaticVelocityConstraint {
          */
         let mut motor_rhs = 0.0;
         let mut motor_inv_lhs = 0.0;
+        let gcross1 = r1.gcross(*axis1);
+        let gcross2 = r2.gcross(*axis2);
 
         let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
             params.dt,
@@ -192,12 +195,24 @@ impl PrismaticVelocityConstraint {
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.dot(&gcross2)
+                - rb1.linvel.dot(&axis1)
+                - rb1.angvel.dot(&gcross1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
         if stiffness != 0.0 || damping != 0.0 {
-            motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma };
+            motor_inv_lhs = if keep_lhs {
+                let inv_projected_mass = crate::utils::inv(
+                    im1 + im2
+                        + gcross1.gdot(ii1.transform_vector(gcross1))
+                        + gcross2.gdot(ii2.transform_vector(gcross2)),
+                );
+
+                gamma * inv_projected_mass
+            } else {
+                gamma
+            };
             motor_rhs /= gamma;
         }
 
@@ -236,8 +251,6 @@ impl PrismaticVelocityConstraint {
                 limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
                     * velocity_based_erp_inv_dt;
 
-                let gcross1 = r1.gcross(*axis1);
-                let gcross2 = r2.gcross(*axis2);
                 limits_inv_lhs = crate::utils::inv(
                     im1 + im2
                         + gcross1.gdot(ii1.transform_vector(gcross1))
@@ -301,8 +314,16 @@ impl PrismaticVelocityConstraint {
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
         // Warmstart motors.
-        mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse);
+        if self.motor_impulse != 0.0 {
-        mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse);
+            let lin_impulse1 = self.motor_axis1 * self.motor_impulse;
+            let lin_impulse2 = self.motor_axis2 * self.motor_impulse;
+
+            mj_lambda1.linear += lin_impulse1 * self.im1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+
+            mj_lambda2.linear -= lin_impulse2 * self.im2;
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
+        }
 
         // Warmstart limits.
         if self.limits_active {
@@ -382,19 +403,31 @@ impl PrismaticVelocityConstraint {
 
     fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
         if self.motor_inv_lhs != 0.0 {
-            let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear)
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
-                - self.motor_axis1.dot(&mj_lambda1.linear)
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = self
+                .motor_axis2
+                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+                - self
+                    .motor_axis1
+                    .dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.motor_rhs;
             let new_impulse = na::clamp(
-                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
+                self.motor_impulse + dvel * self.motor_inv_lhs,
                 -self.motor_max_impulse,
                 self.motor_max_impulse,
             );
             let dimpulse = new_impulse - self.motor_impulse;
             self.motor_impulse = new_impulse;
 
-            mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse);
+            let lin_impulse1 = self.motor_axis1 * dimpulse;
-            mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse);
+            let lin_impulse2 = self.motor_axis2 * dimpulse;
+
+            mj_lambda1.linear += lin_impulse1 * self.im1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+            mj_lambda2.linear -= lin_impulse2 * self.im2;
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
     }