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

Fix torque generation for the prismatic joint motor

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-13 11:44:34 +02:00
parent 7ae8184167
commit d70c6f82e3
4 changed files with 159 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples3d/all_examples3.rs
View File

@@ -23,6 +23,7 @@ mod debug_cylinder3;
mod debug_dynamic_collider_add3;
mod debug_friction3;
mod debug_infinite_fall3;
mod debug_prismatic3;
mod debug_rollback3;
mod debug_triangle3;
mod debug_trimesh3;
@@ -109,6 +110,7 @@ pub fn main() {
("(Debug) trimesh", debug_trimesh3::init_world),
("(Debug) cylinder", debug_cylinder3::init_world),
("(Debug) infinite fall", debug_infinite_fall3::init_world),
("(Debug) prismatic", debug_prismatic3::init_world),
("(Debug) rollback", debug_rollback3::init_world),
];

111
examples3d/debug_prismatic3.rs Normal file
View File

@@ -0,0 +1,111 @@
use na::{Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
fn prismatic_repro(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
box_center: Point3<f32>,
) {
use rapier3d::{dynamics::RigidBodyBuilder, geometry::ColliderBuilder};
let box_rb = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(box_center.x, box_center.y, box_center.z)
.build(),
);
colliders.insert(
ColliderBuilder::cuboid(1.0, 0.25, 1.0).build(),
box_rb,
bodies,
);
let wheel_y = -1.0;
let wheel_positions = vec![
Vector3::new(1.0, wheel_y, -1.0),
Vector3::new(-1.0, wheel_y, -1.0),
Vector3::new(1.0, wheel_y, 1.0),
Vector3::new(-1.0, wheel_y, 1.0),
];
for pos in wheel_positions {
let wheel_pos_in_world = box_center + pos;
let wheel_rb = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(
wheel_pos_in_world.x,
wheel_pos_in_world.y,
wheel_pos_in_world.z,
)
.build(),
);
colliders.insert(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies);
let mut prismatic = rapier3d::dynamics::PrismaticJoint::new(
Point3::new(pos.x, pos.y, pos.z),
Vector3::y_axis(),
Vector3::default(),
Point3::new(0.0, 0.0, 0.0),
Vector3::y_axis(),
Vector3::default(),
);
prismatic.configure_motor_model(rapier3d::dynamics::SpringModel::VelocityBased);
let (stiffness, damping) = (0.05, 0.2);
prismatic.configure_motor_position(0.0, stiffness, damping);
joints.insert(bodies, box_rb, wheel_rb, prismatic);
}
// put a small box under one of the wheels
let gravel = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(box_center.x + 1.0, box_center.y - 2.4, -1.0)
.build(),
);
colliders.insert(
ColliderBuilder::cuboid(0.5, 0.1, 0.5).build(),
gravel,
bodies,
);
}
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
/*
* Ground
*/
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
colliders.insert(collider, handle, &mut bodies);
prismatic_repro(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(0.0, 5.0, 0.0),
);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

4
examples3d/joints3.rs
View File

@@ -107,12 +107,12 @@ fn create_actuated_prismatic_joints(
// 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);
prism.configure_motor_position(2.0, 0.01, 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.motor_max_impulse = 0.7;
prism.limits_enabled = true;
prism.limits[0] = -2.0;
prism.limits[1] = 5.0;

55
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -83,6 +83,7 @@ impl PrismaticVelocityConstraint {
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 * joint.basis1[0];
#[cfg(feature = "dim3")]
@@ -179,6 +180,8 @@ impl PrismaticVelocityConstraint {
*/
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let gcross1 = r1.gcross(*axis1);
let gcross2 = r2.gcross(*axis2);
let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
params.dt,
@@ -192,12 +195,24 @@ impl PrismaticVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.dot(&gcross2)
- rb1.linvel.dot(&axis1)
- rb1.angvel.dot(&gcross1);
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_inv_lhs = if keep_lhs {
let inv_projected_mass = crate::utils::inv(
im1 + im2
+ gcross1.gdot(ii1.transform_vector(gcross1))
+ gcross2.gdot(ii2.transform_vector(gcross2)),
);
gamma * inv_projected_mass
} else {
gamma
};
motor_rhs /= gamma;
}
@@ -236,8 +251,6 @@ impl PrismaticVelocityConstraint {
limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
* velocity_based_erp_inv_dt;
let gcross1 = r1.gcross(*axis1);
let gcross2 = r2.gcross(*axis2);
limits_inv_lhs = crate::utils::inv(
im1 + im2
+ gcross1.gdot(ii1.transform_vector(gcross1))
@@ -301,8 +314,16 @@ impl PrismaticVelocityConstraint {
.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);
if self.motor_impulse != 0.0 {
let lin_impulse1 = self.motor_axis1 * self.motor_impulse;
let lin_impulse2 = self.motor_axis2 * self.motor_impulse;
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));
}
// Warmstart limits.
if self.limits_active {
@@ -382,19 +403,31 @@ impl PrismaticVelocityConstraint {
fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
if self.motor_inv_lhs != 0.0 {
let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear)
- self.motor_axis1.dot(&mj_lambda1.linear)
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let dvel = self
.motor_axis2
.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
- self
.motor_axis1
.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
+ self.motor_rhs;
let new_impulse = na::clamp(
self.motor_impulse + lin_dvel * self.motor_inv_lhs,
self.motor_impulse + 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);
let lin_impulse1 = self.motor_axis1 * dimpulse;
let lin_impulse2 = self.motor_axis2 * 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));
}
}