Merge pull request #422 from pellico/Fix_Revolute-joint-motor-target-…

…position-is-broken-at-and-beyond-90-degree-angles

Fix #378 Added one example join_motor_position

CHANGELOG.md

@@ -1,4 +1,7 @@
 ## Unlereased
+### Fix
+- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the user.
+
 ### Added
 - Add `SphericalJoint::local_frame1/2`, `::set_local_frame1/2`, and `SphericalJointBuilder::local_frame1/2` to set both
   the joint’s anchor and reference orientation.

examples2d/all_examples2.rs

@@ -17,6 +17,7 @@ mod damping2;
 mod debug_box_ball2;
 mod drum2;
 mod heightfield2;
+mod joint_motor_position2;
 mod joints2;
 mod locked_rotations2;
 mod one_way_platforms2;
@@ -79,6 +80,7 @@ pub fn main() {
         ("Rope Joints", rope_joints2::init_world),
         ("Sensor", sensor2::init_world),
         ("Trimesh", trimesh2::init_world),
+        ("Joint motor position", joint_motor_position2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
     ];
 

examples2d/joint_motor_position2.rs

@@ -0,0 +1,79 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Fixed ground to attach one end of the joints.
+     */
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+
+    /*
+     * A rectangle on a motor with target position.
+     */
+    for num in 0..9 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 2.0])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(point![x_pos, 1.5])
+            .local_anchor2(point![0.0, -0.5])
+            .motor_position(
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+                1000.0,
+                150.0,
+            );
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * A rectangle on a motor with limits.
+     */
+    for num in 0..8 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 4.5])
+            .rotation(std::f32::consts::PI)
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(point![x_pos, 5.0])
+            .local_anchor2(point![0.0, -0.5])
+            .motor_velocity(1.5, 30.0)
+            .motor_max_force(100.0)
+            .limits([
+                -std::f32::consts::PI,
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+            ]);
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        vector![0.0, 0.0],
+        (),
+    );
+    testbed.look_at(point![0.0, 0.0], 40.0);
+}

examples3d/all_examples3.rs

@@ -35,6 +35,7 @@ mod joints3;
 // mod joints3;
 mod character_controller3;
 mod debug_internal_edges3;
+mod joint_motor_position3;
 mod keva3;
 mod locked_rotations3;
 mod newton_cradle3;
@@ -97,6 +98,7 @@ pub fn main() {
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Impulse Joints", joints3::init_world_with_joints),
+        ("Joint Motor Position", joint_motor_position3::init_world),
         ("Locked rotations", locked_rotations3::init_world),
         ("One-way platforms", one_way_platforms3::init_world),
         ("Platform", platform3::init_world),

examples3d/joint_motor_position3.rs

@@ -0,0 +1,79 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Fixed ground to attach one end of the joints.
+     */
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+
+    /*
+     * A rectangle on a motor with target position.
+     */
+    for num in 0..9 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 2.0, 0.0])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new(Vector::z_axis())
+            .local_anchor1(point![x_pos, 1.5, 0.0])
+            .local_anchor2(point![0.0, -0.5, 0.0])
+            .motor_position(
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+                1000.0,
+                150.0,
+            );
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * A rectangle on a motor with limits.
+     */
+    for num in 0..8 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 4.5, 0.0])
+            .rotation(vector![0.0, 0.0, std::f32::consts::PI])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new(Vector::z_axis())
+            .local_anchor1(point![x_pos, 5.0, 0.0])
+            .local_anchor2(point![0.0, -0.5, 0.0])
+            .motor_velocity(1.5, 30.0)
+            .motor_max_force(100.0)
+            .limits([
+                -std::f32::consts::PI,
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+            ]);
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        vector![0.0, 0.0, 0.0],
+        (),
+    );
+    testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
+}