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

Take max motor impulse into account for the ball joint.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-22 10:15:13 +01:00
parent cc8dc13fc0
commit f204a5f736
1 changed files with 114 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -23,6 +23,7 @@ pub(crate) struct BallVelocityConstraint {
motor_rhs: AngVector<Real>, motor_rhs: AngVector<Real>,
motor_impulse: AngVector<Real>, motor_impulse: AngVector<Real>,
motor_inv_lhs: Option<AngularInertia<Real>>, motor_inv_lhs: Option<AngularInertia<Real>>,
motor_max_impulse: Real,
im1: Real, im1: Real,
im2: Real, im2: Real,
@@ -88,6 +89,7 @@ impl BallVelocityConstraint {
let mut motor_inv_lhs = None; let mut motor_inv_lhs = None;
let motor_max_impulse = joint.motor_max_impulse; let motor_max_impulse = joint.motor_max_impulse;
if motor_max_impulse > 0.0 {
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
params.dt, params.dt,
joint.motor_stiffness, joint.motor_stiffness,
@@ -95,8 +97,8 @@ impl BallVelocityConstraint {
); );
if stiffness != 0.0 { if stiffness != 0.0 {
let dpos = let dpos = rb2.position.rotation
rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); * (rb1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
motor_rhs += dpos.angle() * stiffness; motor_rhs += dpos.angle() * stiffness;
@@ -135,17 +137,14 @@ impl BallVelocityConstraint {
}; };
motor_rhs /= gamma; motor_rhs /= gamma;
} }
}
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
* params.warmstart_coeff; * params.warmstart_coeff;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let motor_impulse = joint let motor_impulse =
.motor_impulse joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
.try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6)
.unwrap_or_else(na::zero)
* params.warmstart_coeff;
BallVelocityConstraint { BallVelocityConstraint {
joint_id, joint_id,
@@ -161,6 +160,7 @@ impl BallVelocityConstraint {
motor_rhs, motor_rhs,
motor_impulse, motor_impulse,
motor_inv_lhs, motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
} }
@@ -210,11 +210,19 @@ impl BallVelocityConstraint {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs; let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs;
let impulse = motor_inv_lhs.transform_vector(dangvel);
self.motor_impulse += impulse;
mj_lambda1.angular += self.ii1_sqrt.transform_vector(impulse); let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse);
#[cfg(feature = "dim2")]
let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse);
#[cfg(feature = "dim3")]
let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
let effective_impulse = clamped_impulse - self.motor_impulse;
self.motor_impulse = clamped_impulse;
mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
} }
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
@@ -242,6 +250,7 @@ pub(crate) struct BallVelocityGroundConstraint {
motor_rhs: AngVector<Real>, motor_rhs: AngVector<Real>,
motor_impulse: AngVector<Real>, motor_impulse: AngVector<Real>,
motor_inv_lhs: Option<AngularInertia<Real>>, motor_inv_lhs: Option<AngularInertia<Real>>,
motor_max_impulse: Real,
im2: Real, im2: Real,
ii2_sqrt: AngularInertia<Real>, ii2_sqrt: AngularInertia<Real>,
@@ -304,6 +313,7 @@ impl BallVelocityGroundConstraint {
let mut motor_inv_lhs = None; let mut motor_inv_lhs = None;
let motor_max_impulse = joint.motor_max_impulse; let motor_max_impulse = joint.motor_max_impulse;
if motor_max_impulse > 0.0 {
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
params.dt, params.dt,
joint.motor_stiffness, joint.motor_stiffness,
@@ -311,8 +321,8 @@ impl BallVelocityGroundConstraint {
); );
if stiffness != 0.0 { if stiffness != 0.0 {
let dpos = let dpos = rb2.position.rotation
rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); * (rb1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
motor_rhs += dpos.angle() * stiffness; motor_rhs += dpos.angle() * stiffness;
@@ -349,17 +359,14 @@ impl BallVelocityGroundConstraint {
}; };
motor_rhs /= gamma; motor_rhs /= gamma;
} }
}
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
* params.warmstart_coeff; * params.warmstart_coeff;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let motor_impulse = joint let motor_impulse =
.motor_impulse joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
.try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6)
.unwrap_or_else(na::zero)
* params.warmstart_coeff;
BallVelocityGroundConstraint { BallVelocityGroundConstraint {
joint_id, joint_id,
@@ -372,6 +379,7 @@ impl BallVelocityGroundConstraint {
motor_rhs, motor_rhs,
motor_impulse, motor_impulse,
motor_inv_lhs, motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
} }
} }
@@ -405,10 +413,17 @@ impl BallVelocityGroundConstraint {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let dangvel = ang_vel2 + self.motor_rhs; let dangvel = ang_vel2 + self.motor_rhs;
let impulse = motor_inv_lhs.transform_vector(dangvel); let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
self.motor_impulse += impulse;
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse); #[cfg(feature = "dim2")]
let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse);
#[cfg(feature = "dim3")]
let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
let effective_impulse = clamped_impulse - self.motor_impulse;
self.motor_impulse = clamped_impulse;
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
} }
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;