Implement non-linear position stabilization for the generic constraint.
This commit is contained in:
Crozet Sébastien
@@ -5,7 +5,7 @@ use crate::math::{
|
||||
AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
|
||||
DIM,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::{Vector3, Vector6};
|
||||
|
||||
// FIXME: review this code for the case where the center of masses are not the origin.
|
||||
@@ -48,16 +48,136 @@ impl GenericPositionConstraint {
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
return;
|
||||
}
|
||||
let mut position1 = positions[self.position1];
|
||||
let mut position2 = positions[self.position2];
|
||||
let mut params = *params;
|
||||
params.joint_erp = 0.8;
|
||||
|
||||
pub fn solve2(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<Real>],
|
||||
dpos: &mut [DeltaVel<Real>],
|
||||
) {
|
||||
return;
|
||||
/*
|
||||
*
|
||||
* Translation part.
|
||||
*
|
||||
*/
|
||||
{
|
||||
let anchor1 = position1 * self.joint.local_anchor1;
|
||||
let anchor2 = position2 * self.joint.local_anchor2;
|
||||
let basis = anchor1.rotation;
|
||||
let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
|
||||
let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
|
||||
let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
|
||||
let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.xyz(),
|
||||
&self.joint.max_position.xyz(),
|
||||
) * params.joint_erp;
|
||||
|
||||
for i in 0..3 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
}
|
||||
}
|
||||
|
||||
let rotmat = basis.to_rotation_matrix().into_inner();
|
||||
let rmat1 = r1.gcross_matrix() * rotmat;
|
||||
let rmat2 = r2.gcross_matrix() * rotmat;
|
||||
|
||||
// Will be actually inverted right after.
|
||||
// TODO: we should keep the SdpMatrix3 type.
|
||||
let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1)
|
||||
+ self.ii2.quadform(&rmat2).add_diagonal(self.im2))
|
||||
.into_matrix();
|
||||
|
||||
let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
||||
&min_pos_impulse,
|
||||
&max_pos_impulse,
|
||||
&mut Vector3::zeros(),
|
||||
delassus,
|
||||
);
|
||||
|
||||
let local_impulse = (inv_delassus * pos_rhs)
|
||||
.inf(&max_pos_impulse)
|
||||
.sup(&min_pos_impulse);
|
||||
let impulse = basis * local_impulse;
|
||||
|
||||
let rot1 = self.ii1.transform_vector(r1.gcross(impulse));
|
||||
let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
|
||||
|
||||
position1.translation.vector += self.im1 * impulse;
|
||||
position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Rotation part
|
||||
*
|
||||
*/
|
||||
{
|
||||
let anchor1 = position1 * self.joint.local_anchor1;
|
||||
let anchor2 = position2 * self.joint.local_anchor2;
|
||||
let basis = anchor1.rotation;
|
||||
let mut min_pos_impulse = self
|
||||
.joint
|
||||
.min_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
let mut max_pos_impulse = self
|
||||
.joint
|
||||
.max_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
&self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
) * params.joint_erp;
|
||||
|
||||
for i in 0..3 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
}
|
||||
}
|
||||
|
||||
// Will be actually inverted right after.
|
||||
// TODO: we should keep the SdpMatrix3 type.
|
||||
let rotmat = basis.to_rotation_matrix().into_inner();
|
||||
let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix();
|
||||
|
||||
let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
||||
&min_pos_impulse,
|
||||
&max_pos_impulse,
|
||||
&mut Vector3::zeros(),
|
||||
delassus,
|
||||
);
|
||||
|
||||
let local_impulse = (inv_delassus * pos_rhs)
|
||||
.inf(&max_pos_impulse)
|
||||
.sup(&min_pos_impulse);
|
||||
let impulse = basis * local_impulse;
|
||||
|
||||
let rot1 = self.ii1.transform_vector(impulse);
|
||||
let rot2 = self.ii2.transform_vector(impulse);
|
||||
|
||||
position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
|
||||
position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
|
||||
}
|
||||
|
||||
positions[self.position1] = position1;
|
||||
positions[self.position2] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -102,15 +222,127 @@ impl GenericPositionGroundConstraint {
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
|
||||
return;
|
||||
}
|
||||
let mut position2 = positions[self.position2];
|
||||
let mut params = *params;
|
||||
params.joint_erp = 0.8;
|
||||
|
||||
pub fn solve2(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<Real>],
|
||||
dpos: &mut [DeltaVel<Real>],
|
||||
) {
|
||||
return;
|
||||
/*
|
||||
*
|
||||
* Translation part.
|
||||
*
|
||||
*/
|
||||
{
|
||||
let anchor1 = self.anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let basis = anchor1.rotation;
|
||||
let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
|
||||
let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
|
||||
let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.xyz(),
|
||||
&self.joint.max_position.xyz(),
|
||||
) * params.joint_erp;
|
||||
|
||||
for i in 0..3 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
}
|
||||
}
|
||||
|
||||
let rotmat = basis.to_rotation_matrix().into_inner();
|
||||
let rmat2 = r2.gcross_matrix() * rotmat;
|
||||
|
||||
// Will be actually inverted right after.
|
||||
// TODO: we should keep the SdpMatrix3 type.
|
||||
let delassus = self
|
||||
.ii2
|
||||
.quadform(&rmat2)
|
||||
.add_diagonal(self.im2)
|
||||
.into_matrix();
|
||||
|
||||
let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
||||
&min_pos_impulse,
|
||||
&max_pos_impulse,
|
||||
&mut Vector3::zeros(),
|
||||
delassus,
|
||||
);
|
||||
|
||||
let local_impulse = (inv_delassus * pos_rhs)
|
||||
.inf(&max_pos_impulse)
|
||||
.sup(&min_pos_impulse);
|
||||
let impulse = basis * local_impulse;
|
||||
|
||||
let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
|
||||
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Rotation part
|
||||
*
|
||||
*/
|
||||
{
|
||||
let anchor1 = self.anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let basis = anchor1.rotation;
|
||||
let mut min_pos_impulse = self
|
||||
.joint
|
||||
.min_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
let mut max_pos_impulse = self
|
||||
.joint
|
||||
.max_pos_impulse
|
||||
.fixed_rows::<Dim>(DIM)
|
||||
.into_owned();
|
||||
|
||||
let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
|
||||
&anchor1,
|
||||
&anchor2,
|
||||
&basis,
|
||||
&self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
&self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
|
||||
) * params.joint_erp;
|
||||
|
||||
for i in 0..3 {
|
||||
if pos_rhs[i] < 0.0 {
|
||||
min_pos_impulse[i] = -Real::MAX;
|
||||
}
|
||||
if pos_rhs[i] > 0.0 {
|
||||
max_pos_impulse[i] = Real::MAX;
|
||||
}
|
||||
}
|
||||
|
||||
// Will be actually inverted right after.
|
||||
// TODO: we should keep the SdpMatrix3 type.
|
||||
let rotmat = basis.to_rotation_matrix().into_inner();
|
||||
let delassus = self.ii2.quadform(&rotmat).into_matrix();
|
||||
|
||||
let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
|
||||
&min_pos_impulse,
|
||||
&max_pos_impulse,
|
||||
&mut Vector3::zeros(),
|
||||
delassus,
|
||||
);
|
||||
|
||||
let local_impulse = (inv_delassus * pos_rhs)
|
||||
.inf(&max_pos_impulse)
|
||||
.sup(&min_pos_impulse);
|
||||
let impulse = basis * local_impulse;
|
||||
let rot2 = self.ii2.transform_vector(impulse);
|
||||
|
||||
position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
|
||||
}
|
||||
|
||||
positions[self.position2] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user