diff options
| author | pellico <pellico@users.noreply.github.com> | 2022-11-27 18:47:35 +0100 |
|---|---|---|
| committer | Sébastien Crozet <sebcrozet@dimforge.com> | 2023-12-10 12:43:13 +0100 |
| commit | 6432909110c4b96e59d4ee2ebdae1d79abe8d4b3 (patch) | |
| tree | 691b88f6c151407f185c124c311123eb40cccccf /src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs | |
| parent | c33b4eeb5c113b8f8509cd866c4ab48e5d6c5eaa (diff) | |
| download | rapier-6432909110c4b96e59d4ee2ebdae1d79abe8d4b3.tar.gz rapier-6432909110c4b96e59d4ee2ebdae1d79abe8d4b3.tar.bz2 rapier-6432909110c4b96e59d4ee2ebdae1d79abe8d4b3.zip | |
Fix #378 Added one example join_motor_position
Diffstat (limited to 'src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs | 29 |
1 files changed, 21 insertions, 8 deletions
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..e6cb872 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -354,11 +354,18 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + 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; + } #[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.simd_sin(); + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } + } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -795,11 +802,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + 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; + } #[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.simd_sin(); + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); |