diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs | 30 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs | 29 |
2 files changed, 43 insertions, 16 deletions
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..4156680 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 @@ -12,6 +12,8 @@ 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<Real, 1> { pub fn fill_jacobians( @@ -413,11 +415,17 @@ 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(); + 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; + } #[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.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); @@ -791,11 +799,17 @@ 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(); + 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; + } #[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.sin(); + rhs += (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..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); |