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

Joint API and joint motors improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:55:00 +01:00
committed by Sébastien Crozet
parent e740493b98
commit fb20d72ee2
108 changed files with 2650 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody};
use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
use crate::math::{Isometry, Real, DIM};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorSlice, DVectorSliceMut};
@@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint {
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
@@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint {
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0),
}
}
@@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint {
mb2: Option<(&Multibody, usize)>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
@@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint {
locked_axes,
);
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
params,
jacobians,
j_id,
@@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i - DIM,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic(
@@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint {
len += 1;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
params,
jacobians,
j_id,
@@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic(
params,
jacobians,
j_id,
@@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint {
mb1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
@@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint {
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
len
}
@@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint {
let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * dvel,
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);
@@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint {
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
@@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint {
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0),
}
}
@@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint {
mb2: (&Multibody, usize),
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
@@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint {
locked_axes,
);
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic_ground(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground(
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_ground(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb2,
i - DIM,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
@@ -418,27 +421,30 @@ impl JointGenericVelocityGroundConstraint {
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_ground(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_ground(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic_ground(
params,
jacobians,
j_id,
@@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint {
body1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
@@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint {
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_ground(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
jacobians,
&mut out[..len],
&mut out[start..len],
);
len
}
@@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint {
let dvel = self.rhs + vel2;
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * dvel,
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);

98
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder<Real> {
j.copy_from(&wj);
}
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
let rhs_wo_bias = vel2 - vel1;
let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
@@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0,
rhs: rhs_wo_bias,
rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id,
}
}
@@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
@@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint.impulse_bounds = [
@@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - motor_params.target_vel;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint
}
@@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
@@ -409,22 +411,22 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.sin();
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.rhs = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint
}
@@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
let len = constraints.len();
if len == 0 {
return;
}
let ndofs1 = constraints[0].ndofs1;
let ndofs2 = constraints[0].ndofs2;
@@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder<Real> {
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
let inv_dot_jj = crate::utils::inv(dot_jj);
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others
@@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder<Real> {
let vel2 = mb2
.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
.1;
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
let rhs_wo_bias = vel2 - vel1;
let mj_lambda2 = mb2.solver_id;
@@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0,
rhs: rhs_wo_bias,
rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id,
}
}
@@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
@@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint.impulse_bounds = [
@@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - motor_params.target_vel;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint
}
@@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac,
);
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
@@ -778,22 +789,22 @@ impl JointVelocityConstraintBuilder<Real> {
);
let mut rhs = 0.0;
if motor_params.stiffness != 0.0 {
if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.sin();
rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness;
rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs += dvel - motor_params.target_vel;
constraint.rhs_wo_bias = rhs;
constraint.rhs = rhs;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint
}
@@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
let len = constraints.len();
if len == 0 {
return;
}
let ndofs2 = constraints[0].ndofs2;
// Use the modified Gramm-Schmidt orthogonalization.
@@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder<Real> {
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
let dot_jj = jac_j2.dot(&w_jac_j2);
let inv_dot_jj = crate::utils::inv(dot_jj);
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others

307
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,6 +1,8 @@
use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::utils::{WDot, WReal};
@@ -12,10 +14,9 @@ use {
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: WReal> {
pub stiffness: N,
pub damping: N,
pub gamma: N,
// pub keep_lhs: bool,
pub erp_inv_dt: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub target_pos: N,
pub target_vel: N,
pub max_impulse: N,
@@ -24,10 +25,9 @@ pub struct MotorParameters<N: WReal> {
impl<N: WReal> Default for MotorParameters<N> {
fn default() -> Self {
Self {
stiffness: N::zero(),
damping: N::zero(),
gamma: N::zero(),
// keep_lhs: true,
erp_inv_dt: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
target_pos: N::zero(),
target_vel: N::zero(),
max_impulse: N::zero(),
@@ -72,6 +72,8 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub inv_lhs: N,
pub rhs: N,
pub rhs_wo_bias: N,
pub cfm_gain: N,
pub cfm_coeff: N,
pub im1: Vector<N>,
pub im2: Vector<N>,
@@ -91,6 +93,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
ang_jac1: na::zero(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_gain: N::zero(),
cfm_coeff: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im1: na::zero(),
@@ -105,7 +109,7 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
let rhs = dlinvel + dangvel + self.rhs;
let total_impulse = (self.impulse + self.inv_lhs * rhs)
let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
@@ -133,13 +137,14 @@ impl JointVelocityConstraint<Real, 1> {
body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
frame1,
@@ -149,13 +154,62 @@ impl JointVelocityConstraint<Real, 1> {
locked_axes,
);
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] =
builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.motor_angular(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
let limits = if limit_axes & (1 << i) != 0 {
Some([joint.limits[i].min, joint.limits[i].max])
} else {
None
};
out[len] = builder.motor_linear(
params,
[joint_id],
body1,
body2,
i,
&joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
);
len += 1;
}
}
if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular motor constraint.
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint.
// out[len] = builder.motor_linear_coupled(
// params,
// [joint_id],
// body1,
// body2,
// limit_axes & coupled_axes,
// &joint.limits,
// WritebackId::Limit(0), // TODO: writeback
// );
// len += 1;
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular(
@@ -169,52 +223,16 @@ impl JointVelocityConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear(
params,
[joint_id],
body1,
body2,
locked_axes >> DIM,
i,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
if locked_axes & (1 << i) != 0 {
out[len] =
builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular(
params,
[joint_id],
@@ -227,8 +245,40 @@ impl JointVelocityConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_linear(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint.
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint.
out[len] = builder.limit_linear_coupled(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
len
}
@@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub ang_jac2: AngVector<N>,
pub inv_lhs: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub rhs: N,
pub rhs_wo_bias: N,
@@ -367,6 +419,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
lin_jac: Vector::zeros(),
ang_jac2: na::zero(),
inv_lhs: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
rhs: N::zero(),
rhs_wo_bias: N::zero(),
im2: na::zero(),
@@ -379,7 +433,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
let dangvel = mj_lambda2.angular;
let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
let total_impulse = (self.impulse + self.inv_lhs * dvel)
let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
@@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint<Real, 1> {
body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &JointData,
joint: &GenericJoint,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits() as u8;
let motor_axes = joint.motor_axes.bits() as u8;
let limit_axes = joint.limit_axes.bits() as u8;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new(
frame1,
@@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint<Real, 1> {
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.motor_angular_ground(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
let limits = if limit_axes & (1 << i) != 0 {
Some([joint.limits[i].min, joint.limits[i].max])
} else {
None
};
out[len] = builder.motor_linear_ground(
params,
[joint_id],
body1,
body2,
i,
WritebackId::Dof(i),
&joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
);
len += 1;
}
}
if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular motor constraint.
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
/*
// TODO: coupled linear motor constraint.
out[len] = builder.motor_linear_coupled_ground(
params,
[joint_id],
body1,
body2,
motor_axes & coupled_axes,
&joint.motors,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
*/
todo!()
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_ground(
@@ -446,50 +550,22 @@ impl JointVelocityGroundConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_ground(
[joint_id],
body1,
body2,
i,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_ground(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_ground(
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_ground(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular_ground(
params,
[joint_id],
@@ -502,8 +578,39 @@ impl JointVelocityGroundConstraint<Real, 1> {
len += 1;
}
}
for i in 0..DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_linear_ground(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint.
}
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
out[len] = builder.limit_linear_coupled_ground(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
}
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
len
}

405
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
};
use crate::dynamics::solver::joint_constraint::SolverBody;
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{IntegrationParameters, JointIndex};
use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor};
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
use na::SMatrix;
@@ -92,10 +92,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let min_enabled = dist.simd_lt(limits[0]);
let max_enabled = limits[1].simd_lt(dist);
let erp_inv_dt = N::splat(params.erp_inv_dt());
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias =
((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
constraint.cfm_coeff = cfm_coeff;
constraint.impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero),
N::splat(Real::INFINITY).select(max_enabled, zero),
@@ -104,39 +106,114 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraint
}
pub fn limit_linear_coupled<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
JointVelocityConstraint {
joint_id,
mj_lambda1: body1.mj_lambda,
mj_lambda2: body2.mj_lambda,
im1: body1.im,
im2: body2.im,
impulse: N::zero(),
impulse_bounds,
lin_jac,
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
}
pub fn motor_linear<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
_locked_ang_axes: u8,
motor_axis: usize,
motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let mut constraint =
self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
// if locked_ang_axes & (1 << motor_axis) != 0 {
// // FIXME: check that this also works for cases
// // when not all the angular axes are locked.
// constraint.ang_jac1 = na::zero();
// constraint.ang_jac2 = na::zero();
// }
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != N::zero() {
let dist = self.lin_err.dot(&constraint.lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != N::zero() {
let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+ (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
let dist = self.lin_err.dot(&constraint.lin_jac);
target_vel =
target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
};
let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+ (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - target_vel;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
@@ -164,10 +241,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
ang_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -184,6 +262,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -220,12 +300,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).simd_max(zero))
* N::splat(erp_inv_dt);
* erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -242,6 +323,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -264,20 +347,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != N::zero() {
#[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.simd_sin();
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
if motor_params.damping != N::zero() {
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias +=
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -294,6 +374,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
@@ -315,13 +397,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -338,6 +421,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -349,6 +434,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityConstraint<N, LANES>],
) {
let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im1 + constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization.
@@ -357,8 +447,10 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_j.ang_jac1.gdot(c_j.ang_jac1)
+ c_j.ang_jac2.gdot(c_j.ang_jac2);
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
// Don't remove constraints with limited forces from the others
@@ -369,6 +461,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
for i in (j + 1)..len {
let (c_i, c_j) = constraints.index_mut_const(i, j);
let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_i.ang_jac1.gdot(c_j.ang_jac1)
+ c_i.ang_jac2.gdot(c_j.ang_jac2);
@@ -396,6 +489,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let zero = N::zero();
let lin_jac = self.basis.column(limited_axis).into_owned();
let dist = self.lin_err.dot(&lin_jac);
let min_enabled = dist.simd_lt(limits[0]);
let max_enabled = limits[1].simd_lt(dist);
@@ -412,11 +506,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero))
* N::splat(erp_inv_dt);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias =
((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -429,21 +524,97 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
}
}
pub fn limit_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
JointVelocityGroundConstraint {
joint_id,
mj_lambda2: body2.mj_lambda,
im2: body2.im,
impulse: N::zero(),
impulse_bounds,
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
}
pub fn motor_linear_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
motor_axis: usize,
motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
#[cfg(feature = "dim2")]
@@ -452,16 +623,21 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != N::zero() {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != N::zero() {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
let dist = self.lin_err.dot(&lin_jac);
target_vel =
target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
};
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += dvel - target_vel;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -474,12 +650,89 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
}
}
pub fn motor_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
motor_coupled_axes: u8,
motors: &[MotorParameters<N>],
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
todo!()
/*
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
let mut limit = N::zero();
for i in 0..DIM {
if limited_coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
limit += limits[i].max * limits[i].max;
}
}
limit = limit.simd_sqrt();
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
ang_jac2 = body2.sqrt_ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
JointVelocityGroundConstraint {
joint_id,
mj_lambda2: body2.mj_lambda,
im2: body2.im,
impulse: N::zero(),
impulse_bounds,
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs,
rhs_wo_bias,
writeback_id,
}
*/
}
pub fn lock_linear_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
@@ -498,10 +751,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -514,6 +768,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -536,20 +792,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() {
if motor_params.erp_inv_dt != N::zero() {
#[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.simd_sin();
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
}
if motor_params.damping != N::zero() {
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias +=
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
}
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += dvel - motor_params.target_vel;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -562,6 +815,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
@@ -598,12 +853,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).simd_max(zero))
* N::splat(erp_inv_dt);
* erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -616,6 +872,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -636,13 +894,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -655,6 +914,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(),
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
@@ -666,6 +927,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
) {
let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization.
@@ -673,18 +939,23 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let c_j = &mut constraints[j];
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_j.ang_jac2.gdot(c_j.ang_jac2);
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain);
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)]
|| c_j.cfm_gain != N::zero()
{
// Don't remove constraints with limited forces from the others
// because they may not deliver the necessary forces to fulfill
// the removed parts of other constraints.
continue;
}
for i in (j + 1)..len {
for i in j + 1..len {
let (c_i, c_j) = constraints.index_mut_const(i, j);
let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_i.ang_jac2.gdot(c_j.ang_jac2);
let coeff = dot_ij * inv_dot_jj;