Various generic joint fixes.
This commit is contained in:
Crozet Sébastien
@@ -1,4 +1,4 @@
|
||||
use crate::dynamics::RevoluteJoint;
|
||||
use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
|
||||
use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
|
||||
use crate::na::{Rotation3, UnitQuaternion};
|
||||
|
||||
@@ -24,11 +24,16 @@ pub struct GenericJoint {
|
||||
|
||||
pub min_position: SpacialVector<Real>,
|
||||
pub max_position: SpacialVector<Real>,
|
||||
pub target_velocity: SpacialVector<Real>,
|
||||
/// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0
|
||||
pub max_negative_impulse: SpacialVector<Real>,
|
||||
pub min_velocity: SpacialVector<Real>,
|
||||
pub max_velocity: SpacialVector<Real>,
|
||||
/// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0
|
||||
pub min_impulse: SpacialVector<Real>,
|
||||
/// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
|
||||
pub max_positive_impulse: SpacialVector<Real>,
|
||||
pub max_impulse: SpacialVector<Real>,
|
||||
/// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0
|
||||
pub min_pos_impulse: SpacialVector<Real>,
|
||||
/// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0
|
||||
pub max_pos_impulse: SpacialVector<Real>,
|
||||
}
|
||||
|
||||
impl GenericJoint {
|
||||
@@ -40,25 +45,78 @@ impl GenericJoint {
|
||||
impulse: SpacialVector::zeros(),
|
||||
min_position: SpacialVector::zeros(),
|
||||
max_position: SpacialVector::zeros(),
|
||||
target_velocity: SpacialVector::zeros(),
|
||||
max_negative_impulse: SpacialVector::repeat(-Real::MAX),
|
||||
max_positive_impulse: SpacialVector::repeat(Real::MAX),
|
||||
min_velocity: SpacialVector::zeros(),
|
||||
max_velocity: SpacialVector::zeros(),
|
||||
min_impulse: SpacialVector::repeat(-Real::MAX),
|
||||
max_impulse: SpacialVector::repeat(Real::MAX),
|
||||
min_pos_impulse: SpacialVector::repeat(-Real::MAX),
|
||||
max_pos_impulse: SpacialVector::repeat(Real::MAX),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn free_dof(&mut self, dof: u8) {
|
||||
self.min_position[dof as usize] = -Real::MAX;
|
||||
self.max_position[dof as usize] = Real::MAX;
|
||||
self.min_velocity[dof as usize] = -Real::MAX;
|
||||
self.max_velocity[dof as usize] = Real::MAX;
|
||||
self.min_impulse[dof as usize] = 0.0;
|
||||
self.max_impulse[dof as usize] = 0.0;
|
||||
self.min_pos_impulse[dof as usize] = 0.0;
|
||||
self.max_pos_impulse[dof as usize] = 0.0;
|
||||
}
|
||||
|
||||
pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) {
|
||||
self.min_position[dof as usize] = min;
|
||||
self.max_position[dof as usize] = max;
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RevoluteJoint> for GenericJoint {
|
||||
fn from(joint: RevoluteJoint) -> Self {
|
||||
let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]];
|
||||
let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]];
|
||||
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]);
|
||||
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]);
|
||||
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
|
||||
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
|
||||
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
|
||||
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
|
||||
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
|
||||
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
|
||||
|
||||
let mut result = Self::new(local_anchor1, local_anchor2);
|
||||
result.min_position[3] = -Real::MAX;
|
||||
result.max_position[3] = Real::MAX;
|
||||
result.free_dof(3);
|
||||
result
|
||||
}
|
||||
}
|
||||
|
||||
impl From<BallJoint> for GenericJoint {
|
||||
fn from(joint: BallJoint) -> Self {
|
||||
let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero());
|
||||
let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
|
||||
|
||||
let mut result = Self::new(local_anchor1, local_anchor2);
|
||||
result.free_dof(3);
|
||||
result.free_dof(4);
|
||||
result.free_dof(5);
|
||||
result
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PrismaticJoint> for GenericJoint {
|
||||
fn from(joint: PrismaticJoint) -> Self {
|
||||
let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
|
||||
let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
|
||||
let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
|
||||
let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
|
||||
let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
|
||||
let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
|
||||
|
||||
let mut result = Self::new(local_anchor1, local_anchor2);
|
||||
result.free_dof(0);
|
||||
result.set_dof_limits(0, joint.limits[0], joint.limits[1]);
|
||||
result
|
||||
}
|
||||
}
|
||||
|
||||
impl From<FixedJoint> for GenericJoint {
|
||||
fn from(joint: FixedJoint) -> Self {
|
||||
Self::new(joint.local_anchor1, joint.local_anchor2)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -48,57 +48,7 @@ impl GenericPositionConstraint {
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position1 = positions[self.position1 as usize];
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
|
||||
let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
|
||||
|
||||
let delta_pos = Isometry::from_parts(
|
||||
anchor2.translation * anchor1.translation.inverse(),
|
||||
anchor2.rotation * anchor1.rotation.inverse(),
|
||||
);
|
||||
|
||||
let mass_matrix = GenericVelocityConstraint::compute_mass_matrix(
|
||||
&self.joint,
|
||||
self.im1,
|
||||
self.im2,
|
||||
self.ii1,
|
||||
self.ii2,
|
||||
r1,
|
||||
r2,
|
||||
false,
|
||||
);
|
||||
|
||||
let lin_dpos = delta_pos.translation.vector;
|
||||
let ang_dpos = delta_pos.rotation.scaled_axis();
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
let err = dpos
|
||||
- dpos
|
||||
.sup(&self.joint.min_position)
|
||||
.inf(&self.joint.max_position);
|
||||
let impulse = mass_matrix * err;
|
||||
let lin_impulse = impulse.xyz();
|
||||
let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
|
||||
|
||||
position1.rotation = Rotation::new(
|
||||
self.ii1
|
||||
.transform_vector(ang_impulse + r1.gcross(lin_impulse)),
|
||||
) * position1.rotation;
|
||||
position2.rotation = Rotation::new(
|
||||
self.ii2
|
||||
.transform_vector(-ang_impulse - r2.gcross(lin_impulse)),
|
||||
) * position2.rotation;
|
||||
|
||||
position1.translation.vector += self.im1 * lin_impulse;
|
||||
position2.translation.vector -= self.im2 * lin_impulse;
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
return;
|
||||
}
|
||||
|
||||
pub fn solve2(
|
||||
@@ -152,43 +102,7 @@ impl GenericPositionGroundConstraint {
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
|
||||
|
||||
let delta_pos = Isometry::from_parts(
|
||||
anchor2.translation * self.anchor1.translation.inverse(),
|
||||
anchor2.rotation * self.anchor1.rotation.inverse(),
|
||||
);
|
||||
let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix(
|
||||
&self.joint,
|
||||
self.im2,
|
||||
self.ii2,
|
||||
r2,
|
||||
false,
|
||||
);
|
||||
|
||||
let lin_dpos = delta_pos.translation.vector;
|
||||
let ang_dpos = delta_pos.rotation.scaled_axis();
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
let err = dpos
|
||||
- dpos
|
||||
.sup(&self.joint.min_position)
|
||||
.inf(&self.joint.max_position);
|
||||
let impulse = mass_matrix * err;
|
||||
let lin_impulse = impulse.xyz();
|
||||
let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
|
||||
|
||||
position2.rotation = Rotation::new(
|
||||
self.ii2
|
||||
.transform_vector(-ang_impulse - r2.gcross(lin_impulse)),
|
||||
) * position2.rotation;
|
||||
position2.translation.vector -= self.im2 * lin_impulse;
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
return;
|
||||
}
|
||||
|
||||
pub fn solve2(
|
||||
|
||||
@@ -2,7 +2,8 @@ use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
||||
};
|
||||
use crate::math::{AngularInertia, Dim, Isometry, Real, SpacialVector, Vector, DIM};
|
||||
use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM};
|
||||
use crate::na::UnitQuaternion;
|
||||
use crate::parry::math::{AngDim, SpatialVector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
#[cfg(feature = "dim2")]
|
||||
@@ -17,23 +18,10 @@ pub(crate) struct GenericVelocityConstraint {
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
impulse: SpacialVector<Real>,
|
||||
pos_impulse: SpacialVector<Real>,
|
||||
|
||||
max_positive_impulse: SpatialVector<Real>,
|
||||
max_negative_impulse: SpatialVector<Real>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<Real>,
|
||||
|
||||
inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky?
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<Real>,
|
||||
|
||||
pos_rhs: Vector6<Real>,
|
||||
inv_lhs: Matrix3<Real>,
|
||||
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
@@ -46,79 +34,147 @@ pub(crate) struct GenericVelocityConstraint {
|
||||
|
||||
r1: Vector<Real>,
|
||||
r2: Vector<Real>,
|
||||
rot2: Rotation<Real>,
|
||||
|
||||
vel: GenericConstraintPart,
|
||||
pos: GenericConstraintPart,
|
||||
}
|
||||
|
||||
impl GenericVelocityConstraint {
|
||||
#[inline(always)]
|
||||
pub fn compute_mass_matrix(
|
||||
joint: &GenericJoint,
|
||||
pub fn compute_delassus_matrix(
|
||||
im1: Real,
|
||||
im2: Real,
|
||||
ii1: AngularInertia<Real>,
|
||||
ii2: AngularInertia<Real>,
|
||||
r1: Vector<Real>,
|
||||
r2: Vector<Real>,
|
||||
velocity_solver: bool,
|
||||
rot2: Rotation<Real>,
|
||||
) -> Matrix6<Real> {
|
||||
let rmat1 = r1.gcross_matrix();
|
||||
let rmat2 = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D
|
||||
let mut lhs;
|
||||
let rotmat = rot2.to_rotation_matrix().into_inner();
|
||||
let rmat1 = r1.gcross_matrix() * rotmat;
|
||||
let rmat2 = r2.gcross_matrix() * rotmat;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 =
|
||||
let del00 =
|
||||
ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
let del10 =
|
||||
rotmat.transpose() * (ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2));
|
||||
let del11 = (ii1 + ii2).quadform(&rotmat).into_matrix();
|
||||
|
||||
// Note that Cholesky only reads the lower-triangular part of the matrix
|
||||
// so we don't need to fill lhs01.
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
|
||||
// Adjust the mass matrix to take force limits into account.
|
||||
// If a DoF has a force limit, then we need to make its
|
||||
// constraint independent from the others because otherwise
|
||||
// the force clamping will cause errors to propagate in the
|
||||
// other constraints.
|
||||
if velocity_solver {
|
||||
for i in 0..6 {
|
||||
if joint.max_negative_impulse[i] > -Real::MAX
|
||||
|| joint.max_positive_impulse[i] < Real::MAX
|
||||
{
|
||||
let diag = lhs[(i, i)];
|
||||
lhs.column_mut(i).fill(0.0);
|
||||
lhs.row_mut(i).fill(0.0);
|
||||
lhs[(i, i)] = diag;
|
||||
}
|
||||
}
|
||||
}
|
||||
// so we don't need to fill del01.
|
||||
let mut del = Matrix6::zeros();
|
||||
del.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&del00.into_matrix());
|
||||
del.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&del10);
|
||||
del.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&del11);
|
||||
del
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
panic!("Take the rotmat into account.");
|
||||
let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat1.x * ii1 + rmat2.x * ii2;
|
||||
let m23 = rmat1.y * ii1 + rmat2.y * ii2;
|
||||
let m33 = ii1 + ii2;
|
||||
lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
}
|
||||
}
|
||||
|
||||
pub fn compute_velocity_error(
|
||||
min_velocity: &SpatialVector<Real>,
|
||||
max_velocity: &SpatialVector<Real>,
|
||||
r1: &Vector<Real>,
|
||||
r2: &Vector<Real>,
|
||||
_anchor1: &Isometry<Real>,
|
||||
anchor2: &Isometry<Real>,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
) -> SpatialVector<Real> {
|
||||
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2);
|
||||
let ang_dvel = -rb1.angvel + rb2.angvel;
|
||||
|
||||
let lin_dvel2 = anchor2.inverse_transform_vector(&lin_dvel);
|
||||
let ang_dvel2 = anchor2.inverse_transform_vector(&ang_dvel);
|
||||
|
||||
dbg!(lin_dvel);
|
||||
dbg!(lin_dvel2);
|
||||
|
||||
let min_linvel = min_velocity.xyz();
|
||||
let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
||||
let max_linvel = max_velocity.xyz();
|
||||
let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
||||
let lin_rhs = lin_dvel2 - lin_dvel2.sup(&min_linvel).inf(&max_linvel);
|
||||
let ang_rhs = ang_dvel2 - ang_dvel2.sup(&min_angvel).inf(&max_angvel);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
return lhs.try_inverse().expect("Singular system.");
|
||||
return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
return lhs.cholesky().expect("Singular system.").inverse();
|
||||
return Vector6::new(
|
||||
lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z,
|
||||
);
|
||||
}
|
||||
|
||||
pub fn compute_position_error(
|
||||
joint: &GenericJoint,
|
||||
anchor1: &Isometry<Real>,
|
||||
anchor2: &Isometry<Real>,
|
||||
) -> SpatialVector<Real> {
|
||||
let delta_pos = Isometry::from_parts(
|
||||
anchor2.translation * anchor1.translation.inverse(),
|
||||
anchor2.rotation * anchor1.rotation.inverse(),
|
||||
);
|
||||
let lin_dpos = anchor2.inverse_transform_vector(&delta_pos.translation.vector);
|
||||
let ang_dpos = anchor2.inverse_transform_vector(&delta_pos.rotation.scaled_axis());
|
||||
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
|
||||
let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
|
||||
let error_code =
|
||||
(error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize;
|
||||
|
||||
match error_code {
|
||||
0 => error,
|
||||
1 => {
|
||||
let constrained_axis = error.rows(3, 3).iamin();
|
||||
let axis1 = anchor1
|
||||
.rotation
|
||||
.to_rotation_matrix()
|
||||
.into_inner()
|
||||
.column(constrained_axis)
|
||||
.into_owned();
|
||||
let axis2 = anchor2
|
||||
.rotation
|
||||
.to_rotation_matrix()
|
||||
.into_inner()
|
||||
.column(constrained_axis)
|
||||
.into_owned();
|
||||
let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
|
||||
.unwrap_or(UnitQuaternion::identity());
|
||||
let ang_dpos = anchor2.inverse_transform_vector(&rot_cross.scaled_axis());
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
|
||||
dpos - dpos.sup(&joint.min_position).inf(&joint.max_position)
|
||||
}
|
||||
2 => {
|
||||
// TODO
|
||||
error
|
||||
}
|
||||
3 => error,
|
||||
_ => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_params(
|
||||
@@ -136,48 +192,67 @@ impl GenericVelocityConstraint {
|
||||
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - rb1.world_com.coords;
|
||||
let r2 = anchor2.translation.vector - rb2.world_com.coords;
|
||||
let mut min_impulse = joint.min_impulse;
|
||||
let mut max_impulse = joint.max_impulse;
|
||||
let mut min_pos_impulse = joint.min_pos_impulse;
|
||||
let mut max_pos_impulse = joint.max_pos_impulse;
|
||||
let mut min_velocity = joint.min_velocity;
|
||||
let mut max_velocity = joint.max_velocity;
|
||||
|
||||
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
|
||||
let ang_dvel = -rb1.angvel + rb2.angvel;
|
||||
let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2)
|
||||
* params.inv_dt()
|
||||
* params.joint_erp;
|
||||
|
||||
let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true);
|
||||
for i in 0..6 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_impulse[i] = -Real::MAX;
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
min_velocity[i] = 0.0;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_impulse[i] = Real::MAX;
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
max_velocity[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
let rhs = Self::compute_velocity_error(
|
||||
&min_velocity,
|
||||
&max_velocity,
|
||||
&r1,
|
||||
&r2,
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
rb1,
|
||||
rb2,
|
||||
);
|
||||
|
||||
let mut delassus =
|
||||
Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, anchor2.rotation);
|
||||
|
||||
// Adjust the Delassus matrix to take force limits into account.
|
||||
// If a DoF has a force limit, then we need to make its
|
||||
// constraint independent from the others because otherwise
|
||||
// the force clamping will cause errors to propagate in the
|
||||
// other constraints.
|
||||
for i in 0..6 {
|
||||
if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX {
|
||||
let diag = delassus[(i, i)];
|
||||
delassus.column_mut(i).fill(0.0);
|
||||
delassus.row_mut(i).fill(0.0);
|
||||
delassus[(i, i)] = diag;
|
||||
}
|
||||
}
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix.
|
||||
#[cfg(feature = "dim2")]
|
||||
let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
|
||||
let inv_lhs = delassus.try_inverse().expect("Singular system.");
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
|
||||
let target_linvel = anchor2 * joint.target_velocity.xyz();
|
||||
let target_angvel = anchor2 * joint.target_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
||||
let target_vel = Vector6::new(
|
||||
target_linvel.x,
|
||||
target_linvel.y,
|
||||
target_linvel.z,
|
||||
target_angvel.x,
|
||||
target_angvel.y,
|
||||
target_angvel.z,
|
||||
);
|
||||
|
||||
let rhs = dvel - dvel.sup(&target_vel).inf(&target_vel);
|
||||
|
||||
let delta_pos = Isometry::from_parts(
|
||||
anchor2.translation * anchor1.translation.inverse(),
|
||||
anchor2.rotation * anchor1.rotation.inverse(),
|
||||
);
|
||||
let lin_dpos = delta_pos.translation.vector;
|
||||
let ang_dpos = delta_pos.rotation.scaled_axis();
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
|
||||
let pos_rhs = err * params.inv_dt() * params.joint_erp;
|
||||
let inv_lhs = delassus.cholesky().expect("Singular system.").inverse();
|
||||
|
||||
let impulse = (joint.impulse * params.warmstart_coeff)
|
||||
.inf(&joint.max_positive_impulse)
|
||||
.sup(&joint.max_negative_impulse);
|
||||
.inf(&max_impulse)
|
||||
.sup(&min_impulse);
|
||||
|
||||
GenericVelocityConstraint {
|
||||
joint_id,
|
||||
@@ -189,15 +264,22 @@ impl GenericVelocityConstraint {
|
||||
ii2,
|
||||
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
|
||||
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
|
||||
impulse,
|
||||
pos_impulse: na::zero(),
|
||||
max_positive_impulse: joint.max_positive_impulse,
|
||||
max_negative_impulse: joint.max_negative_impulse,
|
||||
inv_lhs,
|
||||
r1,
|
||||
r2,
|
||||
rhs,
|
||||
pos_rhs,
|
||||
rot2: anchor2.rotation,
|
||||
vel: GenericConstraintPart {
|
||||
impulse,
|
||||
min_impulse,
|
||||
max_impulse,
|
||||
rhs,
|
||||
},
|
||||
pos: GenericConstraintPart {
|
||||
impulse: na::zero(),
|
||||
min_impulse: min_pos_impulse,
|
||||
max_impulse: max_pos_impulse,
|
||||
rhs: pos_rhs,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
@@ -205,11 +287,11 @@ impl GenericVelocityConstraint {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
let ang_impulse = self.rot2 * self.vel.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
@@ -227,48 +309,6 @@ impl GenericVelocityConstraint {
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
return;
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
|
||||
+ mj_lambda2.linear
|
||||
+ ang_vel2.gcross(self.r2);
|
||||
let dangvel = -ang_vel1 + ang_vel2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let new_impulse = (self.impulse + self.inv_lhs * dvel)
|
||||
.sup(&self.max_negative_impulse)
|
||||
.inf(&self.max_positive_impulse);
|
||||
let effective_impulse = new_impulse - self.impulse;
|
||||
self.impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve2(
|
||||
@@ -281,82 +321,10 @@ impl GenericVelocityConstraint {
|
||||
let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize];
|
||||
|
||||
/*
|
||||
* Solve velocity.
|
||||
*/
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
|
||||
+ mj_lambda2.linear
|
||||
+ ang_vel2.gcross(self.r2);
|
||||
let dangvel = -ang_vel1 + ang_vel2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let new_impulse = (self.impulse + self.inv_lhs * dvel)
|
||||
.sup(&self.max_negative_impulse)
|
||||
.inf(&self.max_positive_impulse);
|
||||
let effective_impulse = new_impulse - self.impulse;
|
||||
self.impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Solve positions.
|
||||
*/
|
||||
|
||||
let ang_pos1 = self.ii1_sqrt.transform_vector(mj_lambda_pos1.angular);
|
||||
let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular);
|
||||
|
||||
let dlinpos = -mj_lambda_pos1.linear - ang_pos1.gcross(self.r1)
|
||||
+ mj_lambda_pos2.linear
|
||||
+ ang_pos2.gcross(self.r2);
|
||||
let dangpos = -ang_pos1 + ang_pos2;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let dpos = Vector6::new(
|
||||
dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z,
|
||||
) + self.pos_rhs;
|
||||
|
||||
let new_impulse = self.pos_impulse + self.inv_lhs * dpos;
|
||||
let effective_impulse = new_impulse - self.pos_impulse;
|
||||
self.pos_impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda_pos1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda_pos1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda_pos2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda_pos2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2);
|
||||
self.pos.impulse = self
|
||||
.pos
|
||||
.solve(self, &mut mj_lambda_pos1, &mut mj_lambda_pos2);
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
@@ -367,7 +335,7 @@ impl GenericVelocityConstraint {
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::GenericJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse;
|
||||
fixed.impulse = self.vel.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -378,94 +346,61 @@ pub(crate) struct GenericVelocityGroundConstraint {
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
impulse: SpacialVector<Real>,
|
||||
pos_impulse: SpacialVector<Real>,
|
||||
|
||||
max_positive_impulse: SpatialVector<Real>,
|
||||
max_negative_impulse: SpatialVector<Real>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<Real>,
|
||||
|
||||
inv_lhs: Matrix6<Real>, // TODO: replace by Cholesky?
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<Real>,
|
||||
|
||||
pos_rhs: Vector6<Real>,
|
||||
inv_lhs: Matrix3<Real>,
|
||||
|
||||
im2: Real,
|
||||
ii2: AngularInertia<Real>,
|
||||
ii2_sqrt: AngularInertia<Real>,
|
||||
r2: Vector<Real>,
|
||||
rot2: Rotation<Real>,
|
||||
|
||||
vel: GenericConstraintPart,
|
||||
pos: GenericConstraintPart,
|
||||
}
|
||||
|
||||
impl GenericVelocityGroundConstraint {
|
||||
#[inline(always)]
|
||||
pub fn compute_mass_matrix(
|
||||
joint: &GenericJoint,
|
||||
pub fn compute_delassus_matrix(
|
||||
im2: Real,
|
||||
ii2: AngularInertia<Real>,
|
||||
r2: Vector<Real>,
|
||||
velocity_solver: bool,
|
||||
rot2: Rotation<Real>,
|
||||
) -> Matrix6<Real> {
|
||||
let rmat2 = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
let rotmat2 = rot2.to_rotation_matrix().into_inner();
|
||||
let rmat2 = r2.gcross_matrix() * rotmat2;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = ii2.into_matrix();
|
||||
let del00 = ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let del10 = rotmat2.transpose() * ii2.transform_matrix(&rmat2);
|
||||
let del11 = ii2.quadform(&rotmat2).into_matrix();
|
||||
|
||||
// Note that Cholesky only reads the lower-triangular part of the matrix
|
||||
// so we don't need to fill lhs01.
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
|
||||
// Adjust the mass matrix to take force limits into account.
|
||||
// If a DoF has a force limit, then we need to make its
|
||||
// constraint independent from the others because otherwise
|
||||
// the force clamping will cause errors to propagate in the
|
||||
// other constraints.
|
||||
if velocity_solver {
|
||||
for i in 0..6 {
|
||||
if joint.max_negative_impulse[i] > -Real::MAX
|
||||
|| joint.max_positive_impulse[i] < Real::MAX
|
||||
{
|
||||
let diag = lhs[(i, i)];
|
||||
lhs.column_mut(i).fill(0.0);
|
||||
lhs.row_mut(i).fill(0.0);
|
||||
lhs[(i, i)] = diag;
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut del = Matrix6::zeros();
|
||||
del.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&del00.into_matrix());
|
||||
del.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&del10);
|
||||
del.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&del11);
|
||||
del
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
panic!("Properly handle the rotmat2");
|
||||
let m11 = im2 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im2 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat2.x * ii2;
|
||||
let m23 = rmat2.y * ii2;
|
||||
let m33 = ii2;
|
||||
lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
return lhs.try_inverse().expect("Singular system.");
|
||||
#[cfg(feature = "dim3")]
|
||||
return lhs.cholesky().expect("Singular system.").inverse();
|
||||
}
|
||||
|
||||
pub fn from_params(
|
||||
@@ -488,50 +423,70 @@ impl GenericVelocityGroundConstraint {
|
||||
)
|
||||
};
|
||||
|
||||
let r1 = anchor1.translation.vector - rb1.world_com.coords;
|
||||
let im2 = rb2.effective_inv_mass;
|
||||
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - rb1.world_com.coords;
|
||||
let r2 = anchor2.translation.vector - rb2.world_com.coords;
|
||||
let mut min_impulse = joint.min_impulse;
|
||||
let mut max_impulse = joint.max_impulse;
|
||||
let mut min_pos_impulse = joint.min_pos_impulse;
|
||||
let mut max_pos_impulse = joint.max_pos_impulse;
|
||||
let mut min_velocity = joint.min_velocity;
|
||||
let mut max_velocity = joint.max_velocity;
|
||||
|
||||
let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true);
|
||||
let pos_rhs = GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2)
|
||||
* params.inv_dt()
|
||||
* params.joint_erp;
|
||||
|
||||
let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
|
||||
let ang_dvel = rb2.angvel - rb1.angvel;
|
||||
for i in 0..6 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_impulse[i] = -Real::MAX;
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
min_velocity[i] = 0.0;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_impulse[i] = Real::MAX;
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
max_velocity[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
let rhs = GenericVelocityConstraint::compute_velocity_error(
|
||||
&min_velocity,
|
||||
&max_velocity,
|
||||
&r1,
|
||||
&r2,
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
rb1,
|
||||
rb2,
|
||||
);
|
||||
|
||||
let mut delassus = Self::compute_delassus_matrix(im2, ii2, r2, anchor2.rotation);
|
||||
|
||||
// Adjust the Delassus matrix to take force limits into account.
|
||||
// If a DoF has a force limit, then we need to make its
|
||||
// constraint independent from the others because otherwise
|
||||
// the force clamping will cause errors to propagate in the
|
||||
// other constraints.
|
||||
for i in 0..6 {
|
||||
if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX {
|
||||
let diag = delassus[(i, i)];
|
||||
delassus.column_mut(i).fill(0.0);
|
||||
delassus.row_mut(i).fill(0.0);
|
||||
delassus[(i, i)] = diag;
|
||||
}
|
||||
}
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix.
|
||||
#[cfg(feature = "dim2")]
|
||||
let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
let inv_lhs = delassus.try_inverse().expect("Singular system.");
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
let target_linvel = anchor2 * joint.target_velocity.xyz();
|
||||
let target_angvel = anchor2 * joint.target_velocity.fixed_rows::<AngDim>(DIM).into_owned();
|
||||
let target_vel = Vector6::new(
|
||||
target_linvel.x,
|
||||
target_linvel.y,
|
||||
target_linvel.z,
|
||||
target_angvel.x,
|
||||
target_angvel.y,
|
||||
target_angvel.z,
|
||||
);
|
||||
|
||||
let mut rhs = dvel - dvel.sup(&target_vel).inf(&target_vel);
|
||||
|
||||
let delta_pos = Isometry::from_parts(
|
||||
anchor2.translation * anchor1.translation.inverse(),
|
||||
anchor2.rotation * anchor1.rotation.inverse(),
|
||||
);
|
||||
let lin_dpos = delta_pos.translation.vector;
|
||||
let ang_dpos = delta_pos.rotation.scaled_axis();
|
||||
let dpos = Vector6::new(
|
||||
lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
|
||||
);
|
||||
let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
|
||||
let pos_rhs = err * params.inv_dt() * params.joint_erp;
|
||||
let inv_lhs = delassus.cholesky().expect("Singular system.").inverse();
|
||||
|
||||
let impulse = (joint.impulse * params.warmstart_coeff)
|
||||
.inf(&joint.max_positive_impulse)
|
||||
.sup(&joint.max_negative_impulse);
|
||||
.inf(&max_impulse)
|
||||
.sup(&min_impulse);
|
||||
|
||||
GenericVelocityGroundConstraint {
|
||||
joint_id,
|
||||
@@ -539,25 +494,32 @@ impl GenericVelocityGroundConstraint {
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
|
||||
impulse,
|
||||
pos_impulse: na::zero(),
|
||||
max_positive_impulse: joint.max_positive_impulse,
|
||||
max_negative_impulse: joint.max_negative_impulse,
|
||||
inv_lhs,
|
||||
r2,
|
||||
rhs,
|
||||
pos_rhs,
|
||||
rot2: anchor2.rotation,
|
||||
vel: GenericConstraintPart {
|
||||
impulse,
|
||||
min_impulse,
|
||||
max_impulse,
|
||||
rhs,
|
||||
},
|
||||
pos: GenericConstraintPart {
|
||||
impulse: na::zero(),
|
||||
min_impulse: min_pos_impulse,
|
||||
max_impulse: max_pos_impulse,
|
||||
rhs: pos_rhs,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
let ang_impulse = self.rot2 * self.vel.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
@@ -569,37 +531,6 @@ impl GenericVelocityGroundConstraint {
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
|
||||
return;
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let dangvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let new_impulse = (self.impulse + self.inv_lhs * dvel)
|
||||
.sup(&self.max_negative_impulse)
|
||||
.inf(&self.max_positive_impulse);
|
||||
let effective_impulse = new_impulse - self.impulse;
|
||||
self.impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve2(
|
||||
@@ -610,13 +541,47 @@ impl GenericVelocityGroundConstraint {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize];
|
||||
|
||||
/*
|
||||
* Solve velocities.
|
||||
*/
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
self.vel.impulse = self.vel.solve_ground(self, &mut mj_lambda2);
|
||||
self.pos.impulse = self.pos.solve_ground(self, &mut mj_lambda_pos2);
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2;
|
||||
}
|
||||
|
||||
// TODO: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::GenericJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.vel.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
struct GenericConstraintPart {
|
||||
impulse: SpacialVector<Real>,
|
||||
max_impulse: SpatialVector<Real>,
|
||||
min_impulse: SpatialVector<Real>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<Real>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<Real>,
|
||||
}
|
||||
|
||||
impl GenericConstraintPart {
|
||||
fn solve_ground(
|
||||
&self,
|
||||
parent: &GenericVelocityGroundConstraint,
|
||||
mj_lambda2: &mut DeltaVel<Real>,
|
||||
) -> SpatialVector<Real> {
|
||||
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = parent
|
||||
.rot2
|
||||
.inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
|
||||
let dangvel = parent.rot2.inverse_transform_vector(&ang_vel2);
|
||||
|
||||
let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let dangvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
@@ -624,61 +589,71 @@ impl GenericVelocityGroundConstraint {
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let new_impulse = (self.impulse + self.inv_lhs * dvel)
|
||||
.sup(&self.max_negative_impulse)
|
||||
.inf(&self.max_positive_impulse);
|
||||
let new_impulse = (self.impulse + parent.inv_lhs * dvel)
|
||||
.sup(&self.min_impulse)
|
||||
.inf(&self.max_impulse);
|
||||
let effective_impulse = new_impulse - self.impulse;
|
||||
self.impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
let ang_impulse = parent.rot2 * effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
mj_lambda2.linear -= parent.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= parent
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
.transform_vector(ang_impulse + parent.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Solve positions.
|
||||
*/
|
||||
let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular);
|
||||
|
||||
let dlinpos = mj_lambda_pos2.linear + ang_pos2.gcross(self.r2);
|
||||
let dangpos = ang_pos2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinpos.x, dlinpos.y, dangpos) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let dpos = Vector6::new(
|
||||
dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z,
|
||||
) + self.pos_rhs;
|
||||
|
||||
let new_impulse = self.pos_impulse + self.inv_lhs * dpos;
|
||||
let effective_impulse = new_impulse - self.pos_impulse;
|
||||
self.pos_impulse = new_impulse;
|
||||
|
||||
let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda_pos2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda_pos2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2;
|
||||
new_impulse
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::GenericJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse;
|
||||
}
|
||||
fn solve(
|
||||
&self,
|
||||
parent: &GenericVelocityConstraint,
|
||||
mj_lambda1: &mut DeltaVel<Real>,
|
||||
mj_lambda2: &mut DeltaVel<Real>,
|
||||
) -> SpatialVector<Real> {
|
||||
let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = parent.rot2.inverse_transform_vector(
|
||||
&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)
|
||||
+ mj_lambda2.linear
|
||||
+ ang_vel2.gcross(parent.r2)),
|
||||
);
|
||||
let dangvel = parent
|
||||
.rot2
|
||||
.inverse_transform_vector(&(-ang_vel1 + ang_vel2));
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let dvel = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let new_impulse = (self.impulse + parent.inv_lhs * dvel)
|
||||
.sup(&self.min_impulse)
|
||||
.inf(&self.max_impulse);
|
||||
let effective_impulse = new_impulse - self.impulse;
|
||||
|
||||
let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = parent.rot2 * effective_impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += parent.im1 * lin_impulse;
|
||||
mj_lambda1.angular += parent
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + parent.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= parent.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= parent
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + parent.r2.gcross(lin_impulse));
|
||||
|
||||
new_impulse
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user