nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: avoid perpetual movement when the target ang motor position is overshot

Browse Source
This commit is contained in:
Sébastien Crozet
2023-07-10 09:14:16 +02:00
parent 9f3b5c8642
commit a05622cfe9
6 changed files with 165 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -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<Real, 1> {
pub fn fill_jacobians(
@@ -358,7 +357,7 @@ 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];
@@ -393,7 +392,7 @@ impl JointVelocityConstraintBuilder<Real> {
) -> 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<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.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<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];
@@ -800,18 +793,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.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);

41
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -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<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]);
@@ -353,19 +354,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.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<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.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<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]);

10
src/utils.rs
View File

@@ -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)
}