From e9f17f32e8dda4b97d2eb7b2118b7373d0c554d0 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 19 Feb 2021 15:21:25 +0100 Subject: Complete the implementation of non-simd joint motor for the revolute joint. --- src/dynamics/joint/generic_joint.rs | 22 +++ src/dynamics/joint/joint.rs | 34 +++-- src/dynamics/joint/mod.rs | 6 +- src/dynamics/joint/revolute_joint.rs | 95 ++++++++++++- src/dynamics/joint/spring_model.rs | 59 ++++++++ src/dynamics/mod.rs | 9 +- .../generic_velocity_constraint.rs | 152 +++++++++++---------- .../solver/joint_constraint/joint_constraint.rs | 84 ++++++------ .../joint_constraint/joint_position_constraint.rs | 39 +++--- src/dynamics/solver/joint_constraint/mod.rs | 40 +++--- .../revolute_position_constraint.rs | 126 +---------------- .../revolute_velocity_constraint.rs | 126 ++++++++++++----- 12 files changed, 460 insertions(+), 332 deletions(-) create mode 100644 src/dynamics/joint/spring_model.rs (limited to 'src/dynamics') diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index fa35520..5d776b0 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -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 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 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); diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 31c5e0a..fe77b4b 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -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 for JointParams { } } -impl From for JointParams { - fn from(j: GenericJoint) -> Self { - JointParams::GenericJoint(j) - } -} +// impl From for JointParams { +// fn from(j: GenericJoint) -> Self { +// JointParams::GenericJoint(j) +// } +// } #[cfg(feature = "dim3")] impl From for JointParams { diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 92dd715..72a7483 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -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; diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index ad7db0d..9af0ae6 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -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, + /// 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, + // The world-space orientation of the free axis of the first attached body. + pub(crate) prev_axis1: Vector, } 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, + body_pos2: &Isometry, + ) -> 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 + } } diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs new file mode 100644 index 0000000..dd6035d --- /dev/null +++ b/src/dynamics/joint/spring_model.rs @@ -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), + } + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 3b6a977..eab6916 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -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}; diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index 0208dda..f391873 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -32,7 +32,8 @@ pub(crate) struct GenericVelocityConstraint { r1: Vector, r2: Vector, - basis: Rotation, + basis1: Rotation, + basis2: Rotation, dependant_set_mask: SpacialVector, vel: GenericConstraintPart, @@ -44,22 +45,22 @@ impl GenericVelocityConstraint { max_velocity: &SpatialVector, r1: &Vector, r2: &Vector, - basis: &Rotation, + basis1: &Rotation, + basis2: &Rotation, rb1: &RigidBody, rb2: &RigidBody, ) -> SpatialVector { - 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::(DIM).into_owned(); let max_linvel = max_velocity.xyz(); let max_angvel = max_velocity.fixed_rows::(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, + max_impulse: &Vector, + dependant_set_mask: &mut Vector, + mut delassus: na::Matrix3, + ) -> na::Matrix3 { + // 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, @@ -169,32 +196,6 @@ impl GenericVelocityConstraint { } } - pub fn invert_partial_delassus_matrix( - min_impulse: &Vector, - max_impulse: &Vector, - dependant_set_mask: &mut Vector, - mut delassus: na::Matrix3, - ) -> na::Matrix3 { - // 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).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) diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 12b8f77..ebc3c4a 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -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) diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index a13cc27..40f5d5a 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -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")] diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index b8e833e..9196e69 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -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; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index e468508..9075ed7 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -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::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::(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::(0).into_owned(); - let ang_impulse1 = basis1 * impulse.fixed_rows::(3).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::(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]) { 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::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::(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::(0).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::(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; } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 650a9e5..1219c39 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -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, motor_axis2: Vector, @@ -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; - - 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 mut motor_max_impulse = joint.motor_max_impulse; + let mut motor_angle = 0.0; + + 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]) { @@ -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::(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, @@ -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(); - - 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 mut motor_max_impulse = joint.motor_max_impulse; + let mut motor_angle = 0.0; + + 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; } } } -- cgit