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

Joint API and joint motors improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:55:00 +01:00
committed by Sébastien Crozet
parent e740493b98
commit fb20d72ee2
108 changed files with 2650 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
examples3d/joints3.rs
View File

@@ -11,20 +11,17 @@ fn create_prismatic_joints(
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(vector![origin.x, origin.y, origin.z])
.build();
let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(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(vector![origin.x, origin.y, z])
.build();
let rigid_body =
RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
@@ -33,10 +30,10 @@ fn create_prismatic_joints(
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
};
let mut prism = JointData::prismatic(axis)
let mut prism = GenericJoint::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]);
.limits(JointAxis::X, [-2.0, 2.0]);
impulse_joints.insert(curr_parent, curr_child, prism);
@@ -54,20 +51,17 @@ fn create_actuated_prismatic_joints(
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(vector![origin.x, origin.y, origin.z])
.build();
let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(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(vector![origin.x, origin.y, z])
.build();
let rigid_body =
RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
@@ -76,13 +70,13 @@ fn create_actuated_prismatic_joints(
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let mut prism = JointData::prismatic(axis)
let mut prism = GenericJoint::prismatic(axis)
.local_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift]);
if i == 1 {
prism = prism
.limit_axis(JointAxis::X, [-Real::MAX, 5.0])
.limits(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.
@@ -95,7 +89,7 @@ fn create_actuated_prismatic_joints(
// 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]);
.limits(JointAxis::X, [-2.0, 5.0]);
}
impulse_joints.insert(curr_parent, curr_child, prism);
@@ -114,11 +108,9 @@ fn create_revolute_joints(
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(vector![origin.x, origin.y, 0.0])
.build();
let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]);
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num {
@@ -133,11 +125,9 @@ fn create_revolute_joints(
let mut handles = [curr_parent; 4];
for k in 0..4 {
let rigid_body = RigidBodyBuilder::new_dynamic()
.position(positions[k])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
handles[k] = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, handles[k], bodies);
}
@@ -166,63 +156,41 @@ fn create_revolute_joints_with_limits(
impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>,
) {
let ground = bodies.insert(
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
let platform1 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform1,
bodies,
);
let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords));
colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies);
let shift = vector![0.0, 0.0, 6.0];
let platform2 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform2,
bodies,
);
let platform2 =
bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift));
colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
let z = Vector::z_axis();
let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]);
let mut joint1 = RevoluteJoint::new(z).limits(JointAxis::X, [-0.2, 0.2]);
impulse_joints.insert(ground, platform1, joint1);
let mut joint2 = RevoluteJoint::new(z)
.local_anchor2(shift.into())
.limit_axis(JointAxis::Z, [-0.2, 0.2]);
.limits(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(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + vector![-2.0, 4.0, 0.0])
.build(),
RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0),
cuboid_body1,
bodies,
);
let cuboid_body2 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift + vector![2.0, 16.0, 0.0])
.build(),
.translation(origin.coords + shift + vector![2.0, 16.0, 0.0]),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(),
ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0),
cuboid_body2,
bodies,
);
@@ -254,22 +222,20 @@ fn create_fixed_joints(
RigidBodyType::Dynamic
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![
origin.x + fk * shift,
origin.y,
origin.z + fi * shift
])
.build();
let rigid_body = RigidBodyBuilder::new(status).translation(vector![
origin.x + fk * shift,
origin.y,
origin.z + fi * shift
]);
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).build();
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint.
if i > 0 {
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]);
let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -277,7 +243,7 @@ fn create_fixed_joints(
if k > 0 {
let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index];
let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -308,17 +274,19 @@ fn create_ball_joints(
RigidBodyType::Dynamic
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![fk * shift, 0.0, fi * shift * 2.0])
.build();
let rigid_body = RigidBodyBuilder::new(status).translation(vector![
fk * shift,
0.0,
fi * shift * 2.0
]);
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
let collider = ColliderBuilder::capsule_z(rad * 1.25, rad);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -326,7 +294,7 @@ fn create_ball_joints(
if k > 0 {
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]);
let joint = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -343,45 +311,29 @@ fn create_ball_joints_with_limits(
) {
let shift = vector![0.0, 0.0, 3.0];
let ground = bodies.insert(
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
let ball1 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift)
.linvel(vector![20.0, 20.0, 0.0])
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball1,
bodies,
.linvel(vector![20.0, 20.0, 0.0]),
);
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies);
let ball2 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift * 2.0)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball2,
bodies,
);
let ball2 =
bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0));
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies);
let mut joint1 = JointData::ball()
let mut joint1 = GenericJoint::ball()
.local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.2, 0.2])
.limit_axis(JointAxis::Y, [-0.2, 0.2]);
.limits(JointAxis::X, [-0.2, 0.2])
.limits(JointAxis::Y, [-0.2, 0.2]);
impulse_joints.insert(ground, ball1, joint1);
let mut joint2 = JointData::ball()
let mut joint2 = GenericJoint::ball()
.local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.3, 0.3])
.limit_axis(JointAxis::Y, [-0.3, 0.3]);
.limits(JointAxis::X, [-0.3, 0.3])
.limits(JointAxis::Y, [-0.3, 0.3]);
impulse_joints.insert(ball1, ball2, joint2);
}
@@ -418,26 +370,28 @@ fn create_actuated_revolute_joints(
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build();
;
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build();
let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad);
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
let mut joint = joint_template.clone();
let mut joint = joint_template
.clone()
.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 {
joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1);
joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1);
} else if i == num - 1 {
let stiffness = 0.2;
let damping = 1.0;
joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
}
if i == 1 {
joint.local_frame2.translation.vector.y = 2.0;
joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1);
joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1);
}
impulse_joints.insert(parent_handle, child_handle, joint);
@@ -458,7 +412,7 @@ fn create_actuated_ball_joints(
let shift = 2.0;
// 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 joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -477,10 +431,10 @@ fn create_actuated_ball_joints(
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build();
;
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build();
let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad);
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
@@ -516,40 +470,40 @@ pub fn init_world(testbed: &mut Testbed) {
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
create_prismatic_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![20.0, 5.0, 0.0],
4,
);
create_actuated_prismatic_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![25.0, 5.0, 0.0],
4,
);
create_revolute_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![20.0, 0.0, 0.0],
3,
);
create_revolute_joints_with_limits(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![34.0, 0.0, 0.0],
);
create_fixed_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![0.0, 10.0, 0.0],
10,
);
// create_prismatic_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![20.0, 5.0, 0.0],
// 4,
// );
// create_actuated_prismatic_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![25.0, 5.0, 0.0],
// 4,
// );
// create_revolute_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![20.0, 0.0, 0.0],
// 3,
// );
// create_revolute_joints_with_limits(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![34.0, 0.0, 0.0],
// );
// create_fixed_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![0.0, 10.0, 0.0],
// 10,
// );
create_actuated_revolute_joints(
&mut bodies,
&mut colliders,
@@ -557,20 +511,20 @@ pub fn init_world(testbed: &mut Testbed) {
point![20.0, 10.0, 0.0],
6,
);
create_actuated_ball_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![13.0, 10.0, 0.0],
3,
);
create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
create_ball_joints_with_limits(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![-5.0, 0.0, 0.0],
);
// create_actuated_ball_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![13.0, 10.0, 0.0],
// 3,
// );
// create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
// create_ball_joints_with_limits(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![-5.0, 0.0, 0.0],
// );
/*
* Set up the testbed.