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

Implement limits for ball joints.

Browse Source
This commit is contained in:
Sébastien Crozet
2021-08-07 18:20:19 +02:00
committed by Sébastien Crozet
parent ac77c95c9c
commit f7643272f4
6 changed files with 366 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
examples3d/joints3.rs
View File

@@ -368,6 +368,58 @@ fn create_ball_joints(
}
}
fn create_ball_joints_with_limits(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point<f32>,
) {
let shift = vector![0.0, 0.0, 3.0];
let ground = bodies.insert(
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let ball1 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift)
.linvel(vector![20.0, 20.0, 0.0])
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball1,
bodies,
);
let ball2 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift * 2.0)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball2,
bodies,
);
let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift));
joint1.limits_enabled = true;
joint1.limits_local_axis1 = Vector::z_axis();
joint1.limits_local_axis2 = Vector::z_axis();
joint1.limits_angle = 0.2;
joints.insert(ground, ball1, joint1);
let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift));
joint2.limits_enabled = true;
joint2.limits_local_axis1 = Vector::z_axis();
joint2.limits_local_axis2 = Vector::z_axis();
joint2.limits_angle = 0.3;
joints.insert(ball1, ball2, joint2);
}
fn create_actuated_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
@@ -549,6 +601,12 @@ pub fn init_world(testbed: &mut Testbed) {
3,
);
create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
create_ball_joints_with_limits(
&mut bodies,
&mut colliders,
&mut joints,
point![-5.0, 0.0, 0.0],
);
/*
* Set up the testbed.

24
src/dynamics/joint/ball_joint.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::SpringModel;
use crate::math::{Point, Real, Rotation, Vector};
use crate::math::{Point, Real, Rotation, UnitVector, Vector};
#[derive(Copy, Clone, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -38,6 +38,17 @@ pub struct BallJoint {
pub motor_impulse: Vector<Real>,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
/// Are the limits enabled for this joint?
pub limits_enabled: bool,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis1: UnitVector<Real>,
/// The axis of the limit cone for this joint, if the local-space of the first body.
pub limits_local_axis2: UnitVector<Real>,
/// The maximum angle allowed between the two limit axes in world-space.
pub limits_angle: Real,
/// The impulse applied to enforce joint limits.
pub limits_impulse: Real,
}
impl BallJoint {
@@ -62,13 +73,20 @@ impl BallJoint {
motor_impulse: na::zero(),
motor_max_impulse: Real::MAX,
motor_model: SpringModel::default(),
limits_enabled: false,
limits_local_axis1: Vector::x_axis(),
limits_local_axis2: Vector::x_axis(),
limits_angle: Real::MAX,
limits_impulse: 0.0,
}
}
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD ball constraints don't support motors right now.
self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
// SIMD ball constraints don't support motors and limits right now.
!self.limits_enabled
&& (self.motor_max_impulse == 0.0
|| (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.

3
src/dynamics/joint/revolute_joint.rs
View File

@@ -93,7 +93,7 @@ impl RevoluteJoint {
/// Can a SIMD constraint be used for resolving this joint?
pub fn supports_simd_constraints(&self) -> bool {
// SIMD revolute constraints don't support motors right now.
// SIMD revolute constraints don't support motors and limits right now.
!self.limits_enabled
&& (self.motor_max_impulse == 0.0
|| (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
@@ -142,6 +142,7 @@ impl RevoluteJoint {
)
}
/// Estimates the current position of the motor angle given the joint parameters.
pub fn estimate_motor_angle_from_params(
axis1: &Unit<Vector<Real>>,
tangent1: &Vector<Real>,

78
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -3,7 +3,7 @@ use crate::dynamics::{
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[derive(Debug)]
@@ -19,9 +19,15 @@ pub(crate) struct BallPositionConstraint {
ii1: AngularInertia<Real>,
ii2: AngularInertia<Real>,
inv_ii1_ii2: AngularInertia<Real>,
local_anchor1: Point<Real>,
local_anchor2: Point<Real>,
limits_enabled: bool,
limits_angle: Real,
limits_local_axis1: UnitVector<Real>,
limits_local_axis2: UnitVector<Real>,
}
impl BallPositionConstraint {
@@ -33,17 +39,26 @@ impl BallPositionConstraint {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let inv_ii1_ii2 = (ii1 + ii2).inverse();
Self {
local_com1: mprops1.local_mprops.local_com,
local_com2: mprops2.local_mprops.local_com,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
ii1,
ii2,
inv_ii1_ii2,
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
limits_enabled: cparams.limits_enabled,
limits_angle: cparams.limits_angle,
limits_local_axis1: cparams.limits_local_axis1,
limits_local_axis2: cparams.limits_local_axis2,
}
}
@@ -96,6 +111,31 @@ impl BallPositionConstraint {
position1.rotation = Rotation::new(angle1) * position1.rotation;
position2.rotation = Rotation::new(angle2) * position2.rotation;
/*
* Limits part.
*/
if self.limits_enabled {
let axis1 = position1 * self.limits_local_axis1;
let axis2 = position2 * self.limits_local_axis2;
// TODO: handle the case where dot(axis1, axis2) = -1.0
if let Some((axis, angle)) =
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle())
{
if angle >= self.limits_angle {
let ang_error = angle - self.limits_angle;
let ang_impulse = self
.inv_ii1_ii2
.transform_vector(*axis * ang_error * params.joint_erp);
position1.rotation =
Rotation::new(self.ii1.transform_vector(-ang_impulse)) * position1.rotation;
position2.rotation =
Rotation::new(self.ii2.transform_vector(ang_impulse)) * position2.rotation;
}
}
}
positions[self.position1 as usize] = position1;
positions[self.position2 as usize] = position2;
}
@@ -109,6 +149,11 @@ pub(crate) struct BallPositionGroundConstraint {
ii2: AngularInertia<Real>,
local_anchor2: Point<Real>,
local_com2: Point<Real>,
limits_enabled: bool,
limits_angle: Real,
limits_axis1: UnitVector<Real>,
limits_local_axis2: UnitVector<Real>,
}
impl BallPositionGroundConstraint {
@@ -132,6 +177,10 @@ impl BallPositionGroundConstraint {
local_anchor2: cparams.local_anchor1,
position2: ids2.active_set_offset,
local_com2: mprops2.local_mprops.local_com,
limits_enabled: cparams.limits_enabled,
limits_angle: cparams.limits_angle,
limits_axis1: poss1.next_position * cparams.limits_local_axis2,
limits_local_axis2: cparams.limits_local_axis1,
}
} else {
Self {
@@ -141,6 +190,10 @@ impl BallPositionGroundConstraint {
local_anchor2: cparams.local_anchor2,
position2: ids2.active_set_offset,
local_com2: mprops2.local_mprops.local_com,
limits_enabled: cparams.limits_enabled,
limits_angle: cparams.limits_angle,
limits_axis1: poss1.next_position * cparams.limits_local_axis1,
limits_local_axis2: cparams.limits_local_axis2,
}
}
}
@@ -172,6 +225,25 @@ impl BallPositionGroundConstraint {
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
position2.rotation = Rotation::new(angle2) * position2.rotation;
/*
* Limits part.
*/
if self.limits_enabled {
let axis2 = position2 * self.limits_local_axis2;
// TODO: handle the case where dot(axis1, axis2) = -1.0
if let Some((axis, angle)) = Rotation::rotation_between_axis(&axis2, &self.limits_axis1)
.and_then(|r| r.axis_angle())
{
if angle >= self.limits_angle {
let ang_error = angle - self.limits_angle;
let ang_correction = *axis * ang_error * params.joint_erp;
position2.rotation = Rotation::new(ang_correction) * position2.rotation;
}
}
}
positions[self.position2 as usize] = position2;
}
}

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

