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

Complete the implementation of non-simd joint motor for the revolute joint.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-19 15:21:25 +01:00
parent a1ddda5077
commit e9f17f32e8
14 changed files with 483 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/dynamics/joint/generic_joint.rs
View File

@@ -54,6 +54,13 @@ impl GenericJoint {
}
}
pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) {
self.min_velocity[dof as usize] = target_vel;
self.max_velocity[dof as usize] = target_vel;
self.min_impulse[dof as usize] = -max_force;
self.max_impulse[dof as usize] = max_force;
}
pub fn free_dof(&mut self, dof: u8) {
self.min_position[dof as usize] = -Real::MAX;
self.max_position[dof as usize] = Real::MAX;
@@ -82,6 +89,18 @@ impl From<RevoluteJoint> for GenericJoint {
let mut result = Self::new(local_anchor1, local_anchor2);
result.free_dof(3);
if joint.motor_damping != 0.0 {
result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse);
}
result.impulse[0] = joint.impulse[0];
result.impulse[1] = joint.impulse[1];
result.impulse[2] = joint.impulse[2];
result.impulse[3] = joint.motor_impulse;
result.impulse[4] = joint.impulse[3];
result.impulse[5] = joint.impulse[4];
result
}
}
@@ -92,6 +111,9 @@ impl From<BallJoint> for GenericJoint {
let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
let mut result = Self::new(local_anchor1, local_anchor2);
result.impulse[0] = joint.impulse[0];
result.impulse[1] = joint.impulse[1];
result.impulse[2] = joint.impulse[2];
result.free_dof(3);
result.free_dof(4);
result.free_dof(5);

34
src/dynamics/joint/joint.rs
View File

@@ -1,8 +1,6 @@
#[cfg(feature = "dim3")]
use crate::dynamics::RevoluteJoint;
use crate::dynamics::{
BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle,
};
use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
#[derive(Copy, Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -19,7 +17,7 @@ pub enum JointParams {
/// A revolute joint that removes all degrees of degrees of freedom between the affected
/// bodies except for the translation along one axis.
RevoluteJoint(RevoluteJoint),
GenericJoint(GenericJoint),
// GenericJoint(GenericJoint),
}
impl JointParams {
@@ -29,7 +27,7 @@ impl JointParams {
JointParams::BallJoint(_) => 0,
JointParams::FixedJoint(_) => 1,
JointParams::PrismaticJoint(_) => 2,
JointParams::GenericJoint(_) => 3,
// JointParams::GenericJoint(_) => 3,
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => 4,
}
@@ -53,14 +51,14 @@ impl JointParams {
}
}
/// Gets a reference to the underlying generic joint, if `self` is one.
pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
if let JointParams::GenericJoint(j) = self {
Some(j)
} else {
None
}
}
// /// Gets a reference to the underlying generic joint, if `self` is one.
// pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
// if let JointParams::GenericJoint(j) = self {
// Some(j)
// } else {
// None
// }
// }
/// Gets a reference to the underlying prismatic joint, if `self` is one.
pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
@@ -94,11 +92,11 @@ impl From<FixedJoint> for JointParams {
}
}
impl From<GenericJoint> for JointParams {
fn from(j: GenericJoint) -> Self {
JointParams::GenericJoint(j)
}
}
// impl From<GenericJoint> for JointParams {
// fn from(j: GenericJoint) -> Self {
// JointParams::GenericJoint(j)
// }
// }
#[cfg(feature = "dim3")]
impl From<RevoluteJoint> for JointParams {

6
src/dynamics/joint/mod.rs
View File

@@ -1,18 +1,20 @@
pub use self::ball_joint::BallJoint;
pub use self::fixed_joint::FixedJoint;
pub use self::generic_joint::GenericJoint;
// pub use self::generic_joint::GenericJoint;
pub use self::joint::{Joint, JointParams};
pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
pub use self::joint_set::{JointHandle, JointSet};
pub use self::prismatic_joint::PrismaticJoint;
#[cfg(feature = "dim3")]
pub use self::revolute_joint::RevoluteJoint;
pub use self::spring_model::SpringModel;
mod ball_joint;
mod fixed_joint;
mod generic_joint;
// mod generic_joint;
mod joint;
mod joint_set;
mod prismatic_joint;
#[cfg(feature = "dim3")]
mod revolute_joint;
mod spring_model;

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

@@ -1,6 +1,7 @@
use crate::math::{Point, Real, Vector};
use crate::dynamics::SpringModel;
use crate::math::{Isometry, Point, Real, Vector};
use crate::utils::WBasis;
use na::{Unit, Vector5};
use na::{RealField, Unit, Vector5};
#[derive(Copy, Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -22,6 +23,28 @@ pub struct RevoluteJoint {
///
/// The impulse applied to the second body is given by `-impulse`.
pub impulse: Vector5<Real>,
/// The target relative angular velocity the motor will attempt to reach.
pub motor_target_vel: Real,
/// The target relative angle along the joint axis the motor will attempt to reach.
pub motor_target_pos: Real,
/// The motor's stiffness.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_stiffness: Real,
/// The motor's damping.
/// See the documentation of `SpringModel` for more information on this parameter.
pub motor_damping: Real,
/// The maximal impulse the motor is able to deliver.
pub motor_max_impulse: Real,
/// The angular impulse applied by the motor.
pub motor_impulse: Real,
/// The spring-like model used by the motor to reach the target velocity and .
pub motor_model: SpringModel,
// Used to handle cases where the position target ends up being more than pi radians away.
pub(crate) motor_last_angle: Real,
// The angular impulse expressed in world-space.
pub(crate) world_ang_impulse: Vector<Real>,
// The world-space orientation of the free axis of the first attached body.
pub(crate) prev_axis1: Vector<Real>,
}
impl RevoluteJoint {
@@ -41,6 +64,74 @@ impl RevoluteJoint {
basis1: local_axis1.orthonormal_basis(),
basis2: local_axis2.orthonormal_basis(),
impulse: na::zero(),
world_ang_impulse: na::zero(),
motor_target_vel: 0.0,
motor_target_pos: 0.0,
motor_stiffness: 0.0,
motor_damping: 0.0,
motor_max_impulse: Real::MAX,
motor_impulse: 0.0,
prev_axis1: *local_axis1,
motor_model: SpringModel::VelocityBased,
motor_last_angle: 0.0,
}
}
pub fn configure_motor_model(&mut self, model: SpringModel) {
self.motor_model = model;
}
pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
}
pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
self.configure_motor(target_pos, 0.0, stiffness, damping)
}
pub fn configure_motor(
&mut self,
target_pos: Real,
target_vel: Real,
stiffness: Real,
damping: Real,
) {
self.motor_target_vel = target_vel;
self.motor_target_pos = target_pos;
self.motor_stiffness = stiffness;
self.motor_damping = damping;
}
/// Estimates the current position of the motor angle.
pub fn estimate_motor_angle(
&self,
body_pos1: &Isometry<Real>,
body_pos2: &Isometry<Real>,
) -> Real {
let motor_axis1 = body_pos1 * self.local_axis1;
let ref1 = body_pos1 * self.basis1[0];
let ref2 = body_pos2 * self.basis2[0];
let last_angle_cycles = (self.motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
// Measure the position between 0 and 2-pi
let new_angle = if ref1.cross(&ref2).dot(&motor_axis1) < 0.0 {
Real::two_pi() - ref1.angle(&ref2)
} else {
ref1.angle(&ref2)
};
// The last angle between 0 and 2-pi
let last_angle_zero_two_pi = self.motor_last_angle - last_angle_cycles;
// Figure out the smallest angle differance.
let mut angle_diff = new_angle - last_angle_zero_two_pi;
if angle_diff > Real::pi() {
angle_diff -= Real::two_pi()
} else if angle_diff < -Real::pi() {
angle_diff += Real::two_pi()
}
self.motor_last_angle + angle_diff
}
}

