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

Implement multibody joints and the new solver

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-02 14:47:40 +01:00
parent b45d4b5ac2
commit f74b8401ad
182 changed files with 9871 additions and 12645 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
examples3d/joints3.rs
View File

@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
fn create_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
num: usize,
) {
@@ -28,25 +28,17 @@ fn create_prismatic_joints(
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0])
} else {
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
};
let z = Vector::z();
let mut prism = PrismaticJoint::new(
point![0.0, 0.0, 0.0],
axis,
z,
point![0.0, 0.0, -shift],
axis,
z,
);
prism.limits_enabled = true;
prism.limits[0] = -2.0;
prism.limits[1] = 2.0;
let mut prism = JointData::prismatic(axis)
.local_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, 0.0])
.limit_axis(JointAxis::X, [-2.0, 2.0]);
joints.insert(curr_parent, curr_child, prism);
impulse_joints.insert(curr_parent, curr_child, prism);
curr_parent = curr_child;
}
@@ -55,7 +47,7 @@ fn create_prismatic_joints(
fn create_actuated_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
num: usize,
) {
@@ -84,36 +76,29 @@ fn create_actuated_prismatic_joints(
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let z = Vector::z();
let mut prism = PrismaticJoint::new(
point![0.0, 0.0, 0.0],
axis,
z,
point![0.0, 0.0, -shift],
axis,
z,
);
let mut prism = JointData::prismatic(axis)
.local_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift]);
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;
prism = prism
.limit_axis(JointAxis::X, [-Real::MAX, 5.0])
.motor_velocity(JointAxis::X, 1.0, 1.0)
// We set a max impulse so that the motor doesn't fight
// the limits with large forces.
.motor_max_impulse(JointAxis::X, 1.0);
} else if i > 1 {
prism.configure_motor_position(2.0, 0.01, 1.0);
prism = prism.motor_position(JointAxis::X, 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 = 0.7;
prism.limits_enabled = true;
prism.limits[0] = -2.0;
prism.limits[1] = 5.0;
prism = prism
.motor_velocity(JointAxis::X, 1.0, 1.0)
// We set a max impulse so that the motor doesn't fight
// the limits with large forces.
.motor_max_impulse(JointAxis::X, 0.7)
.limit_axis(JointAxis::X, [-2.0, 5.0]);
}
joints.insert(curr_parent, curr_child, prism);
impulse_joints.insert(curr_parent, curr_child, prism);
curr_parent = curr_child;
}
@@ -122,7 +107,7 @@ fn create_actuated_prismatic_joints(
fn create_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
num: usize,
) {
@@ -156,22 +141,20 @@ fn create_revolute_joints(
colliders.insert_with_parent(collider, handles[k], bodies);
}
// Setup four joints.
let o = Point::origin();
// Setup four impulse_joints.
let x = Vector::x_axis();
let z = Vector::z_axis();
let revs = [
RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x),
RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x),
RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(z).local_anchor2(point![-shift, 0.0, 0.0]),
RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(z).local_anchor2(point![shift, 0.0, 0.0]),
];
joints.insert(curr_parent, handles[0], revs[0]);
joints.insert(handles[0], handles[1], revs[1]);
joints.insert(handles[1], handles[2], revs[2]);
joints.insert(handles[2], handles[3], revs[3]);
impulse_joints.insert(curr_parent, handles[0], revs[0]);
impulse_joints.insert(handles[0], handles[1], revs[1]);
impulse_joints.insert(handles[1], handles[2], revs[2]);
impulse_joints.insert(handles[2], handles[3], revs[3]);
curr_parent = handles[3];
}
@@ -180,7 +163,7 @@ fn create_revolute_joints(
fn create_revolute_joints_with_limits(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
) {
let ground = bodies.insert(
@@ -212,25 +195,14 @@ fn create_revolute_joints_with_limits(
bodies,
);
let mut joint1 = RevoluteJoint::new(
Point::origin(),
Vector::z_axis(),
Point::origin(),
Vector::z_axis(),
);
joint1.limits_enabled = true;
joint1.limits = [-0.2, 0.2];
joints.insert(ground, platform1, joint1);
let z = Vector::z_axis();
let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]);
impulse_joints.insert(ground, platform1, joint1);
let mut joint2 = RevoluteJoint::new(
Point::origin(),
Vector::z_axis(),
Point::from(-shift),
Vector::z_axis(),
);
joint2.limits_enabled = true;
joint2.limits = [-0.3, 0.3];
joints.insert(platform1, platform2, joint2);
let mut joint2 = RevoluteJoint::new(z)
.local_anchor2(shift.into())
.limit_axis(JointAxis::Z, [-0.2, 0.2]);
impulse_joints.insert(platform1, platform2, joint2);
// Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
let cuboid_body1 = bodies.insert(
@@ -259,7 +231,7 @@ fn create_revolute_joints_with_limits(
fn create_fixed_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
num: usize,
) {
@@ -268,8 +240,8 @@ fn create_fixed_joints(
let mut body_handles = Vec::new();
for k in 0..num {
for i in 0..num {
for i in 0..num {
for k in 0..num {
let fk = k as f32;
let fi = i as f32;
@@ -295,23 +267,18 @@ fn create_fixed_joints(
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = FixedJoint::new(
Isometry::identity(),
Isometry::translation(0.0, 0.0, -shift),
);
joints.insert(parent_handle, child_handle, joint);
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = JointData::fixed().local_anchor2(point![0.0, 0.0, -shift]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
// Horizontal joint.
if k > 0 {
let parent_index = body_handles.len() - num;
let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index];
let joint = FixedJoint::new(
Isometry::identity(),
Isometry::translation(-shift, 0.0, 0.0),
);
joints.insert(parent_handle, child_handle, joint);
let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
body_handles.push(child_handle);
@@ -322,7 +289,7 @@ fn create_fixed_joints(
fn create_ball_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
num: usize,
) {
let rad = 0.4;
@@ -351,16 +318,16 @@ fn create_ball_joints(
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift * 2.0]);
joints.insert(parent_handle, child_handle, joint);
let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
// Horizontal joint.
if k > 0 {
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]);
joints.insert(parent_handle, child_handle, joint);
let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
body_handles.push(child_handle);
@@ -371,7 +338,7 @@ fn create_ball_joints(
fn create_ball_joints_with_limits(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
) {
let shift = vector![0.0, 0.0, 3.0];
@@ -405,38 +372,32 @@ fn create_ball_joints_with_limits(
bodies,
);
let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift));
joint1.limits_enabled = true;
joint1.limits_local_axis1 = Vector::z_axis();
joint1.limits_local_axis2 = Vector::z_axis();
joint1.limits_angle = 0.2;
joints.insert(ground, ball1, joint1);
let mut joint1 = JointData::ball()
.local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.2, 0.2])
.limit_axis(JointAxis::Y, [-0.2, 0.2]);
impulse_joints.insert(ground, ball1, joint1);
let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift));
joint2.limits_enabled = true;
joint2.limits_local_axis1 = Vector::z_axis();
joint2.limits_local_axis2 = Vector::z_axis();
joint2.limits_angle = 0.3;
joints.insert(ball1, ball2, joint2);
let mut joint2 = JointData::ball()
.local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.3, 0.3])
.limit_axis(JointAxis::Y, [-0.3, 0.3]);
impulse_joints.insert(ball1, ball2, joint2);
}
fn create_actuated_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
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(
Point::origin(),
Vector::z_axis(),
point![0.0, 0.0, -shift],
Vector::z_axis(),
);
// We will reuse this base configuration for all the impulse_joints here.
let z = Vector::z_axis();
let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -467,19 +428,19 @@ fn create_actuated_revolute_joints(
let mut joint = joint_template.clone();
if i % 3 == 1 {
joint.configure_motor_velocity(-20.0, 0.1);
joint = joint.motor_velocity(JointAxis::AngX, -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);
joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
}
if i == 1 {
joint.local_anchor2.y = 2.0;
joint.configure_motor_velocity(-2.0, 0.1);
joint.local_frame2.translation.vector.y = 2.0;
joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1);
}
joints.insert(parent_handle, child_handle, joint);
impulse_joints.insert(parent_handle, child_handle, joint);
}
parent_handle = child_handle;
@@ -489,15 +450,15 @@ fn create_actuated_revolute_joints(
fn create_actuated_ball_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
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(point![0.0, 0.0, shift], Point::origin());
// We will reuse this base configuration for all the impulse_joints here.
let joint_template = JointData::ball().local_anchor1(point![0.0, 0.0, shift]);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -526,18 +487,20 @@ fn create_actuated_ball_joints(
let mut joint = joint_template.clone();
if i == 1 {
joint.configure_motor_velocity(vector![0.0, 0.5, -2.0], 0.1);
joint = joint
.motor_velocity(JointAxis::AngX, 0.0, 0.1)
.motor_velocity(JointAxis::AngY, 0.5, 0.1)
.motor_velocity(JointAxis::AngZ, -2.0, 0.1);
} else if i == num - 1 {
let stiffness = 0.2;
let damping = 1.0;
joint.configure_motor_position(
Rotation::new(vector![0.0, 1.0, 3.14 / 2.0]),
stiffness,
damping,
);
joint = joint
.motor_position(JointAxis::AngX, 0.0, stiffness, damping)
.motor_position(JointAxis::AngY, 1.0, stiffness, damping)
.motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
}
joints.insert(parent_handle, child_handle, joint);
impulse_joints.insert(parent_handle, child_handle, joint);
}
parent_handle = child_handle;
@@ -550,67 +513,68 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
create_prismatic_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![20.0, 5.0, 0.0],
4,
);
create_actuated_prismatic_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![25.0, 5.0, 0.0],
4,
);
create_revolute_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![20.0, 0.0, 0.0],
3,
);
create_revolute_joints_with_limits(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![34.0, 0.0, 0.0],
);
create_fixed_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![0.0, 10.0, 0.0],
10,
);
create_actuated_revolute_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![20.0, 10.0, 0.0],
6,
);
create_actuated_ball_joints(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![13.0, 10.0, 0.0],
3,
);
create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
create_ball_joints_with_limits(
&mut bodies,
&mut colliders,
&mut joints,
&mut impulse_joints,
point![-5.0, 0.0, 0.0],
);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
}