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

Complete the parallel solver fix

Browse Source
This commit is contained in:
Sébastien Crozet
2022-03-06 10:59:29 +01:00
committed by Sébastien Crozet
parent 2e6f133b95
commit 815de4beff
18 changed files with 226 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -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<Real>,
out: &mut Vec<AnyJointVelocityConstraint>,
mut insert_at: Option<usize>,
) {
let num_constraints: usize = self
.links
.iter()
.map(|l| l.joint().num_velocity_constraints())
.sum();
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);
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,
);
}
}
}

5
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -255,6 +255,7 @@ impl MultibodyJoint {
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
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;

24
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -20,6 +20,7 @@ pub fn unit_joint_limit_constraint(
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
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<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
insert_at: &mut Option<usize>,
) {
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;
}