From 6432909110c4b96e59d4ee2ebdae1d79abe8d4b3 Mon Sep 17 00:00:00 2001 From: pellico Date: Sun, 27 Nov 2022 18:47:35 +0100 Subject: Fix #378 Added one example join_motor_position --- .../joint_velocity_constraint_builder.rs | 29 ++++++++++++++++------ 1 file changed, 21 insertions(+), 8 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs') 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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); -- cgit