@@ -3,7 +3,7 @@ use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
use crate::math::{AngVector, AngularInertia, Real, Rotation, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[derive(Debug)]
@@ -26,6 +26,12 @@ pub(crate) struct BallVelocityConstraint {
motor_inv_lhs: Option<AngularInertia<Real>>,
motor_max_impulse: Real,
limits_active: bool,
limits_rhs: Real,
limits_inv_lhs: Real,
limits_impulse: Real,
limits_axis: Vector<Real>,
im1: Real,
im2: Real,
@@ -51,18 +57,18 @@ impl BallVelocityConstraint {
),
joint: &BallJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1;
let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
let anchor_world1 = poss1.position * joint.local_anchor1;
let anchor_world2 = poss2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let anchor_world1 = rb_pos1.position * joint.local_anchor1;
let anchor_world2 = rb_pos2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - rb_mprops1.world_com;
let anchor2 = anchor_world2 - rb_mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
let im1 = rb_mprops1.effective_inv_mass;
let im2 = rb_mprops2.effective_inv_mass;
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -73,12 +79,12 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
{
lhs = mprops2
lhs = rb_mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2)
+ mprops1
+ rb_mprops1
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat1)
@@ -89,8 +95,8 @@ impl BallVelocityConstraint {
// it's just easier that way.
#[cfg(feature = "dim2")]
{
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -114,8 +120,8 @@ impl BallVelocityConstraint {
);
if stiffness != 0.0 {
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
let dpos = rb_pos2.position.rotation
* (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -127,15 +133,15 @@ impl BallVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = vels2.angvel - vels1.angvel;
let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / (ii1 + ii2))
} else {
Some(gamma)
@@ -146,8 +152,8 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
Some((ii1 + ii2).inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -163,10 +169,47 @@ impl BallVelocityConstraint {
let motor_impulse =
joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
/*
* Setup the limits constraint.
*/
let mut limits_active = false;
let mut limits_rhs = 0.0;
let mut limits_inv_lhs = 0.0;
let mut limits_impulse = 0.0;
let mut limits_axis = Vector::zeros();
if joint.limits_enabled {
let axis1 = rb_pos1.position * joint.limits_local_axis1;
let axis2 = rb_pos2.position * joint.limits_local_axis2;
if let Some((axis, angle)) =
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle())
{
// TODO: we should allow predictive constraint activation.
if angle >= joint.limits_angle {
limits_active = true;
limits_rhs = (rb_vels2.angvel.dot(&axis) - rb_vels1.angvel.dot(&axis))
* params.velocity_solve_fraction;
limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
limits_inv_lhs = crate::utils::inv(
axis.dot(&ii2.transform_vector(*axis))
+ axis.dot(&ii1.transform_vector(*axis)),
);
limits_impulse = joint.limits_impulse * params.warmstart_coeff;
limits_axis = *axis;
}
}
}
BallVelocityConstraint {
joint_id,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
mj_lambda1: rb_ids1.active_set_offset,
mj_lambda2: rb_ids2.active_set_offset,
im1,
im2,
impulse: joint.impulse * params.warmstart_coeff,
@@ -178,8 +221,13 @@ impl BallVelocityConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
limits_active,
limits_axis,
limits_rhs,
limits_inv_lhs,
limits_impulse,
}
}
@@ -196,6 +244,16 @@ impl BallVelocityConstraint {
.ii2_sqrt
.transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
/*
* Warmstart limits.
*/
if self.limits_active {
let limit_impulse1 = -self.limits_axis * self.limits_impulse;
let limit_impulse2 = self.limits_axis * self.limits_impulse;
mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1);
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
}
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
@@ -217,6 +275,29 @@ impl BallVelocityConstraint {
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
}
fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
if self.limits_active {
let limits_torquedir1 = -self.limits_axis;
let limits_torquedir2 = self.limits_axis;
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let ang_dvel = limits_torquedir1.dot(&ang_vel1)
+ limits_torquedir2.dot(&ang_vel2)
+ self.limits_rhs;
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
let dimpulse = new_impulse - self.limits_impulse;
self.limits_impulse = new_impulse;
let ang_impulse1 = limits_torquedir1 * dimpulse;
let ang_impulse2 = limits_torquedir2 * dimpulse;
mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1);
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
}
}
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -244,6 +325,7 @@ impl BallVelocityConstraint {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
@@ -256,6 +338,7 @@ impl BallVelocityConstraint {
if let JointParams::BallJoint(ball) = &mut joint.params {
ball.impulse = self.impulse;
ball.motor_impulse = self.motor_impulse;
ball.limits_impulse = self.limits_impulse;
}
}
}
@@ -275,6 +358,12 @@ pub(crate) struct BallVelocityGroundConstraint {
motor_inv_lhs: Option<AngularInertia<Real>>,
motor_max_impulse: Real,
limits_active: bool,
limits_rhs: Real,
limits_inv_lhs: Real,
limits_impulse: Real,
limits_axis: Vector<Real>,
im2: Real,
ii2_sqrt: AngularInertia<Real>,
}
@@ -293,27 +382,27 @@ impl BallVelocityGroundConstraint {
joint: &BallJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (rb_pos1, rb_vels1, rb_mprops1) = rb1;
let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
let (anchor_world1, anchor_world2) = if flipped {
(
poss1.position * joint.local_anchor2,
poss2.position * joint.local_anchor1,
rb_pos1.position * joint.local_anchor2,
rb_pos2.position * joint.local_anchor1,
)
} else {
(
poss1.position * joint.local_anchor1,
poss2.position * joint.local_anchor2,
rb_pos1.position * joint.local_anchor1,
rb_pos2.position * joint.local_anchor2,
)
};
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let anchor1 = anchor_world1 - rb_mprops1.world_com;
let anchor2 = anchor_world2 - rb_mprops2.world_com;
let im2 = mprops2.effective_inv_mass;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let im2 = rb_mprops2.effective_inv_mass;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -324,7 +413,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
lhs = mprops2
lhs = rb_mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
@@ -333,7 +422,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim2")]
{
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im2 + cmat2.x * cmat2.x * ii2;
let m12 = cmat2.x * cmat2.y * ii2;
let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -357,8 +446,8 @@ impl BallVelocityGroundConstraint {
);
if stiffness != 0.0 {
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
let dpos = rb_pos2.position.rotation
* (rb_pos1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -370,14 +459,14 @@ impl BallVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = vels2.angvel - vels1.angvel;
let curr_vel = rb_vels2.angvel - rb_vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / ii2)
} else {
Some(gamma)
@@ -388,7 +477,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
Some(ii2.inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -404,9 +493,50 @@ impl BallVelocityGroundConstraint {
let motor_impulse =
joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
/*
* Setup the limits constraint.
*/
let mut limits_active = false;
let mut limits_rhs = 0.0;
let mut limits_inv_lhs = 0.0;
let mut limits_impulse = 0.0;
let mut limits_axis = Vector::zeros();
if joint.limits_enabled {
let (axis1, axis2) = if flipped {
(
rb_pos1.position * joint.limits_local_axis2,
rb_pos2.position * joint.limits_local_axis1,
)
} else {
(
rb_pos1.position * joint.limits_local_axis1,
rb_pos2.position * joint.limits_local_axis2,
)
};
if let Some((axis, angle)) =
Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle())
{
// TODO: we should allow predictive constraint activation.
if angle >= joint.limits_angle {
limits_active = true;
limits_rhs = (rb_vels2.angvel.dot(&axis) - rb_vels1.angvel.dot(&axis))
* params.velocity_solve_fraction;
limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt();
let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared();
limits_inv_lhs = crate::utils::inv(axis.dot(&ii2.transform_vector(*axis)));
limits_impulse = joint.limits_impulse * params.warmstart_coeff;
limits_axis = *axis;
}
}
}
BallVelocityGroundConstraint {
joint_id,
mj_lambda2: ids2.active_set_offset,
mj_lambda2: rb_ids2.active_set_offset,
im2,
impulse: joint.impulse * params.warmstart_coeff,
r2: anchor2,
@@ -416,7 +546,12 @@ impl BallVelocityGroundConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt,
limits_active,
limits_axis,
limits_rhs,
limits_inv_lhs,
limits_impulse,
}
}
@@ -426,6 +561,15 @@ impl BallVelocityGroundConstraint {
mj_lambda2.angular -= self
.ii2_sqrt
.transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
/*
* Warmstart limits.
*/
if self.limits_active {
let limit_impulse2 = self.limits_axis * self.limits_impulse;
mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2);
}
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
@@ -441,6 +585,21 @@ impl BallVelocityGroundConstraint {
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
}
fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
if self.limits_active {
let limits_torquedir2 = self.limits_axis;
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let ang_dvel = limits_torquedir2.dot(&ang_vel2) + self.limits_rhs;
let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0);
let dimpulse = new_impulse - self.limits_impulse;
self.limits_impulse = new_impulse;
let ang_impulse2 = limits_torquedir2 * dimpulse;
mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2);
}
}
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -464,6 +623,7 @@ impl BallVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
self.solve_limits(&mut mj_lambda2);
self.solve_dofs(&mut mj_lambda2);
self.solve_motors(&mut mj_lambda2);
@@ -476,6 +636,7 @@ impl BallVelocityGroundConstraint {
if let JointParams::BallJoint(ball) = &mut joint.params {
ball.impulse = self.impulse;
ball.motor_impulse = self.motor_impulse;
ball.limits_impulse = self.limits_impulse;
}
}
}

4
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -294,7 +294,9 @@ impl RevoluteVelocityConstraint {
.transform_vector(self.motor_axis2 * self.motor_impulse);
}
// Warmstart limits.
/*
* Warmstart limits.
*/
if self.limits_active {
let limit_impulse1 = -self.motor_axis2 * self.limits_impulse;
let limit_impulse2 = self.motor_axis2 * self.limits_impulse;