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

Add 2-axes coupling for angular joint limits

Browse Source
This commit is contained in:
Sébastien Crozet
2022-03-19 14:18:56 +01:00
committed by Sébastien Crozet
parent 8e07d8799f
commit a041e0d314
5 changed files with 414 additions and 763 deletions
Download Patch File Download Diff File Expand all files Collapse all files

315
examples3d/joints3.rs
View File

@@ -1,12 +1,43 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
fn create_coupled_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
use_articulations: bool,
) {
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
let body1 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.translation(origin.coords)
.linvel(vector![5.0, 5.0, 5.0]),
);
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), body1, bodies);
let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
.limits(JointAxis::X, [-3.0, 3.0])
.limits(JointAxis::Y, [0.0, 3.0])
.limits(JointAxis::Z, [0.0, 3.0])
.coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
if use_articulations {
multibody_joints.insert(ground, body1, joint1);
} else {
impulse_joints.insert(ground, body1, joint1);
}
}
fn create_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 2.0;
@@ -30,13 +61,16 @@ fn create_prismatic_joints(
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
};
let mut prism = GenericJoint::prismatic(axis)
.local_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, 0.0])
.limits(JointAxis::X, [-2.0, 2.0]);
impulse_joints.insert(curr_parent, curr_child, prism);
let prism = PrismaticJointBuilder::new(axis)
.local_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift])
.limits([-2.0, 2.0]);
if use_articulations {
multibody_joints.insert(curr_parent, curr_child, prism);
} else {
impulse_joints.insert(curr_parent, curr_child, prism);
}
curr_parent = curr_child;
}
}
@@ -45,8 +79,10 @@ fn create_actuated_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 2.0;
@@ -70,29 +106,36 @@ fn create_actuated_prismatic_joints(
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let mut prism = GenericJoint::prismatic(axis)
.local_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift]);
let mut prism = PrismaticJointBuilder::new(axis)
.local_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, 0.0])
.build();
if i == 1 {
prism = prism
.limits(JointAxis::X, [-Real::MAX, 5.0])
.motor_velocity(JointAxis::X, 1.0, 1.0)
if i == 0 {
prism
.set_motor_velocity(2.0, 1.0e5)
// We set a max impulse so that the motor doesn't fight
// the limits with large forces.
.motor_max_impulse(JointAxis::X, 1.0);
.set_limits([-2.0, 5.0])
.set_motor_max_force(100.0);
} else if i == 1 {
prism
.set_limits([-Real::MAX, 5.0])
.set_motor_velocity(6.0, 1.0e3)
// We set a max impulse so that the motor doesn't fight
// the limits with large forces.
.set_motor_max_force(100.0);
} else if i > 1 {
prism = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0);
} else {
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)
.limits(JointAxis::X, [-2.0, 5.0]);
prism
.set_motor_position(2.0, 1.0e3, 1.0e2)
.set_motor_max_force(60.0);
}
impulse_joints.insert(curr_parent, curr_child, prism);
if use_articulations {
multibody_joints.insert(curr_parent, curr_child, prism);
} else {
impulse_joints.insert(curr_parent, curr_child, prism);
}
curr_parent = curr_child;
}
@@ -102,8 +145,10 @@ fn create_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 2.0;
@@ -135,16 +180,23 @@ fn create_revolute_joints(
let x = Vector::x_axis();
let z = Vector::z_axis();
let revs = [
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]),
RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
];
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]);
if use_articulations {
multibody_joints.insert(curr_parent, handles[0], revs[0]);
multibody_joints.insert(handles[0], handles[1], revs[1]);
multibody_joints.insert(handles[1], handles[2], revs[2]);
multibody_joints.insert(handles[2], handles[3], revs[3]);
} else {
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];
}
@@ -154,7 +206,9 @@ fn create_revolute_joints_with_limits(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
use_articulations: bool,
) {
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
@@ -167,13 +221,39 @@ fn create_revolute_joints_with_limits(
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).limits(JointAxis::X, [-0.2, 0.2]);
impulse_joints.insert(ground, platform1, joint1);
let joint1 = RevoluteJointBuilder::new(z).limits([-0.2, 0.2]);
// let joint1 = GenericJointBuilder::new(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z)
// .local_axis1(z)
// .local_axis2(z)
// .limits(JointAxis::AngX, [-0.2, 0.2])
// .limits(JointAxis::AngY, [0.0, 0.4])
// .limits(JointAxis::AngZ, [0.0, 0.4])
// .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
let mut joint2 = RevoluteJoint::new(z)
.local_anchor2(shift.into())
.limits(JointAxis::Z, [-0.2, 0.2]);
impulse_joints.insert(platform1, platform2, joint2);
if use_articulations {
multibody_joints.insert(ground, platform1, joint1);
} else {
impulse_joints.insert(ground, platform1, joint1);
}
let joint2 = RevoluteJointBuilder::new(z)
.local_anchor2(-Point::from(shift))
.limits([-0.2, 0.2]);
// let joint2 = GenericJointBuilder::new(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z)
// .local_axis1(z)
// .local_axis2(z)
// .local_anchor2(-Point::from(shift))
// .limits(JointAxis::AngX, [-0.2, 0.2])
// .limits(JointAxis::AngY, [0.0, 0.4])
// .limits(JointAxis::AngZ, [0.0, 0.4])
// .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
if use_articulations {
multibody_joints.insert(platform1, platform2, joint2);
} else {
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(
@@ -200,8 +280,10 @@ fn create_fixed_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 1.0;
@@ -235,15 +317,20 @@ fn create_fixed_joints(
if i > 0 {
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]);
impulse_joints.insert(parent_handle, child_handle, joint);
let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint);
} else {
impulse_joints.insert(parent_handle, child_handle, joint);
}
}
// Horizontal joint.
if k > 0 {
let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index];
let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]);
let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -252,11 +339,13 @@ fn create_fixed_joints(
}
}
fn create_ball_joints(
fn create_spherical_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 1.0;
@@ -286,15 +375,21 @@ fn create_ball_joints(
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
let joint =
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint);
} else {
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 = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]);
let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
@@ -303,11 +398,13 @@ fn create_ball_joints(
}
}
fn create_ball_joints_with_limits(
fn create_spherical_joints_with_limits(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
use_articulations: bool,
) {
let shift = vector![0.0, 0.0, 3.0];
@@ -324,32 +421,40 @@ fn create_ball_joints_with_limits(
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 = GenericJoint::ball()
let joint1 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.2, 0.2])
.limits(JointAxis::Y, [-0.2, 0.2]);
impulse_joints.insert(ground, ball1, joint1);
let mut joint2 = GenericJoint::ball()
let joint2 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.3, 0.3])
.limits(JointAxis::Y, [-0.3, 0.3]);
impulse_joints.insert(ball1, ball2, joint2);
if use_articulations {
multibody_joints.insert(ground, ball1, joint1);
multibody_joints.insert(ball1, ball2, joint2);
} else {
impulse_joints.insert(ground, ball1, joint1);
impulse_joints.insert(ball1, ball2, joint2);
}
}
fn create_actuated_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 2.0;
// 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 joint_template = RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -377,42 +482,47 @@ fn create_actuated_revolute_joints(
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
let mut joint = joint_template
.clone()
.motor_model(MotorModel::AccelerationBased);
let mut joint = joint_template.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 {
joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1);
joint = joint.motor_velocity(-20.0, 100.0);
} else if i == num - 1 {
let stiffness = 0.2;
let damping = 1.0;
jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping);
let stiffness = 200.0;
let damping = 100.0;
joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
}
if i == 1 {
joint.local_frame2.translation.vector.y = 2.0;
joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1);
joint = joint
.local_anchor2(point![0.0, 2.0, -shift])
.motor_velocity(-2.0, 1000.0);
}
impulse_joints.insert(parent_handle, child_handle, joint);
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint);
} else {
impulse_joints.insert(parent_handle, child_handle, joint);
}
}
parent_handle = child_handle;
}
}
fn create_actuated_ball_joints(
fn create_actuated_spherical_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>,
num: usize,
use_articulations: bool,
) {
let rad = 0.4;
let shift = 2.0;
// We will reuse this base configuration for all the impulse_joints here.
let joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]);
let joint_template = SphericalJointBuilder::new().local_anchor1(point![0.0, 0.0, shift]);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -454,76 +564,111 @@ fn create_actuated_ball_joints(
.motor_position(JointAxis::AngZ, 3.14 / 2.0, stiffness, damping);
}
impulse_joints.insert(parent_handle, child_handle, joint);
if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint);
} else {
impulse_joints.insert(parent_handle, child_handle, joint);
}
}
parent_handle = child_handle;
}
}
pub fn init_world(testbed: &mut Testbed) {
fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
// create_prismatic_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![20.0, 5.0, 0.0],
// 4,
// use_articulations,
// );
// create_actuated_prismatic_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![25.0, 5.0, 0.0],
// 4,
// use_articulations,
// );
// create_revolute_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![20.0, 0.0, 0.0],
// 3,
// use_articulations,
// );
// create_revolute_joints_with_limits(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// point![34.0, 0.0, 0.0],
// );
create_revolute_joints_with_limits(
&mut bodies,
&mut colliders,
&mut impulse_joints,
&mut multibody_joints,
point![34.0, 0.0, 0.0],
use_articulations,
);
// create_fixed_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![0.0, 10.0, 0.0],
// 10,
// use_articulations,
// );
create_actuated_revolute_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![20.0, 10.0, 0.0],
6,
);
// create_actuated_ball_joints(
// create_actuated_revolute_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![20.0, 10.0, 0.0],
// 6,
// use_articulations,
// );
// create_actuated_spherical_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![13.0, 10.0, 0.0],
// 3,
// use_articulations,
// );
// create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
// create_ball_joints_with_limits(
// create_spherical_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// 15,
// use_articulations,
// );
// create_spherical_joints_with_limits(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![-5.0, 0.0, 0.0],
// use_articulations,
// );
// create_coupled_joints(
// &mut bodies,
// &mut colliders,
// &mut impulse_joints,
// &mut multibody_joints,
// point![0.0, 20.0, 0.0],
// use_articulations,
// );
/*
@@ -532,3 +677,11 @@ pub fn init_world(testbed: &mut Testbed) {
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]);
}
pub fn init_world_with_joints(testbed: &mut Testbed) {
do_init_world(testbed, false)
}
pub fn init_world_with_articulations(testbed: &mut Testbed) {
do_init_world(testbed, true)
}