diff --git a/Cargo.toml b/Cargo.toml index db1ac16..0b0cdd2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,10 +18,11 @@ members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "e #nalgebra = { path = "../nalgebra" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } -#parry2d = { git = "https://github.com/sebcrozet/parry" } -#parry3d = { git = "https://github.com/sebcrozet/parry" } -#parry2d-f64 = { git = "https://github.com/sebcrozet/parry" } -#parry3d-f64 = { git = "https://github.com/sebcrozet/parry" } +nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } +parry2d = { git = "https://github.com/dimforge/parry" } +parry3d = { git = "https://github.com/dimforge/parry" } +parry2d-f64 = { git = "https://github.com/dimforge/parry" } +parry3d-f64 = { git = "https://github.com/dimforge/parry" } #ncollide2d = { git = "https://github.com/dimforge/ncollide" } #ncollide3d = { git = "https://github.com/dimforge/ncollide" } #nphysics2d = { git = "https://github.com/dimforge/nphysics" } diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index d777655..1246bc4 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -1,7 +1,7 @@ -use na::{Isometry3, Point3, Unit, Vector3}; +use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3}; use rapier3d::dynamics::{ BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, - RigidBodySet, + RigidBodyHandle, RigidBodySet, }; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -14,7 +14,7 @@ fn create_prismatic_joints( num: usize, ) { let rad = 0.4; - let shift = 1.0; + let shift = 2.0; let ground = RigidBodyBuilder::new_static() .translation(origin.x, origin.y, origin.z) @@ -40,7 +40,7 @@ fn create_prismatic_joints( let z = Vector3::z(); let mut prism = PrismaticJoint::new( - Point3::origin(), + Point3::new(0.0, 0.0, 0.0), axis, z, Point3::new(0.0, 0.0, -shift), @@ -50,6 +50,74 @@ fn create_prismatic_joints( prism.limits_enabled = true; prism.limits[0] = -2.0; prism.limits[1] = 2.0; + + joints.insert(bodies, curr_parent, curr_child, prism); + + curr_parent = curr_child; + } +} + +fn create_actuated_prismatic_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(origin.x, origin.y, origin.z) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, bodies); + + for i in 0..num { + let z = origin.z + (i + 1) as f32 * shift; + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(origin.x, origin.y, z) + .build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_child, bodies); + + let axis = if i % 2 == 0 { + Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)) + } else { + Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0)) + }; + + let z = Vector3::z(); + let mut prism = PrismaticJoint::new( + Point3::new(0.0, 0.0, 0.0), + axis, + z, + Point3::new(0.0, 0.0, -shift), + axis, + z, + ); + + if i == 1 { + prism.configure_motor_velocity(1.0, 1.0); + prism.limits_enabled = true; + prism.limits[1] = 5.0; + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + prism.motor_max_impulse = 1.0; + } else if i > 1 { + prism.configure_motor_position(2.0, 0.2, 1.0); + } else { + prism.configure_motor_velocity(1.0, 1.0); + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + prism.motor_max_impulse = 1.0; + prism.limits_enabled = true; + prism.limits[0] = -2.0; + prism.limits[1] = 5.0; + } + joints.insert(bodies, curr_parent, curr_child, prism); curr_parent = curr_child; @@ -222,6 +290,130 @@ fn create_ball_joints( } } +fn create_actuated_revolute_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the joints here. + let joint_template = RevoluteJoint::new( + Point3::origin(), + Vector3::z_axis(), + Point3::new(0.0, 0.0, -shift), + Vector3::z_axis(), + ); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let shifty = (i >= 1) as u32 as f32 * -2.0; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(origin.x, origin.y + shifty, origin.z + fi * shift) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + colliders.insert(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i % 3 == 1 { + joint.configure_motor_velocity(-20.0, 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint.configure_motor_position(3.14 / 2.0, stiffness, damping); + } + + if i == 1 { + joint.local_anchor2.y = 2.0; + joint.configure_motor_velocity(-2.0, 0.1); + } + + joints.insert(bodies, parent_handle, child_handle, joint); + } + + parent_handle = child_handle; + } +} + +fn create_actuated_ball_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the joints here. + let joint_template = BallJoint::new(Point3::new(0.0, 0.0, shift), Point3::origin()); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(origin.x, origin.y, origin.z + fi * shift) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + colliders.insert(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i == 1 { + joint.configure_motor_velocity(Vector3::new(0.0, 0.5, -2.0), 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint.configure_motor_position( + UnitQuaternion::new(Vector3::new(0.0, 1.0, 3.14 / 2.0)), + stiffness, + damping, + ); + } + + joints.insert(bodies, parent_handle, child_handle, joint); + } + + parent_handle = child_handle; + } +} + pub fn init_world(testbed: &mut Testbed) { /* * World @@ -234,8 +426,15 @@ pub fn init_world(testbed: &mut Testbed) { &mut bodies, &mut colliders, &mut joints, - Point3::new(20.0, 10.0, 0.0), - 5, + Point3::new(20.0, 5.0, 0.0), + 4, + ); + create_actuated_prismatic_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(25.0, 5.0, 0.0), + 4, ); create_revolute_joints( &mut bodies, @@ -249,7 +448,21 @@ pub fn init_world(testbed: &mut Testbed) { &mut colliders, &mut joints, Point3::new(0.0, 10.0, 0.0), - 5, + 10, + ); + create_actuated_revolute_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(20.0, 10.0, 0.0), + 6, + ); + create_actuated_ball_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(13.0, 10.0, 0.0), + 3, ); create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15); diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 5d0d221..caad9b5 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,7 +1,7 @@ use crate::math::Real; /// Parameters for a time-step of the physics engine. -#[derive(Clone)] +#[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { /// The timestep length (default: `1.0 / 60.0`) diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index 82e2a10..01b0f7f 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -1,4 +1,5 @@ -use crate::math::{Point, Real, Vector}; +use crate::dynamics::SpringModel; +use crate::math::{Point, Real, Rotation, Vector}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -12,6 +13,31 @@ pub struct BallJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub impulse: Vector, + + /// The target relative angular velocity the motor will attempt to reach. + #[cfg(feature = "dim2")] + pub motor_target_vel: Real, + /// The target relative angular velocity the motor will attempt to reach. + #[cfg(feature = "dim3")] + pub motor_target_vel: Vector, + /// The target angular position of this joint, expressed as an axis-angle. + pub motor_target_pos: Rotation, + /// 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. + #[cfg(feature = "dim2")] + pub motor_impulse: Real, + /// The angular impulse applied by the motor. + #[cfg(feature = "dim3")] + pub motor_impulse: Vector, + /// The spring-like model used by the motor to reach the target velocity and . + pub motor_model: SpringModel, } impl BallJoint { @@ -29,6 +55,76 @@ impl BallJoint { local_anchor1, local_anchor2, impulse, + motor_target_vel: na::zero(), + motor_target_pos: Rotation::identity(), + motor_stiffness: 0.0, + motor_damping: 0.0, + motor_impulse: na::zero(), + motor_max_impulse: Real::MAX, + motor_model: SpringModel::default(), } } + + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + // SIMD ball constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + /// Sets the target velocity and velocity correction factor this motor. + #[cfg(feature = "dim2")] + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + /// Sets the target velocity and velocity correction factor this motor. + #[cfg(feature = "dim3")] + pub fn configure_motor_velocity(&mut self, target_vel: Vector, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + /// Sets the target orientation this motor needs to reach. + pub fn configure_motor_position( + &mut self, + target_pos: Rotation, + stiffness: Real, + damping: Real, + ) { + self.configure_motor(target_pos, na::zero(), stiffness, damping) + } + + /// Sets the target orientation this motor needs to reach. + #[cfg(feature = "dim2")] + pub fn configure_motor( + &mut self, + target_pos: Rotation, + 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; + } + + /// Configure both the target orientation and target velocity of the motor. + #[cfg(feature = "dim3")] + pub fn configure_motor( + &mut self, + target_pos: Rotation, + target_vel: Vector, + stiffness: Real, + damping: Real, + ) { + self.motor_target_vel = target_vel; + self.motor_target_pos = target_pos; + self.motor_stiffness = stiffness; + self.motor_damping = damping; + } } diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 359e14a..2917757 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -30,4 +30,9 @@ impl FixedJoint { impulse: SpacialVector::zeros(), } } + + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + true + } } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs new file mode 100644 index 0000000..c1549ff --- /dev/null +++ b/src/dynamics/joint/generic_joint.rs @@ -0,0 +1,144 @@ +use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; +use crate::math::{Isometry, Real, SpacialVector}; +use crate::na::{Rotation3, UnitQuaternion}; + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that prevents all relative movement between two bodies. +/// +/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. +pub struct GenericJoint { + /// The frame of reference for the first body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor1: Isometry, + /// The frame of reference for the second body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor2: Isometry, + /// The impulse applied to the first body affected by this joint. + /// + /// The impulse applied to the second body affected by this joint is given by `-impulse`. + /// This combines both linear and angular impulses: + /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. + /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. + pub impulse: SpacialVector, + + pub min_position: SpacialVector, + pub max_position: SpacialVector, + pub min_velocity: SpacialVector, + pub max_velocity: SpacialVector, + /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_impulse: SpacialVector, + /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 + pub max_impulse: SpacialVector, + /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_pos_impulse: SpacialVector, + /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0 + pub max_pos_impulse: SpacialVector, +} + +impl GenericJoint { + /// Creates a new fixed joint from the frames of reference of both bodies. + pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { + Self { + local_anchor1, + local_anchor2, + impulse: SpacialVector::zeros(), + min_position: SpacialVector::zeros(), + max_position: SpacialVector::zeros(), + min_velocity: SpacialVector::zeros(), + max_velocity: SpacialVector::zeros(), + min_impulse: SpacialVector::repeat(-Real::MAX), + max_impulse: SpacialVector::repeat(Real::MAX), + min_pos_impulse: SpacialVector::repeat(-Real::MAX), + max_pos_impulse: SpacialVector::repeat(Real::MAX), + } + } + + 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; + self.min_velocity[dof as usize] = -Real::MAX; + self.max_velocity[dof as usize] = Real::MAX; + self.min_impulse[dof as usize] = 0.0; + self.max_impulse[dof as usize] = 0.0; + self.min_pos_impulse[dof as usize] = 0.0; + self.max_pos_impulse[dof as usize] = 0.0; + } + + pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) { + self.min_position[dof as usize] = min; + self.max_position[dof as usize] = max; + } +} + +impl From for GenericJoint { + fn from(joint: RevoluteJoint) -> Self { + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); + let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); + let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); + + 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 + } +} + +impl From for GenericJoint { + fn from(joint: BallJoint) -> Self { + let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero()); + 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); + result + } +} + +impl From for GenericJoint { + fn from(joint: PrismaticJoint) -> Self { + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); + let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); + let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.free_dof(0); + result.set_dof_limits(0, joint.limits[0], joint.limits[1]); + result + } +} + +impl From for GenericJoint { + fn from(joint: FixedJoint) -> Self { + Self::new(joint.local_anchor1, joint.local_anchor2) + } +} diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 9fe6488..e0a9d38 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -17,6 +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), } impl JointParams { @@ -26,8 +27,9 @@ impl JointParams { JointParams::BallJoint(_) => 0, JointParams::FixedJoint(_) => 1, JointParams::PrismaticJoint(_) => 2, + // JointParams::GenericJoint(_) => 3, #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(_) => 3, + JointParams::RevoluteJoint(_) => 4, } } @@ -49,6 +51,15 @@ 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 prismatic joint, if `self` is one. pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> { if let JointParams::PrismaticJoint(j) = self { @@ -81,6 +92,12 @@ impl From for JointParams { } } +// impl From for JointParams { +// fn from(j: GenericJoint) -> Self { +// JointParams::GenericJoint(j) +// } +// } + #[cfg(feature = "dim3")] impl From for JointParams { fn from(j: RevoluteJoint) -> Self { @@ -111,3 +128,16 @@ pub struct Joint { /// The joint geometric parameters and impulse. pub params: JointParams, } + +impl Joint { + /// Can this joint use SIMD-accelerated constraint formulations? + pub fn supports_simd_constraints(&self) -> bool { + match &self.params { + JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(), + JointParams::FixedJoint(joint) => joint.supports_simd_constraints(), + JointParams::BallJoint(joint) => joint.supports_simd_constraints(), + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(), + } + } +} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index b4dd60e..72a7483 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,16 +1,20 @@ pub use self::ball_joint::BallJoint; pub use self::fixed_joint::FixedJoint; +// 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 joint; mod joint_set; mod prismatic_joint; #[cfg(feature = "dim3")] mod revolute_joint; +mod spring_model; diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 174ce79..3736b7f 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::SpringModel; use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::utils::WBasis; use na::Unit; @@ -36,10 +37,23 @@ pub struct PrismaticJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub limits_impulse: Real, - // pub motor_enabled: bool, - // pub target_motor_vel: Real, - // pub max_motor_impulse: Real, - // pub motor_impulse: Real, + + /// The target relative angular velocity the motor will attempt to reach. + pub motor_target_vel: Real, + /// The target relative angle along the joint axis the motor will attempt to reach. + pub motor_target_pos: Real, + /// The motor's stiffness. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_stiffness: Real, + /// The motor's damping. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_damping: Real, + /// The maximal impulse the motor is able to deliver. + pub motor_max_impulse: Real, + /// The angular impulse applied by the motor. + pub motor_impulse: Real, + /// The spring-like model used by the motor to reach the target velocity and . + pub motor_model: SpringModel, } impl PrismaticJoint { @@ -63,10 +77,13 @@ impl PrismaticJoint { limits_enabled: false, limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, - // motor_enabled: false, - // target_motor_vel: 0.0, - // max_motor_impulse: Real::MAX, - // motor_impulse: 0.0, + 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, + motor_model: SpringModel::VelocityBased, } } @@ -89,8 +106,8 @@ impl PrismaticJoint { Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3) { [ - local_bitangent1.into_inner(), local_bitangent1.cross(&local_axis1), + local_bitangent1.into_inner(), ] } else { local_axis1.orthonormal_basis() @@ -100,8 +117,8 @@ impl PrismaticJoint { Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3) { [ - local_bitangent2.into_inner(), local_bitangent2.cross(&local_axis2), + local_bitangent2.into_inner(), ] } else { local_axis2.orthonormal_basis() @@ -118,10 +135,13 @@ impl PrismaticJoint { limits_enabled: false, limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, - // motor_enabled: false, - // target_motor_vel: 0.0, - // max_motor_impulse: Real::MAX, - // motor_impulse: 0.0, + 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, + motor_model: SpringModel::VelocityBased, } } @@ -135,6 +155,12 @@ impl PrismaticJoint { self.local_axis2 } + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + // SIMD revolute constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) + } + // FIXME: precompute this? #[cfg(feature = "dim2")] pub(crate) fn local_frame1(&self) -> Isometry { @@ -190,4 +216,33 @@ impl PrismaticJoint { let translation = self.local_anchor2.coords.into(); Isometry::from_parts(translation, rotation) } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + /// Sets the target velocity this motor needs to reach. + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + /// Sets the target position this motor needs to reach. + pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { + self.configure_motor(target_pos, 0.0, stiffness, damping) + } + + /// Configure both the target position and target velocity of the motor. + 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; + } } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index ad7db0d..d1181e9 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,30 @@ 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 +66,84 @@ 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::default(), + motor_last_angle: 0.0, } } + + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + // SIMD revolute constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + /// Sets the target velocity this motor needs to reach. + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + /// Sets the target angle this motor needs to reach. + pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { + self.configure_motor(target_pos, 0.0, stiffness, damping) + } + + /// Configure both the target angle and target velocity of the motor. + 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..c2c9ebd --- /dev/null +++ b/src/dynamics/joint/spring_model.rs @@ -0,0 +1,65 @@ +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 Default for SpringModel { + fn default() -> Self { + SpringModel::VelocityBased + } +} + +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 8c38dc2..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, 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/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 21cc642..0f01798 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -214,6 +214,12 @@ impl InteractionGroups { continue; } + if !interaction.supports_simd_constraints() { + // This joint does not support simd constraints yet. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + let ijoint = interaction.params.type_id(); let i1 = body1.active_set_offset; let i2 = body2.active_set_offset; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index e75f978..a110fbb 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Real, SdpMatrix, Vector}; +use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -13,13 +13,18 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, rhs: Vector, - pub(crate) impulse: Vector, + impulse: Vector, r1: Vector, r2: Vector, inv_lhs: SdpMatrix, + motor_rhs: AngVector, + motor_impulse: AngVector, + motor_inv_lhs: Option>, + motor_max_impulse: Real, + im1: Real, im2: Real, @@ -33,10 +38,10 @@ impl BallVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &BallJoint, + joint: &BallJoint, ) -> Self { - let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com; - let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com; + let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com; + let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); @@ -77,17 +82,85 @@ impl BallVelocityConstraint { let inv_lhs = lhs.inverse_unchecked(); + /* + * Motor part. + */ + let mut motor_rhs = na::zero(); + let mut motor_inv_lhs = None; + let motor_max_impulse = joint.motor_max_impulse; + + if motor_max_impulse > 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 { + let dpos = rb2.position.rotation + * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + #[cfg(feature = "dim2")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / (ii1 + ii2)) + } else { + Some(gamma) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim3")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some((ii1 + ii2).inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; + } + } + + #[cfg(feature = "dim2")] + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; + #[cfg(feature = "dim3")] + let motor_impulse = + joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; + BallVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, im1, im2, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, r1: anchor1, r2: anchor2, rhs, inv_lhs, + motor_rhs, + motor_impulse, + motor_inv_lhs, + motor_max_impulse: joint.motor_max_impulse, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } @@ -98,18 +171,19 @@ impl BallVelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda1.linear += self.im1 * self.impulse; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse)); + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse); mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -124,6 +198,37 @@ impl BallVelocityConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + } + + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { + if let Some(motor_inv_lhs) = &self.motor_inv_lhs { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs; + + let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); + + #[cfg(feature = "dim2")] + let clamped_impulse = + na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); + #[cfg(feature = "dim3")] + let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); + + let effective_impulse = clamped_impulse - self.motor_impulse; + self.motor_impulse = clamped_impulse; + + mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse); + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -132,7 +237,8 @@ impl BallVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse + ball.impulse = self.impulse; + ball.motor_impulse = self.motor_impulse; } } } @@ -141,10 +247,17 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, + r2: Vector, + rhs: Vector, impulse: Vector, - r2: Vector, inv_lhs: SdpMatrix, + + motor_rhs: AngVector, + motor_impulse: AngVector, + motor_inv_lhs: Option>, + motor_max_impulse: Real, + im2: Real, ii2_sqrt: AngularInertia, } @@ -155,18 +268,18 @@ impl BallVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &BallJoint, + joint: &BallJoint, flipped: bool, ) -> Self { let (anchor1, anchor2) = if flipped { ( - rb1.position * cparams.local_anchor2 - rb1.world_com, - rb2.position * cparams.local_anchor1 - rb2.world_com, + rb1.position * joint.local_anchor2 - rb1.world_com, + rb2.position * joint.local_anchor1 - rb2.world_com, ) } else { ( - rb1.position * cparams.local_anchor1 - rb1.world_com, - rb2.position * cparams.local_anchor2 - rb2.world_com, + rb1.position * joint.local_anchor1 - rb1.world_com, + rb2.position * joint.local_anchor2 - rb2.world_com, ) }; @@ -199,14 +312,80 @@ impl BallVelocityGroundConstraint { let inv_lhs = lhs.inverse_unchecked(); + /* + * Motor part. + */ + let mut motor_rhs = na::zero(); + let mut motor_inv_lhs = None; + let motor_max_impulse = joint.motor_max_impulse; + + if motor_max_impulse > 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 { + let dpos = rb2.position.rotation + * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + #[cfg(feature = "dim2")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / ii2) + } else { + Some(gamma) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim3")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(ii2.inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; + } + } + + #[cfg(feature = "dim2")] + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; + #[cfg(feature = "dim3")] + let motor_impulse = + joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; + BallVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, rhs, inv_lhs, + motor_rhs, + motor_impulse, + motor_inv_lhs, + motor_max_impulse: joint.motor_max_impulse, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } @@ -214,13 +393,13 @@ impl BallVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let vel2 = mj_lambda2.linear + angvel.gcross(self.r2); let dvel = vel2 + self.rhs; @@ -230,6 +409,33 @@ impl BallVelocityGroundConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + } + + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { + if let Some(motor_inv_lhs) = &self.motor_inv_lhs { + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dangvel = ang_vel2 + self.motor_rhs; + let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); + + #[cfg(feature = "dim2")] + let clamped_impulse = + na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); + #[cfg(feature = "dim3")] + let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); + + let effective_impulse = clamped_impulse - self.motor_impulse; + self.motor_impulse = clamped_impulse; + + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -238,7 +444,8 @@ impl BallVelocityGroundConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse + ball.impulse = self.impulse; + ball.motor_impulse = self.motor_impulse; } } } diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs new file mode 100644 index 0000000..be12258 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -0,0 +1,346 @@ +use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{ + AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, + DIM, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use na::{Vector3, Vector6}; + +// FIXME: review this code for the case where the center of masses are not the origin. +#[derive(Debug)] +pub(crate) struct GenericPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + + joint: GenericJoint, +} + +impl GenericPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + + Self { + local_anchor1: joint.local_anchor1, + local_anchor2: joint.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1]; + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; + + /* + * + * Translation part. + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1) + + self.ii2.quadform(&rmat2).add_diagonal(self.im2)) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(r1.gcross(impulse)); + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position1.translation.vector += self.im1 * impulse; + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(impulse); + let rot2 = self.ii2.transform_vector(impulse); + + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + positions[self.position1] = position1; + positions[self.position2] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct GenericPositionGroundConstraint { + position2: usize, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + joint: GenericJoint, +} + +impl GenericPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + + if flipped { + anchor1 = rb1.predicted_position * joint.local_anchor2; + local_anchor2 = joint.local_anchor1; + } else { + anchor1 = rb1.predicted_position * joint.local_anchor1; + local_anchor2 = joint.local_anchor2; + }; + + Self { + anchor1, + local_anchor2, + position2: rb2.active_set_offset, + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; + + /* + * + * Translation part. + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + + // TODO: we should keep the SdpMatrix3 type. + let delassus = self + .ii2 + .quadform(&rmat2) + .add_diagonal(self.im2) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = self.ii2.quadform(&rotmat).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + let rot2 = self.ii2.transform_vector(impulse); + + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + positions[self.position2] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs new file mode 100644 index 0000000..9ceea67 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs @@ -0,0 +1,51 @@ +use super::{GenericPositionConstraint, GenericPositionGroundConstraint}; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{Isometry, Real, SIMD_WIDTH}; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WGenericPositionConstraint { + constraints: [GenericPositionConstraint; SIMD_WIDTH], +} + +impl WGenericPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WGenericPositionGroundConstraint { + constraints: [GenericPositionGroundConstraint; SIMD_WIDTH], +} + +impl WGenericPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs new file mode 100644 index 0000000..f391873 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -0,0 +1,706 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM}; +use crate::na::UnitQuaternion; +use crate::parry::math::{AngDim, SpatialVector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Matrix3, Matrix6, Vector3, Vector6, U3}; +#[cfg(feature = "dim2")] +use na::{Matrix3, Vector3}; + +#[derive(Debug)] +pub(crate) struct GenericVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + inv_lhs_lin: Matrix3, + inv_lhs_ang: Matrix3, + + im1: Real, + im2: Real, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, + basis1: Rotation, + basis2: Rotation, + dependant_set_mask: SpacialVector, + + vel: GenericConstraintPart, +} + +impl GenericVelocityConstraint { + pub fn compute_velocity_error( + min_velocity: &SpatialVector, + max_velocity: &SpatialVector, + r1: &Vector, + r2: &Vector, + basis1: &Rotation, + basis2: &Rotation, + rb1: &RigidBody, + rb2: &RigidBody, + ) -> SpatialVector { + 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_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); + + #[cfg(feature = "dim3")] + return Vector6::new( + lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z, + ); + } + + pub fn compute_lin_position_error( + anchor1: &Isometry, + anchor2: &Isometry, + basis: &Rotation, + min_position: &Vector, + max_position: &Vector, + ) -> Vector { + let dpos = anchor2.translation.vector - anchor1.translation.vector; + let local_dpos = basis.inverse_transform_vector(&dpos); + local_dpos - local_dpos.sup(min_position).inf(max_position) + } + + pub fn compute_ang_position_error( + anchor1: &Isometry, + anchor2: &Isometry, + basis: &Rotation, + min_position: &Vector, + max_position: &Vector, + ) -> Vector { + let drot = anchor2.rotation * anchor1.rotation.inverse(); + let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis()); + + let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position); + let error_code = + (error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize; + + if error_code == 1 { + // Align two axes. + let constrained_axis = error.iamin(); + let axis1 = anchor1 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let axis2 = anchor2 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) + .unwrap_or(UnitQuaternion::identity()); + let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis()); + local_drot_diff - local_drot_diff.sup(min_position).inf(max_position) + } else { + error + } + } + + 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, + anchor2: &Isometry, + basis: &Rotation, + ) -> SpatialVector { + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector); + let ang_dpos = basis.inverse_transform_vector(&delta_pos.rotation.scaled_axis()); + + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); + let error_code = + (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize; + + match error_code { + 1 => { + let constrained_axis = error.rows(3, 3).iamin(); + let axis1 = anchor1 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let axis2 = anchor2 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) + .unwrap_or(UnitQuaternion::identity()); + let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis()); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + dpos - dpos.sup(&joint.min_position).inf(&joint.max_position) + } + _ => error, + } + } + + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + ) -> Self { + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; + 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(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + 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, &basis1) + * params.inv_dt() + * params.joint_erp; + + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } + + 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 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(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::(DIM).into_owned(), + &max_pos_impulse.fixed_rows::(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&max_impulse) + .sup(&min_impulse); + + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::(DIM).into_owned(); + + GenericVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + inv_lhs_lin, + inv_lhs_ang, + r1, + r2, + basis1, + basis2, + dependant_set_mask, + vel: GenericConstraintPart { + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, + }, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + 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_impulse1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); + + mj_lambda2.linear -= self.im2 * lin_impulse2; + mj_lambda2.angular -= self + .ii2_sqrt + .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; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + match &mut joint.params { + JointParams::GenericJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.impulse[3] = self.vel.ang_impulse.x; + out.impulse[4] = self.vel.ang_impulse.y; + out.impulse[5] = self.vel.ang_impulse.z; + } + JointParams::RevoluteJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.motor_impulse = self.vel.ang_impulse.x; + out.impulse[3] = self.vel.ang_impulse.y; + out.impulse[4] = self.vel.ang_impulse.z; + } + _ => unimplemented!(), + } + } +} + +#[derive(Debug)] +pub(crate) struct GenericVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + inv_lhs_lin: Matrix3, + inv_lhs_ang: Matrix3, + + im2: Real, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, + basis: Rotation, + + dependant_set_mask: SpacialVector, + + vel: GenericConstraintPart, +} + +impl GenericVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * joint.local_anchor2, + rb2.position * joint.local_anchor1, + ) + } else { + ( + rb1.position * joint.local_anchor1, + rb2.position * joint.local_anchor2, + ) + }; + + let basis = anchor2.rotation; + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + let mut max_velocity = joint.max_velocity; + let mut dependant_set_mask = SpacialVector::repeat(1.0); + + let pos_rhs = + GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis) + * params.inv_dt() + * params.joint_erp; + + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } + + let rhs = GenericVelocityConstraint::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &basis, + &basis, + rb1, + rb2, + ); + let rhs_lin = rhs.xyz(); + let rhs_ang = rhs.fixed_rows::(DIM).into_owned(); + + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix(); + let delassus11 = ii2.quadform(&rotmat).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::(DIM).into_owned(), + &max_pos_impulse.fixed_rows::(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&max_impulse) + .sup(&min_impulse); + + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::(DIM).into_owned(); + + GenericVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + inv_lhs_lin, + inv_lhs_ang, + r2, + basis, + vel: GenericConstraintPart { + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, + }, + dependant_set_mask, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + 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; + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // TODO: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + match &mut joint.params { + JointParams::GenericJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.impulse[3] = self.vel.ang_impulse.x; + out.impulse[4] = self.vel.ang_impulse.y; + out.impulse[5] = self.vel.ang_impulse.z; + } + JointParams::RevoluteJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.motor_impulse = self.vel.ang_impulse.x; + out.impulse[3] = self.vel.ang_impulse.y; + out.impulse[4] = self.vel.ang_impulse.z; + } + _ => unimplemented!(), + } + } +} + +#[derive(Debug)] +struct GenericConstraintPart { + lin_impulse: Vector3, + max_lin_impulse: Vector3, + min_lin_impulse: Vector3, + rhs_lin: Vector3, + + ang_impulse: Vector3, + max_ang_impulse: Vector3, + min_ang_impulse: Vector3, + rhs_ang: Vector3, +} + +impl GenericConstraintPart { + fn solve( + &self, + parent: &GenericVelocityConstraint, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) -> (Vector3, Vector3) { + let new_lin_impulse; + let new_ang_impulse; + + /* + * + * Solve translations. + * + */ + { + 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 + .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_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_impulse1; + mj_lambda1.angular += parent + .ii1_sqrt + .transform_vector(parent.r1.gcross(effective_impulse1)); + + mj_lambda2.linear -= parent.im2 * effective_impulse2; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse2)); + } + + /* + * + * Solve rotations. + * + */ + { + 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.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_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_impulse1); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2); + } + + (new_lin_impulse, new_ang_impulse) + } + + fn solve_ground( + &self, + parent: &GenericVelocityGroundConstraint, + mj_lambda2: &mut DeltaVel, + ) -> (Vector3, Vector3) { + let new_lin_impulse; + let new_ang_impulse; + + /* + * + * Solve translations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = parent + .basis + .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); + + mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse)); + } + + /* + * + * Solve rotations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = parent.basis.inverse_transform_vector(&ang_vel2); + 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); + + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); + } + + (new_lin_impulse, new_ang_impulse) + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs new file mode 100644 index 0000000..8a6be8c --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs @@ -0,0 +1,472 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix6, Vector6, U3}; +#[cfg(feature = "dim2")] +use { + na::{Matrix3, Vector3}, + parry::utils::SdpMatrix3, +}; + +#[derive(Debug)] +pub(crate) struct WGenericVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: SimdReal, + im2: SimdReal, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl WGenericVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat1: CrossMatrix<_> = r1.gcross_matrix(); + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + 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); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2); + let ang_dvel = -angvel1 + angvel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WGenericVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + ii1, + ii2, + ii1_sqrt, + ii2_sqrt, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WGenericVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: SimdReal, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl WGenericVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + let local_anchor2 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + lhs = Matrix6::zeros(); + 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); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let ang_dvel = angvel2 - angvel1; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WGenericVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + ii2, + ii2_sqrt, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index ed6d758..ed27e51 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -13,6 +13,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::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, @@ -34,6 +37,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), PrismaticConstraint(PrismaticVelocityConstraint), PrismaticGroundConstraint(PrismaticVelocityGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -79,6 +88,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), + // ), #[cfg(feature = "dim3")] JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint( RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), @@ -109,6 +121,12 @@ impl AnyJointVelocityConstraint { params, joint_id, rbs1, rbs2, joints, )) } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointVelocityConstraint::WGenericConstraint( + // WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -148,6 +166,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::PrismaticJoint(p) => { AnyJointVelocityConstraint::PrismaticGroundConstraint( PrismaticVelocityGroundConstraint::from_params( @@ -156,10 +179,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, ), } } @@ -199,6 +220,14 @@ impl AnyJointVelocityConstraint { ), ) } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointVelocityConstraint::WGenericGroundConstraint( + // WGenericVelocityGroundConstraint::from_params( + // params, joint_id, rbs1, rbs2, joints, flipped, + // ), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -235,6 +264,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::PrismaticConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -269,6 +304,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::PrismaticConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -311,6 +352,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::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 97a81ba..635e0b1 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -31,6 +31,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), PrismaticJoint(PrismaticPositionConstraint), PrismaticGroundConstraint(PrismaticPositionGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -61,6 +67,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::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint( PrismaticPositionConstraint::from_params(rb1, rb2, p), ), @@ -89,6 +98,12 @@ impl AnyJointPositionConstraint { rbs1, rbs2, joints, )) } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params( + // rbs1, rbs2, joints, + // )) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -123,6 +138,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::PrismaticJoint(p) => { AnyJointPositionConstraint::PrismaticGroundConstraint( PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped), @@ -161,6 +179,12 @@ impl AnyJointPositionConstraint { WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointPositionConstraint::WGenericGroundConstraint( + // WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -193,6 +217,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::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 154ff83..9196e69 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -18,6 +18,21 @@ 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(crate) use joint_constraint::AnyJointVelocityConstraint; pub(crate) use joint_position_constraint::AnyJointPositionConstraint; pub(self) use prismatic_position_constraint::{ @@ -63,6 +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 joint_constraint; mod joint_position_constraint; mod prismatic_position_constraint; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index a361a37..ae3f3e2 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Vector}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] @@ -41,9 +41,17 @@ pub(crate) struct PrismaticVelocityConstraint { #[cfg(feature = "dim2")] impulse: Vector2, + motor_axis1: Vector, + motor_axis2: Vector, + motor_impulse: Real, + motor_rhs: Real, + motor_inv_lhs: Real, + motor_max_impulse: Real, + limits_impulse: Real, limits_forcedirs: Option<(Vector, Vector)>, limits_rhs: Real, + limits_inv_lhs: Real, #[cfg(feature = "dim2")] basis1: Vector2, @@ -63,35 +71,21 @@ impl PrismaticVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &PrismaticJoint, + joint: &PrismaticJoint, ) -> Self { // Linear part. - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; - let axis1 = rb1.position * cparams.local_axis1; - let axis2 = rb2.position * cparams.local_axis2; + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; + let axis1 = rb1.position * joint.local_axis1; + let axis2 = rb2.position * joint.local_axis2; #[cfg(feature = "dim2")] - let basis1 = rb1.position * cparams.basis1[0]; + let basis1 = rb1.position * joint.basis1[0]; #[cfg(feature = "dim3")] let basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); - // #[cfg(feature = "dim2")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .to_rotation_matrix() - // .into_inner(); - // #[cfg(feature = "dim3")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: we use basis2 := basis1 for now is that allows - // simplifications of the computation without introducing - // much instabilities. - let im1 = rb1.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; @@ -149,25 +143,71 @@ impl PrismaticVelocityConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup motor. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 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 { + let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1); + motor_rhs += (dist - joint.motor_target_pos) * stiffness; + } + + if damping != 0.0 { + let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma }; + motor_rhs /= gamma; + } + + let motor_impulse = na::clamp( + joint.motor_impulse, + -joint.motor_max_impulse, + joint.motor_max_impulse, + ); + + /* + * Setup limit constraint. + */ let mut limits_forcedirs = None; let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; + let mut limits_inv_lhs = 0.0; - if cparams.limits_enabled { + if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at + // TODO: we should allow both limits to be active at // the same time, and allow predictive constraint activation. - if dist < cparams.limits[0] { + if dist < joint.limits[0] { limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner())); limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; - } else if dist > cparams.limits[1] { + limits_impulse = joint.limits_impulse; + } else if dist > joint.limits[1] { limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner())); limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; + limits_impulse = joint.limits_impulse; + } + + if dist < joint.limits[0] || dist > joint.limits[1] { + let gcross1 = r1.gcross(*axis1); + let gcross2 = r2.gcross(*axis2); + limits_inv_lhs = crate::utils::inv( + im1 + im2 + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2)), + ); } } @@ -179,10 +219,17 @@ impl PrismaticVelocityConstraint { ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, limits_forcedirs, limits_rhs, + limits_inv_lhs, + motor_rhs, + motor_inv_lhs, + motor_impulse, + motor_axis1: *axis1, + motor_axis2: *axis2, + motor_max_impulse: joint.motor_max_impulse, basis1, inv_lhs, rhs, @@ -211,22 +258,30 @@ impl PrismaticVelocityConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart motors. + mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse); + mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse); + + // Warmstart limits. if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + let limit_impulse1 = limits_forcedir1 * self.limits_impulse; + let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + + mj_lambda1.linear += self.im1 * limit_impulse1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(limit_impulse1)); + mj_lambda2.linear += self.im2 * limit_impulse2; + mj_lambda2.angular += self + .ii2_sqrt + .transform_vector(self.r2.gcross(limit_impulse2)); } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - /* - * Joint consraint. - */ + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -255,26 +310,55 @@ impl PrismaticVelocityConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0); + let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; - mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + let lin_impulse1 = limits_forcedir1 * dimpulse; + let lin_impulse2 = limits_forcedir2 * dimpulse; + + mj_lambda1.linear += self.im1 * lin_impulse1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + mj_lambda2.linear += self.im2 * lin_impulse2; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); } + } + + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { + if self.motor_inv_lhs != 0.0 { + let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear) + - self.motor_axis1.dot(&mj_lambda1.linear) + + self.motor_rhs; + let new_impulse = na::clamp( + self.motor_impulse + lin_dvel * self.motor_inv_lhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let dimpulse = new_impulse - self.motor_impulse; + self.motor_impulse = new_impulse; + + mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse); + mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -284,6 +368,7 @@ impl PrismaticVelocityConstraint { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::PrismaticJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + revolute.motor_impulse = self.motor_impulse; revolute.limits_impulse = self.limits_impulse; } } @@ -315,6 +400,11 @@ pub(crate) struct PrismaticVelocityGroundConstraint { limits_rhs: Real, axis2: Vector, + motor_impulse: Real, + motor_rhs: Real, + motor_inv_lhs: Real, + motor_max_impulse: Real, + #[cfg(feature = "dim2")] basis1: Vector2, #[cfg(feature = "dim3")] @@ -331,7 +421,7 @@ impl PrismaticVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &PrismaticJoint, + joint: &PrismaticJoint, flipped: bool, ) -> Self { let anchor2; @@ -341,35 +431,35 @@ impl PrismaticVelocityGroundConstraint { let basis1; if flipped { - anchor2 = rb2.position * cparams.local_anchor1; - anchor1 = rb1.position * cparams.local_anchor2; - axis2 = rb2.position * cparams.local_axis1; - axis1 = rb1.position * cparams.local_axis2; + anchor2 = rb2.position * joint.local_anchor1; + anchor1 = rb1.position * joint.local_anchor2; + axis2 = rb2.position * joint.local_axis1; + axis1 = rb1.position * joint.local_axis2; #[cfg(feature = "dim2")] { - basis1 = rb1.position * cparams.basis2[0]; + basis1 = rb1.position * joint.basis2[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis2[0], - rb1.position * cparams.basis2[1], + rb1.position * joint.basis2[0], + rb1.position * joint.basis2[1], ]); } } else { - anchor2 = rb2.position * cparams.local_anchor2; - anchor1 = rb1.position * cparams.local_anchor1; - axis2 = rb2.position * cparams.local_axis2; - axis1 = rb1.position * cparams.local_axis1; + anchor2 = rb2.position * joint.local_anchor2; + anchor1 = rb1.position * joint.local_anchor1; + axis2 = rb2.position * joint.local_axis2; + axis1 = rb1.position * joint.local_axis1; #[cfg(feature = "dim2")] { - basis1 = rb1.position * cparams.basis1[0]; + basis1 = rb1.position * joint.basis1[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); } }; @@ -438,26 +528,61 @@ impl PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup motor. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 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 { + let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1); + motor_rhs += (dist - joint.motor_target_pos) * stiffness; + } + + if damping != 0.0 { + let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma }; + motor_rhs /= gamma; + } + + let motor_impulse = na::clamp( + joint.motor_impulse, + -joint.motor_max_impulse, + joint.motor_max_impulse, + ); + + /* + * Setup limit constraint. + */ let mut limits_forcedir2 = None; let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; - if cparams.limits_enabled { + if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at + // TODO: we should allow both limits to be active at // the same time. - // FIXME: allow predictive constraint activation. - if dist < cparams.limits[0] { + // TODO: allow predictive constraint activation. + if dist < joint.limits[0] { limits_forcedir2 = Some(axis2.into_inner()); limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; - } else if dist > cparams.limits[1] { + limits_impulse = joint.limits_impulse; + } else if dist > joint.limits[1] { limits_forcedir2 = Some(-axis2.into_inner()); limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; + limits_impulse = joint.limits_impulse; } } @@ -466,8 +591,12 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2: rb2.active_set_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, + motor_rhs, + motor_inv_lhs, + motor_impulse, + motor_max_impulse: joint.motor_max_impulse, basis1, inv_lhs, rhs, @@ -492,6 +621,10 @@ impl PrismaticVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart motors. + mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse); + + // Warmstart limits. if let Some(limits_forcedir2) = self.limits_forcedir2 { mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); } @@ -499,12 +632,7 @@ impl PrismaticVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - /* - * Joint consraint. - */ + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); let lin_dvel = self.basis1.tr_mul(&lin_vel2); @@ -526,13 +654,10 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { if let Some(limits_forcedir2) = self.limits_forcedir2 { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) @@ -543,15 +668,39 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); } + } + + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { + if self.motor_inv_lhs != 0.0 { + let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs; + let new_impulse = na::clamp( + self.motor_impulse + lin_dvel * self.motor_inv_lhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let dimpulse = new_impulse - self.motor_impulse; + self.motor_impulse = new_impulse; + + mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_limits(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); + self.solve_dofs(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - // FIXME: duplicated code with the non-ground constraint. + // TODO: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::PrismaticJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + revolute.motor_impulse = self.motor_impulse; revolute.limits_impulse = self.limits_impulse; } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 77a6fe7..442393d 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -7,7 +7,7 @@ use crate::dynamics::{ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] @@ -48,6 +48,7 @@ pub(crate) struct WPrismaticVelocityConstraint { limits_impulse: SimdReal, limits_forcedirs: Option<(Vector, Vector)>, limits_rhs: SimdReal, + limits_inv_lhs: SimdReal, #[cfg(feature = "dim2")] basis1: Vector2, @@ -187,10 +188,13 @@ impl WPrismaticVelocityConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup limit constraint. + */ let mut limits_forcedirs = None; let mut limits_rhs = na::zero(); let mut limits_impulse = na::zero(); + let mut limits_inv_lhs = na::zero(); let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); if limits_enabled.any() { @@ -210,9 +214,17 @@ impl WPrismaticVelocityConstraint { let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); if sign != _0 { + let gcross1 = r1.gcross(axis1); + let gcross2 = r2.gcross(axis2); + limits_forcedirs = Some((axis1 * -sign, axis2 * sign)); limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign; limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0); + limits_inv_lhs = SimdReal::splat(1.0) + / (im1 + + im2 + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2))); } } @@ -228,6 +240,7 @@ impl WPrismaticVelocityConstraint { limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), limits_forcedirs, limits_rhs, + limits_inv_lhs, basis1, inv_lhs, rhs, @@ -270,9 +283,19 @@ impl WPrismaticVelocityConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart limits. if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + let limit_impulse1 = limits_forcedir1 * self.limits_impulse; + let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + + mj_lambda1.linear += limit_impulse1 * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(limit_impulse1)); + mj_lambda2.linear += limit_impulse2 * self.im2; + mj_lambda2.angular += self + .ii2_sqrt + .transform_vector(self.r2.gcross(limit_impulse2)); } for ii in 0..SIMD_WIDTH { @@ -285,27 +308,11 @@ impl WPrismaticVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), - }; - let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), - }; - - /* - * Joint consraint. - */ + fn solve_dofs( + &mut self, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -334,13 +341,14 @@ impl WPrismaticVelocityConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits( + &mut self, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) { if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -348,13 +356,40 @@ impl WPrismaticVelocityConstraint { + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; let new_impulse = - (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero()); + (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero()); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; - mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + let lin_impulse1 = limits_forcedir1 * dimpulse; + let lin_impulse2 = limits_forcedir2 * dimpulse; + + mj_lambda1.linear += lin_impulse1 * self.im1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + mj_lambda2.linear += lin_impulse2 * self.im2; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -477,19 +512,6 @@ impl WPrismaticVelocityGroundConstraint { let axis1 = position1 * local_axis1; let axis2 = position2 * local_axis2; - // #[cfg(feature = "dim2")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .to_rotation_matrix() - // .into_inner(); - // #[cfg(feature = "dim3")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: we use basis2 := basis1 for now is that allows - // simplifications of the computation without introducing - // much instabilities. let ii2 = ii2_sqrt.squared(); let r1 = anchor1 - world_com1; let r2 = anchor2 - world_com2; @@ -616,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), - }; - - /* - * Joint consraint. - */ + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); let lin_dvel = self.basis1.tr_mul(&lin_vel2); @@ -650,10 +660,9 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { if let Some(limits_forcedir2) = self.limits_forcedir2 { // FIXME: the transformation by ii2_sqrt could be avoided by // reusing some computations above. @@ -667,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + self.solve_dofs(&mut mj_lambda2); + self.solve_limits(&mut mj_lambda2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); @@ -674,7 +697,6 @@ impl WPrismaticVelocityGroundConstraint { } } - // FIXME: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { for ii in 0..SIMD_WIDTH { let joint = &mut joints_all[self.joint_id[ii]].weight; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index afc23f3..2da49e6 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,6 +1,6 @@ use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; -use crate::utils::WAngularInertia; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::Unit; #[derive(Debug)] @@ -8,13 +8,15 @@ pub(crate) struct RevolutePositionConstraint { position1: usize, position2: usize, + local_com1: Point, + local_com2: Point, + im1: Real, im2: Real, ii1: AngularInertia, ii2: AngularInertia, - lin_inv_lhs: Real, ang_inv_lhs: AngularInertia, local_anchor1: Point, @@ -22,6 +24,8 @@ pub(crate) struct RevolutePositionConstraint { local_axis1: Unit>, local_axis2: Unit>, + local_basis1: [Vector; 2], + local_basis2: [Vector; 2], } impl RevolutePositionConstraint { @@ -30,7 +34,6 @@ impl RevolutePositionConstraint { let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; - let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); Self { @@ -38,14 +41,17 @@ impl RevolutePositionConstraint { im2, ii1, ii2, - lin_inv_lhs, ang_inv_lhs, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, position1: rb1.active_set_offset, position2: rb2.active_set_offset, + local_basis1: cparams.basis1, + local_basis2: cparams.basis2, } } @@ -53,27 +59,56 @@ impl RevolutePositionConstraint { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; - let axis1 = position1 * self.local_axis1; - let axis2 = position2 * self.local_axis2; - let delta_rot = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_error = delta_rot.scaled_axis() * params.joint_erp; - let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error); + /* + * Linear part. + */ + { + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; - position1.rotation = - Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; - position2.rotation = - Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + let r1 = anchor1 - position1 * self.local_com1; + let r2 = anchor2 - position2 * self.local_com2; - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; + // TODO: don't do the "to_matrix". + let lhs = (self + .ii2 + .quadform(&r2.gcross_matrix()) + .add_diagonal(self.im2) + + self + .ii1 + .quadform(&r1.gcross_matrix()) + .add_diagonal(self.im1)) + .into_matrix(); + let inv_lhs = lhs.try_inverse().unwrap(); - let delta_tra = anchor2 - anchor1; - let lin_error = delta_tra * params.joint_erp; - let lin_impulse = self.lin_inv_lhs * lin_error; + let delta_tra = anchor2 - anchor1; + let lin_error = delta_tra * params.joint_erp; + let lin_impulse = inv_lhs * lin_error; - position1.translation.vector += self.im1 * lin_impulse; - position2.translation.vector -= self.im2 * lin_impulse; + let rot1 = self.ii1 * r1.gcross(lin_impulse); + let rot2 = self.ii2 * r2.gcross(lin_impulse); + 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; + } + + /* + * Angular part. + */ + { + let axis1 = position1 * self.local_axis1; + let axis2 = position2 * self.local_axis2; + let delta_rot = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_error = delta_rot.scaled_axis() * params.joint_erp; + let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error); + + position1.rotation = + Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; + position2.rotation = + Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + } positions[self.position1 as usize] = position1; positions[self.position2 as usize] = position2; @@ -83,10 +118,16 @@ impl RevolutePositionConstraint { #[derive(Debug)] pub(crate) struct RevolutePositionGroundConstraint { position2: usize, + local_com2: Point, + im2: Real, + ii2: AngularInertia, anchor1: Point, local_anchor2: Point, axis1: Unit>, local_axis2: Unit>, + + basis1: [Vector; 2], + local_basis2: [Vector; 2], } impl RevolutePositionGroundConstraint { @@ -100,42 +141,82 @@ impl RevolutePositionGroundConstraint { let local_anchor2; let axis1; let local_axis2; + let basis1; + let local_basis2; if flipped { anchor1 = rb1.predicted_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; axis1 = rb1.predicted_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; + basis1 = [ + rb1.predicted_position * cparams.basis2[0], + rb1.predicted_position * cparams.basis2[1], + ]; + local_basis2 = cparams.basis1; } else { anchor1 = rb1.predicted_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; axis1 = rb1.predicted_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; + basis1 = [ + rb1.predicted_position * cparams.basis1[0], + rb1.predicted_position * cparams.basis1[1], + ]; + local_basis2 = cparams.basis2; }; Self { anchor1, local_anchor2, + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, axis1, local_axis2, position2: rb2.active_set_offset, + basis1, + local_basis2, } } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; - let axis2 = position2 * self.local_axis2; + /* + * Linear part. + */ + { + let anchor2 = position2 * self.local_anchor2; - let delta_rot = - Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp) + let r2 = anchor2 - position2 * self.local_com2; + // TODO: don't the the "to_matrix". + let lhs = self + .ii2 + .quadform(&r2.gcross_matrix()) + .add_diagonal(self.im2) + .into_matrix(); + let inv_lhs = lhs.try_inverse().unwrap(); + + let delta_tra = anchor2 - self.anchor1; + let lin_error = delta_tra * params.joint_erp; + let lin_impulse = inv_lhs * lin_error; + + let rot2 = self.ii2 * r2.gcross(lin_impulse); + position2.rotation = Rotation::new(-rot2) * position2.rotation; + position2.translation.vector -= self.im2 * lin_impulse; + } + + /* + * Angular part. + */ + { + let axis2 = position2 * self.local_axis2; + let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2) .unwrap_or_else(Rotation::identity); - position2.rotation = delta_rot * position2.rotation; - - let anchor2 = position2 * self.local_anchor2; - let delta_tra = anchor2 - self.anchor1; - let lin_error = delta_tra * params.joint_erp; - position2.translation.vector -= lin_error; + 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 6270a8e..61cb720 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -1,8 +1,9 @@ -use crate::dynamics::solver::DeltaVel; +use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel}; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngularInertia, Real, Vector}; +use crate::math::{AngularInertia, Real, Rotation, Vector}; +use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -20,7 +21,17 @@ pub(crate) struct RevoluteVelocityConstraint { rhs: Vector5, impulse: Vector5, + motor_inv_lhs: Real, + 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, + basis1: Matrix3x2, + basis2: Matrix3x2, im1: Real, im2: Real, @@ -35,23 +46,23 @@ impl RevoluteVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &RevoluteJoint, + joint: &RevoluteJoint, ) -> Self { // Linear part. - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; let basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: to simplify, we use basis2 = basis1. - // Though we may want to test if that does not introduce any instability. + let basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis2[0], + rb2.position * joint.basis2[1], + ]); + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; + let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; @@ -64,12 +75,13 @@ impl RevoluteVelocityConstraint { let r2_mat = r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); - let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat)); + let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix(); - // Note that cholesky won't read the upper-right part + // 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()); @@ -78,10 +90,64 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); + let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + /* + * Motor. + */ + 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_angle = 0.0; + let motor_max_impulse = joint.motor_max_impulse; + + 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; + } + + /* + * Adjust the warmstart impulse. + * If the velocity along the free axis is somewhat high, + * we need to adjust the angular warmstart impulse because it + * may have a direction that is too different than last frame, + * making it counter-productive. + */ + let mut impulse = joint.impulse * params.warmstart_coeff; + let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1) + .unwrap_or_else(UnitQuaternion::identity); + 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; + RevoluteVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, @@ -89,13 +155,21 @@ impl RevoluteVelocityConstraint { im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, basis1, + basis2, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse, inv_lhs, rhs, r1, r2, + motor_rhs, + motor_inv_lhs, + motor_max_impulse, + motor_axis1, + motor_axis2, + motor_impulse, + motor_angle, } } @@ -103,49 +177,90 @@ impl RevoluteVelocityConstraint { 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.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); - 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)); + + /* + * Motor + */ + if self.motor_inv_lhs != 0.0 { + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.motor_axis1 * self.motor_impulse); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.motor_axis2 * self.motor_impulse); + } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2)) + - (mj_lambda1.linear + ang_vel1.gcross(self.r1)); + let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse1 = impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); + + mj_lambda2.linear -= self.im2 * lin_impulse2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); + } + + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { + if self.motor_inv_lhs != 0.0 { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + 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 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); + } + } + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) - - mj_lambda1.linear - - ang_vel1.gcross(self.r1); - let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); - let rhs = - Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; - let impulse = self.inv_lhs * rhs; - self.impulse += impulse; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); - - mj_lambda1.linear += self.im1 * lin_impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -155,6 +270,11 @@ impl RevoluteVelocityConstraint { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::RevoluteJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + 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; } } } @@ -171,7 +291,14 @@ pub(crate) struct RevoluteVelocityGroundConstraint { rhs: Vector5, impulse: Vector5, - basis1: Matrix3x2, + motor_axis2: Vector, + motor_inv_lhs: Real, + motor_rhs: Real, + motor_impulse: Real, + motor_max_impulse: Real, + motor_angle: Real, // Exists just for writing it into the joint. + + basis2: Matrix3x2, im2: Real, @@ -184,34 +311,46 @@ impl RevoluteVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &RevoluteJoint, + joint: &RevoluteJoint, flipped: bool, - ) -> Self { + ) -> AnyJointVelocityConstraint { let anchor2; let anchor1; + let axis1; + let axis2; let basis1; + let basis2; if flipped { - anchor1 = rb1.position * cparams.local_anchor2; - anchor2 = rb2.position * cparams.local_anchor1; + axis1 = rb1.position * *joint.local_axis2; + axis2 = rb2.position * *joint.local_axis1; + anchor1 = rb1.position * joint.local_anchor2; + anchor2 = rb2.position * joint.local_anchor1; basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis2[0], - rb1.position * cparams.basis2[1], + rb1.position * joint.basis2[0], + rb1.position * joint.basis2[1], + ]); + basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis1[0], + rb2.position * joint.basis1[1], ]); } else { - anchor1 = rb1.position * cparams.local_anchor1; - anchor2 = rb2.position * cparams.local_anchor2; + axis1 = rb1.position * *joint.local_axis1; + axis2 = rb2.position * *joint.local_axis2; + anchor1 = rb1.position * joint.local_anchor1; + anchor2 = rb2.position * joint.local_anchor2; basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], + ]); + basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis2[0], + rb2.position * joint.basis2[1], ]); }; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = /*r21 * */ basis1; + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; @@ -220,8 +359,8 @@ impl RevoluteVelocityGroundConstraint { let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); - let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)); + let lhs11 = 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. @@ -232,54 +371,130 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); + let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); - RevoluteVelocityGroundConstraint { + /* + * Motor part. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 0.0; + let mut motor_angle = 0.0; + let motor_max_impulse = joint.motor_max_impulse; + + 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(&axis2) - rb1.angvel.dot(&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(axis2.dot(&ii2.transform_vector(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, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, - basis1, + impulse: joint.impulse * params.warmstart_coeff, + basis2, inv_lhs, rhs, r2, - } + motor_inv_lhs, + motor_impulse, + motor_axis2: axis2, + motor_max_impulse, + motor_rhs, + motor_angle, + }; + + AnyJointVelocityConstraint::RevoluteGroundConstraint(result) } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + /* + * Motor + */ + if self.motor_inv_lhs != 0.0 { + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.motor_axis2 * self.motor_impulse); + } + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let ang_dvel = self.basis2.tr_mul(&ang_vel2); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { + if self.motor_inv_lhs != 0.0 { + 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 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); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -289,6 +504,8 @@ impl RevoluteVelocityGroundConstraint { let joint = &mut joints_all[self.joint_id].weight; 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; } } } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 047763d..7750d98 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -4,7 +4,9 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, +}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; @@ -22,7 +24,9 @@ pub(crate) struct WRevoluteVelocityConstraint { rhs: Vector5, impulse: Vector5, + axis1: [Vector; SIMD_WIDTH], basis1: Matrix3x2, + basis2: Matrix3x2, im1: SimdReal, im2: SimdReal, @@ -37,7 +41,7 @@ impl WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], rbs1: [&RigidBody; SIMD_WIDTH], rbs2: [&RigidBody; SIMD_WIDTH], - cparams: [&RevoluteJoint; SIMD_WIDTH], + joints: [&RevoluteJoint; SIMD_WIDTH], ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); @@ -59,26 +63,27 @@ impl WRevoluteVelocityConstraint { ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]); let local_basis1 = [ - Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]), - Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]), ]; - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let local_basis2 = [ + Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]), + ]; + let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; let basis1 = Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]); + let basis2 = + Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]); + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: to simplify, we use basis2 = basis1. - // Though we may want to test if that does not introduce any instability. let ii1 = ii1_sqrt.squared(); let r1 = anchor1 - world_com1; let r1_mat = r1.gcross_matrix(); @@ -90,10 +95,10 @@ impl WRevoluteVelocityConstraint { let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); - let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat)); + let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix(); - // Note that cholesky won't read the upper-right part + // 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()); @@ -103,19 +108,42 @@ impl WRevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + /* + * Adjust the warmstart impulse. + * If the velocity along the free axis is somewhat high, + * we need to adjust the angular warmstart impulse because it + * may have a direction that is too different than last frame, + * making it counter-productive. + */ + let warmstart_coeff = SimdReal::splat(params.warmstart_coeff); + let mut impulse = impulse * warmstart_coeff; + + let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH]; + let rotated_impulse = Vector::from(array![|ii| { + let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii]) + .unwrap_or_else(Rotation::identity); + axis_rot * joints[ii].world_ang_impulse + }; SIMD_WIDTH]); + + let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse); + impulse[3] = rotated_basis_impulse.x * warmstart_coeff; + impulse[4] = rotated_basis_impulse.y * warmstart_coeff; + WRevoluteVelocityConstraint { joint_id, mj_lambda1, mj_lambda2, im1, ii1_sqrt, + axis1, basis1, + basis2, im2, ii2_sqrt, - impulse: impulse * SimdReal::splat(params.warmstart_coeff), + impulse, inv_lhs, rhs, r1, @@ -141,18 +169,20 @@ impl WRevoluteVelocityConstraint { ), }; - let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.linear += lin_impulse1 * self.im1; 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 -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -184,26 +214,28 @@ impl WRevoluteVelocityConstraint { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) - - mj_lambda1.linear - - ang_vel1.gcross(self.r1); - let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); + + let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2)) + - (mj_lambda1.linear + ang_vel1.gcross(self.r1)); + let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.linear += lin_impulse1 * self.im1; 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 -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -216,10 +248,15 @@ impl WRevoluteVelocityConstraint { } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let rot_part = self.impulse.fixed_rows::(3).into_owned(); + let world_ang_impulse = self.basis1 * rot_part; + for ii in 0..SIMD_WIDTH { let joint = &mut joints_all[self.joint_id[ii]].weight; if let JointParams::RevoluteJoint(rev) = &mut joint.params { - rev.impulse = self.impulse.extract(ii) + rev.impulse = self.impulse.extract(ii); + rev.world_ang_impulse = world_ang_impulse.extract(ii); + rev.prev_axis1 = self.axis1[ii]; } } } @@ -237,7 +274,7 @@ pub(crate) struct WRevoluteVelocityGroundConstraint { rhs: Vector5, impulse: Vector5, - basis1: Matrix3x2, + basis2: Matrix3x2, im2: SimdReal, @@ -250,7 +287,7 @@ impl WRevoluteVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], rbs1: [&RigidBody; SIMD_WIDTH], rbs2: [&RigidBody; SIMD_WIDTH], - cparams: [&RevoluteJoint; SIMD_WIDTH], + joints: [&RevoluteJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); @@ -267,33 +304,40 @@ impl WRevoluteVelocityGroundConstraint { array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH], ); let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH], ); let basis1 = Matrix3x2::from_columns(&[ position1 * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH], ), position1 * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH], ), ]); + let basis2 = Matrix3x2::from_columns(&[ + position2 + * Vector::from( + array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH], + ), + position2 + * Vector::from( + array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH], + ), + ]); + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = /*r21 * */ basis1; let ii2 = ii2_sqrt.squared(); let r1 = anchor1 - world_com1; let r2 = anchor2 - world_com2; @@ -301,8 +345,8 @@ impl WRevoluteVelocityGroundConstraint { let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); - let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)); + let lhs11 = 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. @@ -313,8 +357,8 @@ impl WRevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1)); + let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); WRevoluteVelocityGroundConstraint { @@ -323,7 +367,7 @@ impl WRevoluteVelocityGroundConstraint { im2, ii2_sqrt, impulse: impulse * SimdReal::splat(params.warmstart_coeff), - basis1, + basis2, inv_lhs, rhs, r2, @@ -341,7 +385,7 @@ impl WRevoluteVelocityGroundConstraint { }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= lin_impulse * self.im2; mj_lambda2.angular -= self @@ -366,13 +410,13 @@ impl WRevoluteVelocityGroundConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let ang_dvel = self.basis2.tr_mul(&ang_vel2); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= lin_impulse * self.im2; mj_lambda2.angular -= self diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index 8b88e28..698e255 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -121,7 +121,11 @@ impl NPhysicsWorld { } nphysics_joints.insert(c); - } + } // JointParams::GenericJoint(_) => { + // eprintln!( + // "Joint type currently unsupported by the nphysics backend: GenericJoint." + // ) + // } } } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 319736c..f2a4f8a 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -344,13 +344,27 @@ impl PhysxWorld { .into_physx() .into(); - physx_sys::phys_PxRevoluteJointCreate( + let revolute_joint = physx_sys::phys_PxRevoluteJointCreate( physics.as_mut_ptr(), actor1, &frame1 as *const _, actor2, &frame2 as *const _, ); + + physx_sys::PxRevoluteJoint_setDriveVelocity_mut( + revolute_joint, + params.motor_target_vel, + true, + ); + + if params.motor_damping != 0.0 { + physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut( + revolute_joint, + physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED, + true, + ); + } } JointParams::PrismaticJoint(params) => { @@ -420,7 +434,11 @@ impl PhysxWorld { actor2, &frame2 as *const _, ); - } + } // JointParams::GenericJoint(_) => { + // eprintln!( + // "Joint type currently unsupported by the PhysX backend: GenericJoint." + // ) + // } } } }