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

fix more 2D angular motor/limits jacobians

Browse Source
This commit is contained in:
Sébastien Crozet
2023-12-10 21:39:11 +01:00
parent 7c43e96943
commit fef7f0f92a
2 changed files with 41 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,6 +13,8 @@ use na::{DVector, SVector};
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
use crate::utils::WAngularInertia; use crate::utils::WAngularInertia;
#[cfg(feature = "dim2")]
use na::Vector1;
impl SolverBody<Real, 1> { impl SolverBody<Real, 1> {
pub fn fill_jacobians( pub fn fill_jacobians(
@@ -295,10 +297,13 @@ impl JointVelocityConstraintBuilder<Real> {
body2: &SolverBody<Real, 1>, body2: &SolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>, mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>,
locked_axis: usize, _locked_axis: usize,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityConstraint { ) -> JointGenericVelocityConstraint {
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic( let mut constraint = self.lock_jacobians_generic(
params, params,
@@ -319,7 +324,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
constraint.rhs += rhs_bias; constraint.rhs += rhs_bias;
constraint constraint
} }
@@ -334,11 +339,14 @@ impl JointVelocityConstraintBuilder<Real> {
body2: &SolverBody<Real, 1>, body2: &SolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>, mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>,
limited_axis: usize, _limited_axis: usize,
limits: [Real; 2], limits: [Real; 2],
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityConstraint { ) -> JointGenericVelocityConstraint {
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); #[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic( let mut constraint = self.lock_jacobians_generic(
params, params,
@@ -359,7 +367,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang = (self.ang_err.angle() / 2.0).sin(); let s_ang = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang = self.ang_err.imag()[limited_axis]; let s_ang = self.ang_err.imag()[_limited_axis];
let min_enabled = s_ang < s_limits[0]; let min_enabled = s_ang < s_limits[0];
let max_enabled = s_limits[1] < s_ang; let max_enabled = s_limits[1] < s_ang;
let impulse_bounds = [ let impulse_bounds = [
@@ -390,9 +398,8 @@ impl JointVelocityConstraintBuilder<Real> {
motor_params: &MotorParameters<Real>, motor_params: &MotorParameters<Real>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityConstraint { ) -> JointGenericVelocityConstraint {
// let mut ang_jac = self.ang_basis.column(motor_axis).into_owned();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0); // self.ang_basis[(0, 0)]); let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.basis.column(_motor_axis).into_owned(); let ang_jac = self.basis.column(_motor_axis).into_owned();
@@ -683,10 +690,13 @@ impl JointVelocityConstraintBuilder<Real> {
joint_id: JointIndex, joint_id: JointIndex,
body1: &SolverBody<Real, 1>, body1: &SolverBody<Real, 1>,
mb2: (&Multibody, usize), mb2: (&Multibody, usize),
locked_axis: usize, _locked_axis: usize,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint { ) -> JointGenericVelocityGroundConstraint {
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground( let mut constraint = self.lock_jacobians_generic_ground(
params, params,
@@ -705,7 +715,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
constraint.rhs += rhs_bias; constraint.rhs += rhs_bias;
constraint constraint
} }
@@ -718,11 +728,14 @@ impl JointVelocityConstraintBuilder<Real> {
joint_id: JointIndex, joint_id: JointIndex,
body1: &SolverBody<Real, 1>, body1: &SolverBody<Real, 1>,
mb2: (&Multibody, usize), mb2: (&Multibody, usize),
limited_axis: usize, _limited_axis: usize,
limits: [Real; 2], limits: [Real; 2],
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint { ) -> JointGenericVelocityGroundConstraint {
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); #[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic_ground( let mut constraint = self.lock_jacobians_generic_ground(
params, params,
@@ -741,7 +754,7 @@ impl JointVelocityConstraintBuilder<Real> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang = (self.ang_err.angle() / 2.0).sin(); let s_ang = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang = self.ang_err.imag()[limited_axis]; let s_ang = self.ang_err.imag()[_limited_axis];
let min_enabled = s_ang < s_limits[0]; let min_enabled = s_ang < s_limits[0];
let max_enabled = s_limits[1] < s_ang; let max_enabled = s_limits[1] < s_ang;
let impulse_bounds = [ let impulse_bounds = [
@@ -771,7 +784,6 @@ impl JointVelocityConstraintBuilder<Real> {
motor_params: &MotorParameters<Real>, motor_params: &MotorParameters<Real>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointGenericVelocityGroundConstraint { ) -> JointGenericVelocityGroundConstraint {
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0); let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]

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

@@ -282,7 +282,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
limited_axis: usize, _limited_axis: usize,
limits: [N; 2], limits: [N; 2],
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> { ) -> JointVelocityConstraint<N, LANES> {
@@ -292,7 +292,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang = (self.ang_err.angle() * half).simd_sin(); let s_ang = (self.ang_err.angle() * half).simd_sin();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang = self.ang_err.imag()[limited_axis]; let s_ang = self.ang_err.imag()[_limited_axis];
let min_enabled = s_ang.simd_lt(s_limits[0]); let min_enabled = s_ang.simd_lt(s_limits[0]);
let max_enabled = s_limits[1].simd_lt(s_ang); let max_enabled = s_limits[1].simd_lt(s_ang);
@@ -302,9 +302,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
]; ];
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = self.ang_basis[limited_axis]; let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel; let rhs_wo_bias = dvel;
@@ -346,7 +346,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
motor_params: &MotorParameters<N>, motor_params: &MotorParameters<N>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> { ) -> JointVelocityConstraint<N, LANES> {
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = N::one(); let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
@@ -397,13 +396,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
locked_axis: usize, _locked_axis: usize,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> { ) -> JointVelocityConstraint<N, LANES> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = self.ang_basis[locked_axis]; let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel; let rhs_wo_bias = dvel;
@@ -413,7 +412,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * 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_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -790,7 +789,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
motor_params: &MotorParameters<N>, motor_params: &MotorParameters<N>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> { ) -> JointVelocityGroundConstraint<N, LANES> {
// let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = N::one(); let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
@@ -857,7 +855,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
]; ];
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = self.ang_basis[limited_axis]; let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let ang_jac = self.ang_basis.column(limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
@@ -894,13 +892,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
locked_axis: usize, _locked_axis: usize,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> { ) -> JointVelocityGroundConstraint<N, LANES> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = self.ang_basis[locked_axis]; let ang_jac = N::one();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel; let rhs_wo_bias = dvel;
@@ -909,7 +908,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;