aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--CHANGELOG.md3
-rw-r--r--examples2d/all_examples2.rs2
-rw-r--r--examples2d/joint_motor_position2.rs79
-rw-r--r--examples3d/all_examples3.rs2
-rw-r--r--examples3d/joint_motor_position3.rs79
-rw-r--r--src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs75
-rw-r--r--src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs74
-rw-r--r--src/utils.rs10
8 files changed, 260 insertions, 64 deletions
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3224880..71576cd 100644
--- a/CHANGELOG.md
+++ b/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.
diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index d102b46..032e4d6 100644
--- a/examples2d/all_examples2.rs
+++ b/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),
];
diff --git a/examples2d/joint_motor_position2.rs b/examples2d/joint_motor_position2.rs
new file mode 100644
index 0000000..c8374ff
--- /dev/null
+++ b/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);
+}
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..0a15e63
--- /dev/null
+++ b/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]);
+}
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 1e7df89..c16151d 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,12 +6,15 @@ 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")]
+use na::Vector1;
impl SolverBody<Real, 1> {
pub fn fill_jacobians(
@@ -211,8 +214,8 @@ impl JointVelocityConstraintBuilder<Real> {
);
let dist = self.lin_err.dot(&lin_jac);
- let min_enabled = dist < limits[0];
- let max_enabled = limits[1] < dist;
+ let min_enabled = dist <= limits[0];
+ let max_enabled = limits[1] <= dist;
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -294,10 +297,13 @@ impl JointVelocityConstraintBuilder<Real> {
body2: &SolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
- locked_axis: usize,
+ _locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
- let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+ #[cfg(feature = "dim2")]
+ let ang_jac = Vector1::new(1.0);
+ #[cfg(feature = "dim3")]
+ let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
@@ -318,7 +324,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
- let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+ let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint
}
@@ -333,11 +339,14 @@ impl JointVelocityConstraintBuilder<Real> {
body2: &SolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
- limited_axis: usize,
+ _limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
- let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+ #[cfg(feature = "dim2")]
+ let ang_jac = Vector1::new(1.0);
+ #[cfg(feature = "dim3")]
+ let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
params,
@@ -356,11 +365,11 @@ impl JointVelocityConstraintBuilder<Real> {
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];
- let max_enabled = s_limits[1] < s_ang;
+ let s_ang = self.ang_err.imag()[_limited_axis];
+ let min_enabled = s_ang <= s_limits[0];
+ let max_enabled = s_limits[1] <= s_ang;
let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX,
@@ -389,7 +398,6 @@ impl JointVelocityConstraintBuilder<Real> {
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityConstraint {
- // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned();
#[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")]
@@ -413,11 +421,12 @@ impl JointVelocityConstraintBuilder<Real> {
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.im;
+ 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_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);
@@ -603,8 +612,8 @@ impl JointVelocityConstraintBuilder<Real> {
);
let dist = self.lin_err.dot(&lin_jac);
- let min_enabled = dist < limits[0];
- let max_enabled = limits[1] < dist;
+ let min_enabled = dist <= limits[0];
+ let max_enabled = limits[1] <= dist;
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -681,10 +690,13 @@ impl JointVelocityConstraintBuilder<Real> {
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
mb2: (&Multibody, usize),
- locked_axis: usize,
+ _locked_axis: usize,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
- let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+ #[cfg(feature = "dim2")]
+ let ang_jac = Vector1::new(1.0);
+ #[cfg(feature = "dim3")]
+ let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
@@ -703,7 +715,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
- let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+ let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint
}
@@ -716,11 +728,14 @@ impl JointVelocityConstraintBuilder<Real> {
joint_id: JointIndex,
body1: &SolverBody<Real, 1>,
mb2: (&Multibody, usize),
- limited_axis: usize,
+ _limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
- let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+ #[cfg(feature = "dim2")]
+ let ang_jac = Vector1::new(1.0);
+ #[cfg(feature = "dim3")]
+ let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground(
params,
@@ -737,11 +752,11 @@ impl JointVelocityConstraintBuilder<Real> {
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];
- let max_enabled = s_limits[1] < s_ang;
+ let s_ang = self.ang_err.imag()[_limited_axis];
+ let min_enabled = s_ang <= s_limits[0];
+ let max_enabled = s_limits[1] <= s_ang;
let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX,
@@ -769,7 +784,6 @@ impl JointVelocityConstraintBuilder<Real> {
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint {
- // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")]
@@ -791,11 +805,12 @@ impl JointVelocityConstraintBuilder<Real> {
let mut rhs = 0.0;
if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")]
- let s_ang_dist = self.ang_err.im;
+ 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_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 439030e..f7101a1 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;
@@ -94,8 +95,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
let dist = self.lin_err.dot(&constraint.lin_jac);
- let min_enabled = dist.simd_lt(limits[0]);
- let max_enabled = limits[1].simd_lt(dist);
+ let min_enabled = dist.simd_le(limits[0]);
+ let max_enabled = limits[1].simd_le(dist);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
@@ -281,7 +282,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
- limited_axis: usize,
+ _limited_axis: usize,
limits: [N; 2],
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
@@ -289,11 +290,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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]);
- let max_enabled = s_limits[1].simd_lt(s_ang);
+ let s_ang = self.ang_err.imag()[_limited_axis];
+ let min_enabled = s_ang.simd_le(s_limits[0]);
+ let max_enabled = s_limits[1].simd_le(s_ang);
let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -301,9 +302,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
];
#[cfg(feature = "dim2")]
- let ang_jac = self.ang_basis[limited_axis];
+ let ang_jac = N::one();
#[cfg(feature = "dim3")]
- let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+ let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
@@ -345,7 +346,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
motor_params: &MotorParameters<N>,
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
- // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")]
let ang_jac = N::one();
#[cfg(feature = "dim3")]
@@ -353,12 +353,15 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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.im;
+ 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_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);
@@ -393,13 +396,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
- locked_axis: usize,
+ _locked_axis: usize,
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
#[cfg(feature = "dim2")]
- let ang_jac = self.ang_basis[locked_axis];
+ let ang_jac = N::one();
#[cfg(feature = "dim3")]
- let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+ let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
@@ -409,7 +412,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
- let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+ let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -495,8 +498,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let lin_jac = self.basis.column(limited_axis).into_owned();
let dist = self.lin_err.dot(&lin_jac);
- let min_enabled = dist.simd_lt(limits[0]);
- let max_enabled = limits[1].simd_lt(dist);
+ let min_enabled = dist.simd_le(limits[0]);
+ let max_enabled = limits[1].simd_le(dist);
let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -786,7 +789,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
motor_params: &MotorParameters<N>,
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
- // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")]
let ang_jac = N::one();
#[cfg(feature = "dim3")]
@@ -794,12 +796,15 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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.im;
+ 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_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);
@@ -830,7 +835,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
- limited_axis: usize,
+ _limited_axis: usize,
limits: [N; 2],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
@@ -838,11 +843,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
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]);
- let max_enabled = s_limits[1].simd_lt(s_ang);
+ let s_ang = self.ang_err.imag()[_limited_axis];
+ let min_enabled = s_ang.simd_le(s_limits[0]);
+ let max_enabled = s_limits[1].simd_le(s_ang);
let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -850,9 +855,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
];
#[cfg(feature = "dim2")]
- let ang_jac = self.ang_basis[limited_axis];
+ let ang_jac = N::one();
#[cfg(feature = "dim3")]
- let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+ let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
@@ -887,13 +892,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
- locked_axis: usize,
+ _locked_axis: usize,
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
#[cfg(feature = "dim2")]
- let ang_jac = self.ang_basis[locked_axis];
+ let ang_jac = N::one();
#[cfg(feature = "dim3")]
- let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+ let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
+
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
@@ -902,7 +908,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
- let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+ let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
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<T> IndexMut2<usize> for [T] {
}
}
}
+
+/// Calculate the difference with smallest absolute value between the two given values.
+pub fn smallest_abs_diff_between_sin_angles<N: WReal>(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)
+}
generated by cgit (git 2.34.1) at 2026-01-30 01:50:33 +0000