59
src/dynamics/joint/spring_model.rs Normal file
View File

@@ -0,0 +1,59 @@
use crate::math::Real;
/// The spring-like model used for constraints resolution.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum SpringModel {
/// No equation is solved.
Disabled,
/// The solved spring-like equation is:
/// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
///
/// Here the `stiffness` is the ratio of position error to be solved at each timestep (like
/// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at
/// each timestep.
VelocityBased,
/// The solved spring-like equation is:
/// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
AccelerationBased,
/// The solved spring-like equation is:
/// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
ForceBased,
}
impl SpringModel {
/// Combines the coefficients used for solving the spring equation.
///
/// Returns the new coefficients (stiffness, damping, inv_lhs_scale, keep_inv_lhs)
/// coefficients for the equivalent impulse-based equation. These new
/// coefficients must be used in the following way:
/// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
/// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`.
/// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero.
pub fn combine_coefficients(
self,
dt: Real,
stiffness: Real,
damping: Real,
) -> (Real, Real, Real, bool) {
match self {
SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
SpringModel::AccelerationBased => {
let effective_stiffness = stiffness * dt;
let effective_damping = damping * dt;
// TODO: Using gamma behaves very badly for some reasons.
// Maybe I got the formulation wrong, so let's keep it to 1.0 for now,
// and get back to this later.
// let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, 1.0, true)
}
SpringModel::ForceBased => {
let effective_stiffness = stiffness * dt;
let effective_damping = damping * dt;
let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, gamma, false)
}
SpringModel::Disabled => return (0.0, 0.0, 0.0, false),
}
}
}

