From a05622cfe995cb681f01bc22956addf99c208cc6 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 10 Jul 2023 09:14:16 +0200 Subject: fix: avoid perpetual movement when the target ang motor position is overshot --- examples2d/joint_motor_position2.rs | 44 ++++++++++-- examples3d/all_examples3.rs | 2 + examples3d/joint_motor_position3.rs | 84 ++++++++++++++++++++++ .../joint_generic_velocity_constraint_builder.rs | 41 ++++------- .../joint_velocity_constraint_builder.rs | 41 +++++------ src/utils.rs | 10 +++ 6 files changed, 165 insertions(+), 57 deletions(-) create mode 100644 examples3d/joint_motor_position3.rs diff --git a/examples2d/joint_motor_position2.rs b/examples2d/joint_motor_position2.rs index a701bd0..c44ea13 100644 --- a/examples2d/joint_motor_position2.rs +++ b/examples2d/joint_motor_position2.rs @@ -22,20 +22,52 @@ pub fn init_world(testbed: &mut Testbed) { colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* - * A rectangle on a motor + * 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]); + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![x_pos, 2.0]) + .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_anchor2(point![x_pos, 1.5]) - .local_anchor1(point![0.0, -0.5]) - .motor_position(std::f32::consts::PI / 4.0 * num as f32, 100.0, 15.0); - impulse_joints.insert(handle, ground_handle, joint, true); + .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 2..3 { + let x_pos = -6.0 + 1.5 * num as f32; + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![x_pos, 5.0]) + .angvel(4.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, 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 / 4.0 * num as f32, + ]); + impulse_joints.insert(ground_handle, handle, joint, true); } /* diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index e51e185..ad4d226 100644 --- a/examples3d/all_examples3.rs +++ b/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), diff --git a/examples3d/joint_motor_position3.rs b/examples3d/joint_motor_position3.rs new file mode 100644 index 0000000..d149d82 --- /dev/null +++ b/examples3d/joint_motor_position3.rs @@ -0,0 +1,84 @@ +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(); + + /* + * The ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * 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..9 { + let x_pos = -6.0 + 1.5 * num as f32; + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![x_pos, 5.0, 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 / 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]); +} diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 50a37e2..a38f9d3 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -6,14 +6,13 @@ use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, use crate::dynamics::solver::MotorParameters; use crate::dynamics::{IntegrationParameters, JointIndex, Multibody}; use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM}; +use crate::utils; use crate::utils::IndexMut2; use crate::utils::WDot; use na::{DVector, SVector}; #[cfg(feature = "dim3")] use crate::utils::WAngularInertia; -#[cfg(feature = "dim2")] -const PI: Real = std::f64::consts::PI as Real; impl SolverBody { pub fn fill_jacobians( @@ -358,7 +357,7 @@ impl JointVelocityConstraintBuilder { let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang < s_limits[0]; @@ -393,7 +392,7 @@ impl JointVelocityConstraintBuilder { ) -> JointGenericVelocityConstraint { // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned(); #[cfg(feature = "dim2")] - let ang_jac = na::Vector1::new(1.0); + let ang_jac = na::Vector1::new(1.0); // self.ang_basis[(0, 0)]); #[cfg(feature = "dim3")] let ang_jac = self.basis.column(_motor_axis).into_owned(); @@ -415,18 +414,12 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos / 2.0).sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -746,7 +739,7 @@ impl JointVelocityConstraintBuilder { let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang < s_limits[0]; @@ -800,18 +793,12 @@ impl JointVelocityConstraintBuilder { let mut rhs = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos / 2.0).sin(); + rhs += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 2fae33a..b8b0fe4 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -5,6 +5,7 @@ use crate::dynamics::solver::joint_constraint::SolverBody; use crate::dynamics::solver::MotorParameters; use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; +use crate::utils; use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use na::SMatrix; @@ -289,7 +290,7 @@ impl JointVelocityConstraintBuilder { let half = N::splat(0.5); let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang.simd_lt(s_limits[0]); @@ -353,19 +354,15 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { + let half = N::splat(0.5); + #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos * half).simd_sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -801,19 +798,15 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { + let half = N::splat(0.5); + #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos * half).simd_sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -852,7 +845,7 @@ impl JointVelocityConstraintBuilder { let half = N::splat(0.5); let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang.simd_lt(s_limits[0]); diff --git a/src/utils.rs b/src/utils.rs index 4cbc398..90401a4 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -804,3 +804,13 @@ impl IndexMut2 for [T] { } } } + +/// Calculate the difference with smallest absolute value between the two given values. +pub fn smallest_abs_diff_between_sin_angles(a: N, b: N) -> N { + // Select the smallest path among the two angles to reach the target. + let s_err = a - b; + let sgn = s_err.simd_signum(); + let s_err_complement = s_err - sgn * N::splat(2.0); + let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs()); + s_err.select(s_err_is_smallest, s_err_complement) +} -- cgit