From b96e61853bf3c541f26f2dcef33379dae5d932b4 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Tue, 26 Dec 2023 22:25:17 -0800 Subject: [PATCH 1/3] Make testing environment with bug repro code --- examples3d/all_examples3.rs | 3 +++ examples3d/spherical_joint_testing.rs | 38 +++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 examples3d/spherical_joint_testing.rs diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 851bc8af2..178fca776 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -53,6 +53,8 @@ mod trimesh3; mod vehicle_controller3; mod vehicle_joints3; +mod spherical_joint_testing; + fn demo_name_from_command_line() -> Option { let mut args = std::env::args(); @@ -149,6 +151,7 @@ pub fn main() { debug_shape_modification3::init_world, ), ("(Debug) deserialize", debug_deserialize3::init_world), + ("Spherical Joint testing", spherical_joint_testing::init_world) ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples3d/spherical_joint_testing.rs b/examples3d/spherical_joint_testing.rs new file mode 100644 index 000000000..e5f6acefb --- /dev/null +++ b/examples3d/spherical_joint_testing.rs @@ -0,0 +1,38 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::{Testbed, TestbedApp}; + +pub fn init_world(testbed: &mut Testbed) { + + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + + let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 0.0, 0.0]); + let body = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0); + colliders.insert_with_parent(collider, body, &mut bodies); + + + let rigid_body = RigidBodyBuilder::dynamic().position(Isometry::translation(0.0, 1.0, 0.0)); + let body_part = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0).density(1.0); + colliders.insert_with_parent(collider, body_part, &mut bodies); + + + let joint = SphericalJointBuilder::new() + .local_anchor1(point![0.0, 4.0, 0.0]) + .local_anchor2(point![0.0, 0.0, 0.0]) + + .motor_position(JointAxis::AngX, 1.0, 1000.0, 200.0) + .motor_position(JointAxis::AngY, 0.0, 1000.0, 200.0) + .motor_position(JointAxis::AngZ, 0.0, 1000.0, 200.0); + + + multibody_joints.insert(body, body_part, joint, true); + + + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![20.0, 0.0, 0.0], point![0.0, 0.0, 0.0]); +} \ No newline at end of file From 2ed34bacf4c32cea5fa419a604b685822392c77a Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Tue, 26 Dec 2023 22:47:33 -0800 Subject: [PATCH 2/3] Fix dimforge#416 - update ball joint motor pos --- examples3d/spherical_joint_testing.rs | 4 ++-- src/dynamics/joint/multibody_joint/multibody_joint.rs | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/examples3d/spherical_joint_testing.rs b/examples3d/spherical_joint_testing.rs index e5f6acefb..7a601f94d 100644 --- a/examples3d/spherical_joint_testing.rs +++ b/examples3d/spherical_joint_testing.rs @@ -1,11 +1,11 @@ use rapier3d::prelude::*; -use rapier_testbed3d::{Testbed, TestbedApp}; +use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let impulse_joints = ImpulseJointSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 62fc43427..11ea890ce 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -115,6 +115,9 @@ impl MultibodyJoint { let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]); let disp = UnitQuaternion::new_eps(angvel * dt, 0.0); self.joint_rot = disp * self.joint_rot; + self.coords[3] += angvel[0] * dt; + self.coords[4] += angvel[1] * dt; + self.coords[5] += angvel[2] * dt; } _ => unreachable!(), } From 9fed726aa9c221fe1591784d1ea57c3f8d80ced6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 24 Jan 2024 23:06:08 +0100 Subject: [PATCH 3/3] chore: rename the multibody motor debug demo --- examples3d/all_examples3.rs | 8 +++++--- ...t_testing.rs => debug_multibody_ang_motor_pos3.rs} | 11 ++--------- 2 files changed, 7 insertions(+), 12 deletions(-) rename examples3d/{spherical_joint_testing.rs => debug_multibody_ang_motor_pos3.rs} (95%) diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 178fca776..1bc841978 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -38,6 +38,7 @@ mod debug_chain_high_mass_ratio3; mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; mod debug_long_chain3; +mod debug_multibody_ang_motor_pos3; mod joint_motor_position3; mod keva3; mod locked_rotations3; @@ -53,8 +54,6 @@ mod trimesh3; mod vehicle_controller3; mod vehicle_joints3; -mod spherical_joint_testing; - fn demo_name_from_command_line() -> Option { let mut args = std::env::args(); @@ -151,7 +150,10 @@ pub fn main() { debug_shape_modification3::init_world, ), ("(Debug) deserialize", debug_deserialize3::init_world), - ("Spherical Joint testing", spherical_joint_testing::init_world) + ( + "(Debug) multibody ang. motor pos.", + debug_multibody_ang_motor_pos3::init_world, + ), ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples3d/spherical_joint_testing.rs b/examples3d/debug_multibody_ang_motor_pos3.rs similarity index 95% rename from examples3d/spherical_joint_testing.rs rename to examples3d/debug_multibody_ang_motor_pos3.rs index 7a601f94d..60c820dd9 100644 --- a/examples3d/spherical_joint_testing.rs +++ b/examples3d/debug_multibody_ang_motor_pos3.rs @@ -2,37 +2,30 @@ use rapier3d::prelude::*; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { - let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let mut impulse_joints = ImpulseJointSet::new(); + let impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); - let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 0.0, 0.0]); let body = bodies.insert(ground); let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0); colliders.insert_with_parent(collider, body, &mut bodies); - let rigid_body = RigidBodyBuilder::dynamic().position(Isometry::translation(0.0, 1.0, 0.0)); let body_part = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0).density(1.0); colliders.insert_with_parent(collider, body_part, &mut bodies); - let joint = SphericalJointBuilder::new() .local_anchor1(point![0.0, 4.0, 0.0]) .local_anchor2(point![0.0, 0.0, 0.0]) - .motor_position(JointAxis::AngX, 1.0, 1000.0, 200.0) .motor_position(JointAxis::AngY, 0.0, 1000.0, 200.0) .motor_position(JointAxis::AngZ, 0.0, 1000.0, 200.0); - multibody_joints.insert(body, body_part, joint, true); - testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point![20.0, 0.0, 0.0], point![0.0, 0.0, 0.0]); -} \ No newline at end of file +}