9
src/dynamics/mod.rs
View File

@@ -5,7 +5,14 @@ pub(crate) use self::joint::JointIndex;
#[cfg(feature = "dim3")]
pub use self::joint::RevoluteJoint;
pub use self::joint::{
BallJoint, FixedJoint, GenericJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
BallJoint,
FixedJoint,
Joint,
JointHandle,
JointParams,
JointSet,
PrismaticJoint,
SpringModel, // GenericJoint
};
pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};

152
src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
View File

@@ -32,7 +32,8 @@ pub(crate) struct GenericVelocityConstraint {
r1: Vector<Real>,
r2: Vector<Real>,
basis: Rotation<Real>,
basis1: Rotation<Real>,
basis2: Rotation<Real>,
dependant_set_mask: SpacialVector<Real>,
vel: GenericConstraintPart,
@@ -44,22 +45,22 @@ impl GenericVelocityConstraint {
max_velocity: &SpatialVector<Real>,
r1: &Vector<Real>,
r2: &Vector<Real>,
basis: &Rotation<Real>,
basis1: &Rotation<Real>,
basis2: &Rotation<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 = basis.inverse_transform_vector(&lin_dvel);
let ang_dvel2 = basis.inverse_transform_vector(&ang_dvel);
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+ basis2.inverse_transform_vector(&rb2.angvel);
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);
let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
#[cfg(feature = "dim2")]
return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
@@ -120,6 +121,32 @@ impl GenericVelocityConstraint {
}
}
pub fn invert_partial_delassus_matrix(
min_impulse: &Vector<Real>,
max_impulse: &Vector<Real>,
dependant_set_mask: &mut Vector<Real>,
mut delassus: na::Matrix3<Real>,
) -> na::Matrix3<Real> {
// 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..3 {
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;
dependant_set_mask[i] = 0.0;
} else {
dependant_set_mask[i] = 1.0;
}
}
delassus.try_inverse().unwrap()
}
pub fn compute_position_error(
joint: &GenericJoint,
anchor1: &Isometry<Real>,
@@ -169,32 +196,6 @@ impl GenericVelocityConstraint {
}
}
pub fn invert_partial_delassus_matrix(
min_impulse: &Vector<Real>,
max_impulse: &Vector<Real>,
dependant_set_mask: &mut Vector<Real>,
mut delassus: na::Matrix3<Real>,
) -> na::Matrix3<Real> {
// 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..3 {
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;
dependant_set_mask[i] = 0.0;
} else {
dependant_set_mask[i] = 1.0;
}
}
delassus.try_inverse().unwrap()
}
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
@@ -204,7 +205,8 @@ impl GenericVelocityConstraint {
) -> Self {
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let basis = anchor1.rotation;
let basis1 = anchor1.rotation;
let basis2 = anchor2.rotation;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
@@ -219,7 +221,7 @@ impl GenericVelocityConstraint {
let mut max_velocity = joint.max_velocity;
let mut dependant_set_mask = SpacialVector::repeat(1.0);
let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis)
let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
* params.inv_dt()
* params.joint_erp;
@@ -236,19 +238,28 @@ impl GenericVelocityConstraint {
}
}
let rhs =
Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2);
let rhs = Self::compute_velocity_error(
&min_velocity,
&max_velocity,
&r1,
&r2,
&basis1,
&basis2,
rb1,
rb2,
);
let rhs_lin = rhs.xyz();
let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
// TODO: we should keep the SdpMatrix3 type.
let rotmat = basis.to_rotation_matrix().into_inner();
let rmat1 = r1.gcross_matrix() * rotmat;
let rmat2 = r2.gcross_matrix() * rotmat;
let rotmat1 = basis1.to_rotation_matrix().into_inner();
let rotmat2 = basis2.to_rotation_matrix().into_inner();
let rmat1 = r1.gcross_matrix() * rotmat1;
let rmat2 = r2.gcross_matrix() * rotmat2;
let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
+ ii2.quadform(&rmat2).add_diagonal(im2))
.into_matrix();
let delassus11 = (ii1.quadform(&rotmat) + ii2.quadform(&rotmat)).into_matrix();
let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
&min_pos_impulse.xyz(),
@@ -288,7 +299,8 @@ impl GenericVelocityConstraint {
inv_lhs_ang,
r1,
r2,
basis,
basis1,
basis2,
dependant_set_mask,
vel: GenericConstraintPart {
lin_impulse,
@@ -307,21 +319,20 @@ 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.basis * self.vel.lin_impulse;
#[cfg(feature = "dim2")]
let ang_impulse = self.basis * self.vel.impulse[2];
#[cfg(feature = "dim3")]
let ang_impulse = self.basis * self.vel.ang_impulse;
let lin_impulse1 = self.basis1 * self.vel.lin_impulse;
let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
mj_lambda1.linear += self.im1 * lin_impulse;
mj_lambda1.linear += self.im1 * lin_impulse1;
mj_lambda1.angular += self
.ii1_sqrt
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
.transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
mj_lambda2.linear -= self.im2 * lin_impulse;
mj_lambda2.linear -= self.im2 * lin_impulse2;
mj_lambda2.angular -= self
.ii2_sqrt
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
.transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -441,6 +452,7 @@ impl GenericVelocityGroundConstraint {
&r1,
&r2,
&basis,
&basis,
rb1,
rb2,
);
@@ -585,28 +597,30 @@ impl GenericConstraintPart {
let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
let dvel = parent.basis.inverse_transform_vector(
&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)
+ mj_lambda2.linear
+ ang_vel2.gcross(parent.r2)),
);
let dvel = parent
.basis1
.inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
+ parent
.basis2
.inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
let err = dvel + self.rhs_lin;
new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
.sup(&self.min_lin_impulse)
.inf(&self.max_lin_impulse);
let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
mj_lambda1.linear += parent.im1 * effective_impulse;
mj_lambda1.linear += parent.im1 * effective_impulse1;
mj_lambda1.angular += parent
.ii1_sqrt
.transform_vector(parent.r1.gcross(effective_impulse));
.transform_vector(parent.r1.gcross(effective_impulse1));
mj_lambda2.linear -= parent.im2 * effective_impulse;
mj_lambda2.linear -= parent.im2 * effective_impulse2;
mj_lambda2.angular -= parent
.ii2_sqrt
.transform_vector(parent.r2.gcross(effective_impulse));
.transform_vector(parent.r2.gcross(effective_impulse2));
}
/*
@@ -618,18 +632,18 @@ impl GenericConstraintPart {
let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
let dvel = parent
.basis
.inverse_transform_vector(&(ang_vel2 - ang_vel1));
let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
- parent.basis1.inverse_transform_vector(&ang_vel1);
let err = dvel + self.rhs_ang;
new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
.sup(&self.min_ang_impulse)
.inf(&self.max_ang_impulse);
let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse);
mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
}
(new_lin_impulse, new_ang_impulse)

84
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -14,9 +14,9 @@ use super::{
#[cfg(feature = "dim3")]
#[cfg(feature = "simd-is-enabled")]
use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
GenericVelocityConstraint, GenericVelocityGroundConstraint,
};
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -38,12 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint {
WFixedConstraint(WFixedVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedVelocityGroundConstraint),
GenericConstraint(GenericVelocityConstraint),
GenericGroundConstraint(GenericVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericConstraint(WGenericVelocityConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericGroundConstraint(WGenericVelocityGroundConstraint),
// GenericConstraint(GenericVelocityConstraint),
// GenericGroundConstraint(GenericVelocityGroundConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericConstraint(WGenericVelocityConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericGroundConstraint(WGenericVelocityGroundConstraint),
PrismaticConstraint(PrismaticVelocityConstraint),
PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
@@ -89,9 +89,9 @@ impl AnyJointVelocityConstraint {
JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
),
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
// GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
// ),
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
@@ -167,11 +167,11 @@ impl AnyJointVelocityConstraint {
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
),
JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
GenericVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
),
// JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
// GenericVelocityGroundConstraint::from_params(
// params, joint_id, rb1, rb2, p, flipped,
// ),
// ),
JointParams::PrismaticJoint(p) => {
AnyJointVelocityConstraint::PrismaticGroundConstraint(
PrismaticVelocityGroundConstraint::from_params(
@@ -180,10 +180,8 @@ impl AnyJointVelocityConstraint {
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
RevoluteVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
params, joint_id, rb1, rb2, p, flipped,
),
}
}
@@ -267,12 +265,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
// AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -307,12 +305,12 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
// AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
#[cfg(feature = "simd-is-enabled")]
@@ -355,16 +353,16 @@ impl AnyJointVelocityConstraint {
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
c.writeback_impulses(joints_all)
}
// AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
// AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
// c.writeback_impulses(joints_all)
// }
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
// c.writeback_impulses(joints_all)
// }
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
c.writeback_impulses(joints_all)

39
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -1,7 +1,6 @@
use super::{
BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint,
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
};
#[cfg(feature = "dim3")]
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
@@ -33,12 +32,12 @@ pub(crate) enum AnyJointPositionConstraint {
WFixedJoint(WFixedPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WFixedGroundConstraint(WFixedPositionGroundConstraint),
GenericJoint(GenericPositionConstraint),
GenericGroundConstraint(GenericPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericJoint(WGenericPositionConstraint),
#[cfg(feature = "simd-is-enabled")]
WGenericGroundConstraint(WGenericPositionGroundConstraint),
// GenericJoint(GenericPositionConstraint),
// GenericGroundConstraint(GenericPositionGroundConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericJoint(WGenericPositionConstraint),
// #[cfg(feature = "simd-is-enabled")]
// WGenericGroundConstraint(WGenericPositionGroundConstraint),
PrismaticJoint(PrismaticPositionConstraint),
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
#[cfg(feature = "simd-is-enabled")]
@@ -69,9 +68,9 @@ impl AnyJointPositionConstraint {
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
FixedPositionConstraint::from_params(rb1, rb2, p),
),
JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
GenericPositionConstraint::from_params(rb1, rb2, p),
),
// JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
// GenericPositionConstraint::from_params(rb1, rb2, p),
// ),
JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
PrismaticPositionConstraint::from_params(rb1, rb2, p),
),
@@ -140,9 +139,9 @@ impl AnyJointPositionConstraint {
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
),
JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
),
// JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
// GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
// ),
JointParams::PrismaticJoint(p) => {
AnyJointPositionConstraint::PrismaticGroundConstraint(
PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -219,12 +218,12 @@ impl AnyJointPositionConstraint {
AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]
AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
// AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
// AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
// #[cfg(feature = "simd-is-enabled")]
// AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
#[cfg(feature = "simd-is-enabled")]

40
src/dynamics/solver/joint_constraint/mod.rs
View File

@@ -18,20 +18,20 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity
pub(self) use fixed_velocity_constraint_wide::{
WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
};
pub(self) use generic_position_constraint::{
GenericPositionConstraint, GenericPositionGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
pub(self) use generic_position_constraint_wide::{
WGenericPositionConstraint, WGenericPositionGroundConstraint,
};
pub(self) use generic_velocity_constraint::{
GenericVelocityConstraint, GenericVelocityGroundConstraint,
};
#[cfg(feature = "simd-is-enabled")]
pub(self) use generic_velocity_constraint_wide::{
WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
};
// pub(self) use generic_position_constraint::{
// GenericPositionConstraint, GenericPositionGroundConstraint,
// };
// #[cfg(feature = "simd-is-enabled")]
// pub(self) use generic_position_constraint_wide::{
// WGenericPositionConstraint, WGenericPositionGroundConstraint,
// };
// pub(self) use generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
// #[cfg(feature = "simd-is-enabled")]
// pub(self) use generic_velocity_constraint_wide::{
// WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
// };
pub(crate) use joint_constraint::AnyJointVelocityConstraint;
pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
@@ -78,12 +78,12 @@ mod fixed_position_constraint_wide;
mod fixed_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod fixed_velocity_constraint_wide;
mod generic_position_constraint;
#[cfg(feature = "simd-is-enabled")]
mod generic_position_constraint_wide;
mod generic_velocity_constraint;
#[cfg(feature = "simd-is-enabled")]
mod generic_velocity_constraint_wide;
// mod generic_position_constraint;
// #[cfg(feature = "simd-is-enabled")]
// mod generic_position_constraint_wide;
// mod generic_velocity_constraint;
// #[cfg(feature = "simd-is-enabled")]
// mod generic_velocity_constraint_wide;
mod joint_constraint;
mod joint_position_constraint;
mod prismatic_position_constraint;

126
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -59,72 +59,6 @@ impl RevolutePositionConstraint {
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 axis1 = position1 * self.local_axis1;
let axis2 = position2 * self.local_axis2;
let basis1 = Matrix3x2::from_columns(&[
position1 * self.local_basis1[0],
position1 * self.local_basis1[1],
]);
let basis2 = Matrix3x2::from_columns(&[
position2 * self.local_basis2[0],
position2 * self.local_basis2[1],
]);
let basis_filter1 = basis1 * basis1.transpose();
let basis_filter2 = basis2 * basis2.transpose();
let basis2 = basis_filter2 * basis1;
let r1 = anchor1 - position1 * self.local_com1;
let r2 = anchor2 - position2 * self.local_com2;
let r1_mat = basis_filter1 * r1.gcross_matrix();
let r2_mat = basis_filter2 * r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2)
+ self.ii1.quadform(&r1_mat).add_diagonal(self.im1);
let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)) + basis1.tr_mul(&(self.ii1 * r1_mat));
let lhs11 = (self.ii1.quadform3x2(&basis1) + self.ii2.quadform3x2(&basis2)).into_matrix();
// Note that cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<na::U3, na::U3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0)
.copy_from(&lhs10);
lhs.fixed_slice_mut::<na::U2, na::U2>(3, 3)
.copy_from(&lhs11);
let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse();
let delta_tra = anchor2 - anchor1;
let lin_error = delta_tra * params.joint_erp;
let delta_rot =
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp;
let error = na::Vector5::new(
lin_error.x,
lin_error.y,
lin_error.z,
ang_error.x,
ang_error.y,
);
let impulse = inv_lhs * error;
let lin_impulse = impulse.fixed_rows::<na::U3>(0).into_owned();
let ang_impulse1 = basis1 * impulse.fixed_rows::<na::U2>(3).into_owned();
let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(3).into_owned();
let rot1 = self.ii1 * (r1_mat * lin_impulse + ang_impulse1);
let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2);
position1.rotation = Rotation::new(rot1) * position1.rotation;
position2.rotation = Rotation::new(-rot2) * position2.rotation;
position1.translation.vector += self.im1 * lin_impulse;
position2.translation.vector -= self.im2 * lin_impulse;
/*
/*
* Linear part.
*/
@@ -134,7 +68,8 @@ impl RevolutePositionConstraint {
let r1 = anchor1 - position1 * self.local_com1;
let r2 = anchor2 - position2 * self.local_com2;
// TODO: don't the the "to_matrix".
// TODO: don't do the "to_matrix".
let lhs = (self
.ii2
.quadform(&r2.gcross_matrix())
@@ -174,7 +109,6 @@ impl RevolutePositionConstraint {
position2.rotation =
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
}
*/
positions[self.position1 as usize] = position1;
positions[self.position2 as usize] = position2;
@@ -249,61 +183,6 @@ impl RevolutePositionGroundConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = positions[self.position2 as usize];
let anchor1 = self.anchor1;
let anchor2 = position2 * self.local_anchor2;
let axis1 = self.axis1;
let axis2 = position2 * self.local_axis2;
let basis1 = Matrix3x2::from_columns(&self.basis1[..]);
let basis2 = Matrix3x2::from_columns(&[
position2 * self.local_basis2[0],
position2 * self.local_basis2[1],
]);
let basis_filter2 = basis2 * basis2.transpose();
let basis2 = basis_filter2 * basis1;
let r2 = anchor2 - position2 * self.local_com2;
let r2_mat = basis_filter2 * r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2);
let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat));
let lhs11 = self.ii2.quadform3x2(&basis2).into_matrix();
// Note that cholesky won't read the upper-right part
// of lhs so we don't have to fill it.
lhs.fixed_slice_mut::<na::U3, na::U3>(0, 0)
.copy_from(&lhs00.into_matrix());
lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0)
.copy_from(&lhs10);
lhs.fixed_slice_mut::<na::U2, na::U2>(3, 3)
.copy_from(&lhs11);
let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse();
let delta_tra = anchor2 - anchor1;
let lin_error = delta_tra * params.joint_erp;
let delta_rot =
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp;
let error = na::Vector5::new(
lin_error.x,
lin_error.y,
lin_error.z,
ang_error.x,
ang_error.y,
);
let impulse = inv_lhs * error;
let lin_impulse = impulse.fixed_rows::<na::U3>(0).into_owned();
let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(3).into_owned();
let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2);
position2.rotation = Rotation::new(-rot2) * position2.rotation;
position2.translation.vector -= self.im2 * lin_impulse;
/*
/*
* Linear part.
*/
@@ -338,7 +217,6 @@ impl RevolutePositionGroundConstraint {
let ang_error = delta_rot.scaled_axis() * params.joint_erp;
position2.rotation = Rotation::new(-ang_error) * position2.rotation;
}
*/
positions[self.position2 as usize] = position2;
}

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

