Some minor cleanup and joint constraint refactoring.

examples3d/joints3.rs

@@ -57,6 +57,73 @@ fn create_prismatic_joints(
     }
 }
 
+fn create_actuated_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, origin.z)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(origin.x, origin.y, z)
+            .build();
+        let curr_child = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        colliders.insert(collider, curr_child, bodies);
+
+        let axis = if i % 2 == 0 {
+            Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
+        } else {
+            Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
+        };
+
+        let z = Vector3::z();
+        let mut prism = PrismaticJoint::new(
+            Point3::new(0.0, 0.0, 0.0),
+            axis,
+            z,
+            Point3::new(0.0, 0.0, -shift),
+            axis,
+            z,
+        );
+
+        if i == 1 {
+            prism.configure_motor_velocity(1.0, 1.0);
+            prism.limits_enabled = true;
+            prism.limits[1] = 5.0;
+            // We set a max impulse so that the motor doesn't fight
+            // 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);
+        } 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.limits_enabled = true;
+            prism.limits[0] = -2.0;
+            prism.limits[1] = 5.0;
+        }
+
+        joints.insert(bodies, curr_parent, curr_child, prism);
+
+        curr_parent = curr_child;
+    }
+}
+
 fn create_revolute_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
@@ -362,6 +429,13 @@ pub fn init_world(testbed: &mut Testbed) {
         Point3::new(20.0, 5.0, 0.0),
         4,
     );
+    create_actuated_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(25.0, 5.0, 0.0),
+        4,
+    );
     create_revolute_joints(
         &mut bodies,
         &mut colliders,

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -183,10 +183,7 @@ impl BallVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -201,10 +198,9 @@ impl BallVelocityConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
 
-        /*
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-         * Motor part.
-         */
         if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -224,6 +220,14 @@ impl BallVelocityConstraint {
             mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
             mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -232,7 +236,8 @@ impl BallVelocityConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }
@@ -393,9 +398,7 @@ impl BallVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
         let dvel = vel2 + self.rhs;
@@ -405,10 +408,9 @@ impl BallVelocityGroundConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
 
-        /*
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-         * Motor part.
-         */
         if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
@@ -425,6 +427,13 @@ impl BallVelocityGroundConstraint {
 
             mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -433,7 +442,8 @@ impl BallVelocityGroundConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -308,27 +308,11 @@ impl WPrismaticVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(
-        let mut mj_lambda1 = DeltaVel {
+        &mut self,
-            linear: Vector::from(
+        mj_lambda1: &mut DeltaVel<SimdReal>,
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+        mj_lambda2: &mut DeltaVel<SimdReal>,
-            ),
+    ) {
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -357,10 +341,13 @@ impl WPrismaticVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(
-         * Joint limits.
+        &mut self,
-         */
+        mj_lambda1: &mut DeltaVel<SimdReal>,
+        mj_lambda2: &mut DeltaVel<SimdReal>,
+    ) {
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -381,6 +368,28 @@ impl WPrismaticVelocityConstraint {
             mj_lambda2.linear += lin_impulse2 * self.im2;
             mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -503,19 +512,6 @@ impl WPrismaticVelocityGroundConstraint {
         let axis1 = position1 * local_axis1;
         let axis2 = position2 * local_axis2;
 
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -642,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let lin_dvel = self.basis1.tr_mul(&lin_vel2);
@@ -676,10 +660,9 @@ impl WPrismaticVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-         * Joint limits.
-         */
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
             // FIXME: the transformation by ii2_sqrt could be avoided by
             // reusing some computations above.
@@ -693,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint {
 
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -700,7 +697,6 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    // FIXME: duplicated code with the non-ground constraint.
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -209,10 +209,7 @@ impl RevoluteVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
@@ -237,10 +234,9 @@ impl RevoluteVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+    }
 
-        /*
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-         * Motor.
-         */
         if self.motor_inv_lhs != 0.0 {
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -258,6 +254,14 @@ impl RevoluteVelocityConstraint {
             mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
             mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -452,9 +456,7 @@ impl RevoluteVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
@@ -470,10 +472,8 @@ impl RevoluteVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
-
+    }
-        /*
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-         * Motor.
-         */
         if self.motor_inv_lhs != 0.0 {
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
             let ang_dvel = ang_vel2.dot(&self.motor_axis2);
@@ -489,6 +489,13 @@ impl RevoluteVelocityGroundConstraint {
 
             mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }