From 815de4beff2ca31255c7fb937337602eb784ed67 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 6 Mar 2022 10:59:29 +0100 Subject: Complete the parallel solver fix --- src/dynamics/joint/impulse_joint/impulse_joint.rs | 1 - .../joint/impulse_joint/impulse_joint_set.rs | 1 - src/dynamics/joint/multibody_joint/multibody.rs | 43 ++++++++++++++++------ .../joint/multibody_joint/multibody_joint.rs | 5 +++ .../joint/multibody_joint/unit_multibody_joint.rs | 24 +++++++++--- 5 files changed, 55 insertions(+), 19 deletions(-) (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index 4677a1f..f80380e 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -15,5 +15,4 @@ pub struct ImpulseJoint { // A joint needs to know its handle to simplify its removal. pub(crate) handle: ImpulseJointHandle, - pub(crate) constraint_index: usize, } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index cd11a61..448b87d 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -187,7 +187,6 @@ impl ImpulseJointSet { data, impulses: na::zero(), handle: ImpulseJointHandle(handle), - constraint_index: 0, }; let default_id = InteractionGraph::<(), ()>::invalid_graph_index(); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index f2fa623..58d11b6 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -970,6 +970,16 @@ impl Multibody { .any(|link| link.joint().num_velocity_constraints() != 0) } + #[inline] + pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) { + let num_constraints: usize = self + .links + .iter() + .map(|l| l.joint().num_velocity_constraints()) + .sum(); + (num_constraints, num_constraints) + } + #[inline] pub fn generate_internal_constraints( &self, @@ -977,21 +987,32 @@ impl Multibody { j_id: &mut usize, jacobians: &mut DVector, out: &mut Vec, + mut insert_at: Option, ) { - let num_constraints: usize = self - .links - .iter() - .map(|l| l.joint().num_velocity_constraints()) - .sum(); - - let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; - if jacobians.nrows() < required_jacobian_len { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + if !cfg!(feature = "parallel") { + let num_constraints: usize = self + .links + .iter() + .map(|l| l.joint().num_velocity_constraints()) + .sum(); + + let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; + if jacobians.nrows() < required_jacobian_len { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } } for link in self.links.iter() { - link.joint() - .velocity_constraints(params, self, link, 0, j_id, jacobians, out); + link.joint().velocity_constraints( + params, + self, + link, + 0, + j_id, + jacobians, + out, + &mut insert_at, + ); } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 2f7a71e..5a04070 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -255,6 +255,7 @@ impl MultibodyJoint { j_id: &mut usize, jacobians: &mut DVector, constraints: &mut Vec, + insert_at: &mut Option, ) { let locked_bits = self.data.locked_axes.bits(); let limit_bits = self.data.limit_axes.bits(); @@ -281,6 +282,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } @@ -295,6 +297,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } curr_free_dof += 1; @@ -329,6 +332,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); Some(limits) } else { @@ -347,6 +351,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } curr_free_dof += 1; diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 42212be..a1ec483 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -20,6 +20,7 @@ pub fn unit_joint_limit_constraint( j_id: &mut usize, jacobians: &mut DVector, constraints: &mut Vec, + insert_at: &mut Option, ) { let ndofs = multibody.ndofs(); let joint_velocity = multibody.joint_velocity(link); @@ -60,9 +61,14 @@ pub fn unit_joint_limit_constraint( writeback_id: WritebackId::Limit(dof_id), }; - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); + if let Some(at) = insert_at { + constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); + *at += 1; + } else { + constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( + constraint, + )); + } *j_id += 2 * ndofs; } @@ -79,6 +85,7 @@ pub fn unit_joint_motor_constraint( j_id: &mut usize, jacobians: &mut DVector, constraints: &mut Vec, + insert_at: &mut Option, ) { let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); @@ -128,8 +135,13 @@ pub fn unit_joint_motor_constraint( writeback_id: WritebackId::Limit(dof_id), }; - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); + if let Some(at) = insert_at { + constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); + *at += 1; + } else { + constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( + constraint, + )); + } *j_id += 2 * ndofs; } -- cgit