@@ -1,11 +1,11 @@
use crate::dynamics::solver::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel};
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint,
RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::na::UnitQuaternion;
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use downcast_rs::Downcast;
use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, RealField, Vector5, U2, U3};
#[derive(Debug)]
@@ -26,6 +26,7 @@ pub(crate) struct RevoluteVelocityConstraint {
motor_rhs: Real,
motor_impulse: Real,
motor_max_impulse: Real,
motor_angle: Real, // Exists only to write it back into the joint.
motor_axis1: Vector<Real>,
motor_axis2: Vector<Real>,
@@ -47,7 +48,7 @@ impl RevoluteVelocityConstraint {
rb1: &RigidBody,
rb2: &RigidBody,
joint: &RevoluteJoint,
) -> AnyJointVelocityConstraint {
) -> Self {
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
@@ -100,16 +101,35 @@ impl RevoluteVelocityConstraint {
let motor_axis2 = rb2.position * *joint.local_axis2;
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let mut motor_max_impulse = 0.0;
let mut motor_max_impulse = joint.motor_max_impulse;
let mut motor_angle = 0.0;
if let Some(motor_target_vel) = joint.motor_target_vel {
motor_rhs =
rb2.angvel.dot(&motor_axis1) - rb1.angvel.dot(&motor_axis1) - motor_target_vel;
motor_inv_lhs = crate::utils::inv(
motor_axis2.dot(&ii2.transform_vector(motor_axis2))
+ motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
);
motor_max_impulse = joint.motor_max_torque;
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
params.dt,
joint.motor_stiffness,
joint.motor_damping,
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
crate::utils::inv(
motor_axis2.dot(&ii2.transform_vector(motor_axis2))
+ motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
) * gamma
} else {
gamma
};
motor_rhs /= gamma;
}
/*
@@ -125,8 +145,10 @@ impl RevoluteVelocityConstraint {
let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse));
impulse[3] = rotated_impulse.x * params.warmstart_coeff;
impulse[4] = rotated_impulse.y * params.warmstart_coeff;
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
* params.warmstart_coeff;
let result = RevoluteVelocityConstraint {
RevoluteVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
@@ -146,10 +168,9 @@ impl RevoluteVelocityConstraint {
motor_max_impulse,
motor_axis1,
motor_axis2,
motor_impulse: joint.motor_impulse * params.warmstart_coeff,
};
AnyJointVelocityConstraint::RevoluteConstraint(result)
motor_impulse,
motor_angle,
}
}
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
@@ -174,7 +195,7 @@ impl RevoluteVelocityConstraint {
/*
* Motor
*/
{
if self.motor_inv_lhs != 0.0 {
mj_lambda1.angular += self
.ii1_sqrt
.transform_vector(self.motor_axis1 * self.motor_impulse);
@@ -224,8 +245,14 @@ impl RevoluteVelocityConstraint {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1);
let rhs = ang_dvel + self.motor_rhs;
let impulse = self.motor_inv_lhs * rhs;
self.motor_impulse += impulse;
let new_motor_impulse = na::clamp(
self.motor_impulse + self.motor_inv_lhs * rhs,
-self.motor_max_impulse,
self.motor_max_impulse,
);
let impulse = new_motor_impulse - self.motor_impulse;
self.motor_impulse = new_motor_impulse;
mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
@@ -242,6 +269,7 @@ impl RevoluteVelocityConstraint {
let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
revolute.world_ang_impulse = self.basis1 * rot_part;
revolute.prev_axis1 = self.motor_axis1;
revolute.motor_last_angle = self.motor_angle;
revolute.motor_impulse = self.motor_impulse;
}
}
@@ -264,6 +292,7 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
motor_rhs: Real,
motor_impulse: Real,
motor_max_impulse: Real,
motor_angle: Real, // Exists just for writing it into the joint.
basis2: Matrix3x2<Real>,
@@ -328,18 +357,41 @@ impl RevoluteVelocityGroundConstraint {
/*
* Motor part.
*/
let motor_axis1 = rb1.position * *joint.local_axis1;
let motor_axis2 = rb2.position * *joint.local_axis2;
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let mut motor_max_impulse = 0.0;
let mut motor_axis2 = Vector::zeros();
let mut motor_max_impulse = joint.motor_max_impulse;
let mut motor_angle = 0.0;
if let Some(motor_target_vel) = joint.motor_target_vel {
motor_axis2 = rb2.position * *joint.local_axis2;
motor_rhs = rb2.angvel.dot(&motor_axis2) - motor_target_vel;
motor_inv_lhs = crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2)));
motor_max_impulse = joint.motor_max_torque;
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
params.dt,
joint.motor_stiffness,
joint.motor_damping,
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))) * gamma
} else {
gamma
};
motor_rhs /= gamma;
}
let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
* params.warmstart_coeff;
let result = RevoluteVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
@@ -351,10 +403,11 @@ impl RevoluteVelocityGroundConstraint {
rhs,
r2,
motor_inv_lhs,
motor_impulse: joint.motor_impulse,
motor_impulse,
motor_axis2,
motor_max_impulse,
motor_rhs,
motor_angle,
};
AnyJointVelocityConstraint::RevoluteGroundConstraint(result)
@@ -374,7 +427,7 @@ impl RevoluteVelocityGroundConstraint {
/*
* Motor
*/
{
if self.motor_inv_lhs != 0.0 {
mj_lambda2.angular -= self
.ii2_sqrt
.transform_vector(self.motor_axis2 * self.motor_impulse);
@@ -410,8 +463,14 @@ impl RevoluteVelocityGroundConstraint {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let ang_dvel = ang_vel2.dot(&self.motor_axis2);
let rhs = ang_dvel + self.motor_rhs;
let impulse = self.motor_inv_lhs * rhs;
self.motor_impulse += impulse;
let new_motor_impulse = na::clamp(
self.motor_impulse + self.motor_inv_lhs * rhs,
-self.motor_max_impulse,
self.motor_max_impulse,
);
let impulse = new_motor_impulse - self.motor_impulse;
self.motor_impulse = new_motor_impulse;
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
}
@@ -425,6 +484,7 @@ impl RevoluteVelocityGroundConstraint {
if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
revolute.impulse = self.impulse;
revolute.motor_impulse = self.motor_impulse;
revolute.motor_last_angle = self.motor_angle;
}
}
}