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

Some minor cleanup and joint constraint refactoring.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-22 13:58:43 +01:00
parent 052a5a5fc0
commit 4c9138fd2b
4 changed files with 170 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
examples3d/joints3.rs
View File

@@ -57,6 +57,73 @@ fn create_prismatic_joints(
}
}
fn create_actuated_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
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;
}
}
fn create_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
@@ -362,6 +429,13 @@ pub fn init_world(testbed: &mut Testbed) {
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,
&mut colliders,

40
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -183,10 +183,7 @@ impl BallVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<Real>, mj_lambda2: &mut DeltaVel<Real>) {
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);
@@ -201,10 +198,9 @@ impl BallVelocityConstraint {
mj_lambda2.linear -= self.im2 * impulse;
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
}
/*
* Motor part.
*/
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
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);
@@ -224,6 +220,14 @@ impl BallVelocityConstraint {
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<Real>]) {
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;
@@ -232,7 +236,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;
}
}
}
@@ -393,9 +398,7 @@ impl BallVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
let dvel = vel2 + self.rhs;
@@ -405,10 +408,9 @@ impl BallVelocityGroundConstraint {
mj_lambda2.linear -= self.im2 * impulse;
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
}
/*
* Motor part.
*/
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -425,6 +427,13 @@ impl BallVelocityGroundConstraint {
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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;
}
@@ -433,7 +442,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;
}
}
}

104
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -308,27 +308,11 @@ impl WPrismaticVelocityConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<SimdReal>,
mj_lambda2: &mut DeltaVel<SimdReal>,
) {
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);
@@ -357,10 +341,13 @@ 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<SimdReal>,
mj_lambda2: &mut DeltaVel<SimdReal>,
) {
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -381,6 +368,28 @@ impl WPrismaticVelocityConstraint {
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<Real>]) {
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);
@@ -503,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;
@@ -642,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint {
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<SimdReal>) {
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);
@@ -676,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<SimdReal>) {
if let Some(limits_forcedir2) = self.limits_forcedir2 {
// FIXME: the transformation by ii2_sqrt could be avoided by
// reusing some computations above.
@@ -693,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint {
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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);
@@ -700,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;

35
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -209,10 +209,7 @@ impl RevoluteVelocityConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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<Real>, mj_lambda2: &mut DeltaVel<Real>) {
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
@@ -237,10 +234,9 @@ impl RevoluteVelocityConstraint {
mj_lambda2.angular -= self
.ii2_sqrt
.transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
}
/*
* Motor.
*/
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
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);
@@ -258,6 +254,14 @@ impl RevoluteVelocityConstraint {
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<Real>]) {
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;
@@ -452,9 +456,7 @@ impl RevoluteVelocityGroundConstraint {
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
@@ -470,10 +472,8 @@ impl RevoluteVelocityGroundConstraint {
mj_lambda2.angular -= self
.ii2_sqrt
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
/*
* Motor.
*/
}
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
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);
@@ -489,6 +489,13 @@ impl RevoluteVelocityGroundConstraint {
mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
}
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
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;
}