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

9
examples3d/all_examples3.rs
View File

@@ -8,7 +8,6 @@ use inflector::Inflector;
use rapier_testbed3d::{Testbed, TestbedApp}; use rapier_testbed3d::{Testbed, TestbedApp};
use std::cmp::Ordering; use std::cmp::Ordering;
mod articulations3;
mod ccd3; mod ccd3;
mod collision_groups3; mod collision_groups3;
mod compound3; mod compound3;
@@ -32,6 +31,7 @@ mod debug_trimesh3;
mod domino3; mod domino3;
mod fountain3; mod fountain3;
mod heightfield3; mod heightfield3;
mod joints3;
// mod joints3; // mod joints3;
mod keva3; mod keva3;
mod locked_rotations3; mod locked_rotations3;
@@ -81,10 +81,7 @@ pub fn main() {
let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
("Fountain", fountain3::init_world), ("Fountain", fountain3::init_world),
("Primitives", primitives3::init_world), ("Primitives", primitives3::init_world),
( ("Multibody joints", joints3::init_world_with_articulations),
"Multibody joints",
articulations3::init_world_with_articulations,
),
("CCD", ccd3::init_world), ("CCD", ccd3::init_world),
("Collision groups", collision_groups3::init_world), ("Collision groups", collision_groups3::init_world),
("Compound", compound3::init_world), ("Compound", compound3::init_world),
@@ -93,7 +90,7 @@ pub fn main() {
("Damping", damping3::init_world), ("Damping", damping3::init_world),
("Domino", domino3::init_world), ("Domino", domino3::init_world),
("Heightfield", heightfield3::init_world), ("Heightfield", heightfield3::init_world),
("Impulse Joints", articulations3::init_world_with_joints), ("Impulse Joints", joints3::init_world_with_joints),
("Locked rotations", locked_rotations3::init_world), ("Locked rotations", locked_rotations3::init_world),
("One-way platforms", one_way_platforms3::init_world), ("One-way platforms", one_way_platforms3::init_world),
("Platform", platform3::init_world), ("Platform", platform3::init_world),

671
examples3d/articulations3.rs
View File

@@ -1,671 +0,0 @@
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;
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);
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]);
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
UnitVector::new_normalize(vector![1.0f32, 1.0, 0.0])
} else {
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
};
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;
}
}
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;
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);
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]);
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
} else {
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
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 == 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.
.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
.set_motor_position(2.0, 1.0e3, 1.0e2)
.set_motor_max_force(60.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;
}
}
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;
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);
colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num {
// Create four bodies.
let z = origin.z + i as f32 * shift * 2.0 + shift;
let positions = [
Isometry::translation(origin.x, origin.y, z),
Isometry::translation(origin.x + shift, origin.y, z),
Isometry::translation(origin.x + shift, origin.y, z + shift),
Isometry::translation(origin.x, origin.y, z + shift),
];
let mut handles = [curr_parent; 4];
for k in 0..4 {
let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
handles[k] = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, handles[k], bodies);
}
// Setup four impulse_joints.
let x = Vector::x_axis();
let z = Vector::z_axis();
let revs = [
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]),
];
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];
}
}
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));
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));
colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
let z = Vector::z_axis();
let joint1 = RevoluteJointBuilder::new(z).limits([-0.2, 0.2]);
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]);
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(
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),
cuboid_body1,
bodies,
);
let cuboid_body2 = bodies.insert(
RigidBodyBuilder::new_dynamic()
.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),
cuboid_body2,
bodies,
);
}
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;
let mut body_handles = Vec::new();
for i in 0..num {
for k in 0..num {
let fk = k as f32;
let fi = i as f32;
// NOTE: the num - 2 test is to avoid two consecutive
// fixed bodies. Because physx will crash if we add
// a joint between these.
let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
RigidBodyType::Static
} else {
RigidBodyType::Dynamic
};
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);
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 = 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 = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
body_handles.push(child_handle);
}
}
}
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;
let mut body_handles = Vec::new();
for k in 0..num {
for i in 0..num {
let fk = k as f32;
let fi = i as f32;
let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
RigidBodyType::Static
} else {
RigidBodyType::Dynamic
};
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);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
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 = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint);
}
body_handles.push(child_handle);
}
}
}
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];
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]),
);
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));
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies);
let joint1 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.2, 0.2])
.limits(JointAxis::Y, [-0.2, 0.2]);
let joint2 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.3, 0.3])
.limits(JointAxis::Y, [-0.3, 0.3]);
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 = RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]);
let mut parent_handle = RigidBodyHandle::invalid();
for i in 0..num {
let fi = i as f32;
// NOTE: the num - 2 test is to avoid two consecutive
// fixed bodies. Because physx will crash if we add
// a joint between these.
let status = if i == 0 {
RigidBodyType::Static
} else {
RigidBodyType::Dynamic
};
let shifty = (i >= 1) as u32 as f32 * -2.0;
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))
;
let child_handle = bodies.insert(rigid_body);
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.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 {
joint = joint.motor_velocity(-20.0, 100.0);
} else if i == num - 1 {
let stiffness = 200.0;
let damping = 100.0;
joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
}
if i == 1 {
joint = joint
.local_anchor2(point![0.0, 2.0, -shift])
.motor_velocity(-2.0, 1000.0);
}
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_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 = SphericalJointBuilder::new().local_anchor1(point![0.0, 0.0, shift]);
let mut parent_handle = RigidBodyHandle::invalid();
for i in 0..num {
let fi = i as f32;
// NOTE: the num - 2 test is to avoid two consecutive
// fixed bodies. Because physx will crash if we add
// a joint between these.
let status = if i == 0 {
RigidBodyType::Static
} else {
RigidBodyType::Dynamic
};
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))
;
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad);
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
let mut joint = joint_template.clone();
if i == 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 = 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);
}
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 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 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,
&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,
&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_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,
);
/*
* Set up the 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)
}

315
examples3d/joints3.rs
View File

@@ -1,12 +1,43 @@
use rapier3d::prelude::*; use rapier3d::prelude::*;
use rapier_testbed3d::Testbed; 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( fn create_prismatic_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
@@ -30,13 +61,16 @@ fn create_prismatic_joints(
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0])
}; };
let mut prism = GenericJoint::prismatic(axis) let prism = PrismaticJointBuilder::new(axis)
.local_anchor1(point![0.0, 0.0, shift]) .local_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, 0.0]) .local_anchor2(point![0.0, 0.0, -shift])
.limits(JointAxis::X, [-2.0, 2.0]); .limits([-2.0, 2.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; curr_parent = curr_child;
} }
} }
@@ -45,8 +79,10 @@ fn create_actuated_prismatic_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
@@ -70,29 +106,36 @@ fn create_actuated_prismatic_joints(
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
}; };
let mut prism = GenericJoint::prismatic(axis) let mut prism = PrismaticJointBuilder::new(axis)
.local_anchor1(point![0.0, 0.0, 0.0]) .local_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, -shift]); .local_anchor2(point![0.0, 0.0, 0.0])
.build();
if i == 1 { if i == 0 {
prism = prism prism
.limits(JointAxis::X, [-Real::MAX, 5.0]) .set_motor_velocity(2.0, 1.0e5)
.motor_velocity(JointAxis::X, 1.0, 1.0)
// We set a max impulse so that the motor doesn't fight // We set a max impulse so that the motor doesn't fight
// the limits with large forces. // 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 { } else if i > 1 {
prism = prism.motor_position(JointAxis::X, 2.0, 0.01, 1.0); prism
} else { .set_motor_position(2.0, 1.0e3, 1.0e2)
prism = prism .set_motor_max_force(60.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, 0.7)
.limits(JointAxis::X, [-2.0, 5.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; curr_parent = curr_child;
} }
@@ -102,8 +145,10 @@ fn create_revolute_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
@@ -135,16 +180,23 @@ fn create_revolute_joints(
let x = Vector::x_axis(); let x = Vector::x_axis();
let z = Vector::z_axis(); let z = Vector::z_axis();
let revs = [ let revs = [
RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(z).local_anchor2(point![-shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
RevoluteJoint::new(x).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(z).local_anchor2(point![shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
]; ];
impulse_joints.insert(curr_parent, handles[0], revs[0]); if use_articulations {
impulse_joints.insert(handles[0], handles[1], revs[1]); multibody_joints.insert(curr_parent, handles[0], revs[0]);
impulse_joints.insert(handles[1], handles[2], revs[2]); multibody_joints.insert(handles[0], handles[1], revs[1]);
impulse_joints.insert(handles[2], handles[3], revs[3]); 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]; curr_parent = handles[3];
} }
@@ -154,7 +206,9 @@ fn create_revolute_joints_with_limits(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
use_articulations: bool,
) { ) {
let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); 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); colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
let z = Vector::z_axis(); let z = Vector::z_axis();
let mut joint1 = RevoluteJoint::new(z).limits(JointAxis::X, [-0.2, 0.2]); let joint1 = RevoluteJointBuilder::new(z).limits([-0.2, 0.2]);
impulse_joints.insert(ground, platform1, joint1); // 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) if use_articulations {
.local_anchor2(shift.into()) multibody_joints.insert(ground, platform1, joint1);
.limits(JointAxis::Z, [-0.2, 0.2]); } else {
impulse_joints.insert(platform1, platform2, joint2); 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’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
let cuboid_body1 = bodies.insert( let cuboid_body1 = bodies.insert(
@@ -200,8 +280,10 @@ fn create_fixed_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 1.0; let shift = 1.0;
@@ -235,15 +317,20 @@ fn create_fixed_joints(
if i > 0 { if i > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]); let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
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);
}
} }
// Horizontal joint. // Horizontal joint.
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - 1; let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index]; 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); 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, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 1.0; let shift = 1.0;
@@ -286,15 +375,21 @@ fn create_ball_joints(
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); let joint =
impulse_joints.insert(parent_handle, child_handle, 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. // Horizontal joint.
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); 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, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
use_articulations: bool,
) { ) {
let shift = vector![0.0, 0.0, 3.0]; 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)); 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); 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)) .local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.2, 0.2]) .limits(JointAxis::X, [-0.2, 0.2])
.limits(JointAxis::Y, [-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)) .local_anchor2(Point::from(-shift))
.limits(JointAxis::X, [-0.3, 0.3]) .limits(JointAxis::X, [-0.3, 0.3])
.limits(JointAxis::Y, [-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( fn create_actuated_revolute_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
// We will reuse this base configuration for all the impulse_joints here. // We will reuse this base configuration for all the impulse_joints here.
let z = Vector::z_axis(); 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(); let mut parent_handle = RigidBodyHandle::invalid();
@@ -377,42 +482,47 @@ fn create_actuated_revolute_joints(
colliders.insert_with_parent(collider, child_handle, bodies); colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 { if i > 0 {
let mut joint = joint_template let mut joint = joint_template.motor_model(MotorModel::AccelerationBased);
.clone()
.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 { 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 { } else if i == num - 1 {
let stiffness = 0.2; let stiffness = 200.0;
let damping = 1.0; let damping = 100.0;
jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
} }
if i == 1 { if i == 1 {
joint.local_frame2.translation.vector.y = 2.0; joint = joint
joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1); .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; parent_handle = child_handle;
} }
} }
fn create_actuated_ball_joints( fn create_actuated_spherical_joints(
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
colliders: &mut ColliderSet, colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
origin: Point<f32>, origin: Point<f32>,
num: usize, num: usize,
use_articulations: bool,
) { ) {
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
// We will reuse this base configuration for all the impulse_joints here. // 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(); 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); .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; parent_handle = child_handle;
} }
} }
pub fn init_world(testbed: &mut Testbed) { fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
/* /*
* World * World
*/ */
let mut bodies = RigidBodySet::new(); let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new(); let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new(); let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new(); let mut multibody_joints = MultibodyJointSet::new();
// create_prismatic_joints( // create_prismatic_joints(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &mut impulse_joints,
// &mut multibody_joints,
// point![20.0, 5.0, 0.0], // point![20.0, 5.0, 0.0],
// 4, // 4,
// use_articulations,
// ); // );
// create_actuated_prismatic_joints( // create_actuated_prismatic_joints(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &mut impulse_joints,
// &mut multibody_joints,
// point![25.0, 5.0, 0.0], // point![25.0, 5.0, 0.0],
// 4, // 4,
// use_articulations,
// ); // );
// create_revolute_joints( // create_revolute_joints(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &mut impulse_joints,
// &mut multibody_joints,
// point![20.0, 0.0, 0.0], // point![20.0, 0.0, 0.0],
// 3, // 3,
// use_articulations,
// ); // );
// create_revolute_joints_with_limits( create_revolute_joints_with_limits(
// &mut bodies, &mut bodies,
// &mut colliders, &mut colliders,
// &mut impulse_joints, &mut impulse_joints,
// point![34.0, 0.0, 0.0], &mut multibody_joints,
// ); point![34.0, 0.0, 0.0],
use_articulations,
);
// create_fixed_joints( // create_fixed_joints(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &mut impulse_joints,
// &mut multibody_joints,
// point![0.0, 10.0, 0.0], // point![0.0, 10.0, 0.0],
// 10, // 10,
// use_articulations,
// ); // );
create_actuated_revolute_joints( // create_actuated_revolute_joints(
&mut bodies,
&mut colliders,
&mut impulse_joints,
point![20.0, 10.0, 0.0],
6,
);
// create_actuated_ball_joints(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &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], // point![13.0, 10.0, 0.0],
// 3, // 3,
// use_articulations,
// ); // );
// create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); // create_spherical_joints(
// create_ball_joints_with_limits(
// &mut bodies, // &mut bodies,
// &mut colliders, // &mut colliders,
// &mut impulse_joints, // &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], // 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.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]); 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)
}

28
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -195,7 +195,7 @@ impl JointVelocityConstraint<Real, 1> {
} }
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint. // TODO: coupled linear motor constraint.
// out[len] = builder.motor_linear_coupled( // out[len] = builder.motor_linear_coupled(
// params, // params,
// [joint_id], // [joint_id],
@@ -207,6 +207,7 @@ impl JointVelocityConstraint<Real, 1> {
// ); // );
// len += 1; // len += 1;
} }
JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]); JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
let start = len; let start = len;
@@ -260,12 +261,21 @@ impl JointVelocityConstraint<Real, 1> {
} }
} }
#[cfg(feature = "dim3")]
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint. out[len] = builder.limit_angular_coupled(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
} }
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// TODO: coupled linear limit constraint.
out[len] = builder.limit_linear_coupled( out[len] = builder.limit_linear_coupled(
params, params,
[joint_id], [joint_id],
@@ -593,8 +603,18 @@ impl JointVelocityGroundConstraint<Real, 1> {
} }
} }
#[cfg(feature = "dim3")]
if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
// TODO: coupled angular limit constraint. out[len] = builder.limit_angular_coupled_ground(
params,
[joint_id],
body1,
body2,
limit_axes & coupled_axes,
&joint.limits,
WritebackId::Limit(0), // TODO: writeback
);
len += 1;
} }
if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {

154
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -5,12 +5,13 @@ use crate::dynamics::solver::joint_constraint::SolverBody;
use crate::dynamics::solver::MotorParameters; use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits}; use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use crate::utils::{IndexMut2, WBasis, WCross, WCrossMatrix, WDot, WQuat, WReal};
use na::SMatrix; use na::SMatrix;
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct JointVelocityConstraintBuilder<N: WReal> { pub struct JointVelocityConstraintBuilder<N: WReal> {
pub basis: Matrix<N>, pub basis: Matrix<N>,
pub basis2: Matrix<N>, // TODO: used for angular coupling. Can we avoid storing this?
pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>, pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>, pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,
pub ang_basis: SMatrix<N, ANG_DIM, ANG_DIM>, pub ang_basis: SMatrix<N, ANG_DIM, ANG_DIM>,
@@ -66,6 +67,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
Self { Self {
basis, basis,
basis2: frame2.rotation.to_rotation_matrix().into_inner(),
cmat1_basis: cmat1 * basis, cmat1_basis: cmat1 * basis,
cmat2_basis: cmat2 * basis, cmat2_basis: cmat2 * basis,
ang_basis, ang_basis,
@@ -967,3 +969,153 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
} }
} }
} }
impl JointVelocityConstraintBuilder<Real> {
// TODO: this method is almost identical to the ground version, except for the
// return type. Could they share their implementation somehow?
#[cfg(feature = "dim3")]
pub fn limit_angular_coupled(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; 1],
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
limited_coupled_axes: u8,
limits: &[JointLimits<Real>],
writeback_id: WritebackId,
) -> JointVelocityConstraint<Real, 1> {
// NOTE: right now, this only supports exactly 2 coupled axes.
let ang_coupled_axes = limited_coupled_axes >> DIM;
assert_eq!(ang_coupled_axes.count_ones(), 2);
let not_coupled_index = ang_coupled_axes.trailing_ones() as usize;
let axis1 = self.basis.column(not_coupled_index).into_owned();
let axis2 = self.basis2.column(not_coupled_index).into_owned();
let rot = Rotation::rotation_between(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let (ang_jac, angle) = rot
.axis_angle()
.map(|(axis, angle)| (axis.into_inner(), angle))
.unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0));
let mut ang_limits = [0.0, 0.0];
for k in 0..3 {
if (ang_coupled_axes & (1 << k)) != 0 {
let limit = &limits[DIM + k];
ang_limits[0] += limit.min * limit.min;
ang_limits[1] += limit.max * limit.max;
}
}
ang_limits[0] = ang_limits[0].sqrt();
ang_limits[1] = ang_limits[1].sqrt();
let min_enabled = angle <= ang_limits[0];
let max_enabled = ang_limits[1] <= angle;
let impulse_bounds = [
if min_enabled { -Real::INFINITY } else { 0.0 },
if max_enabled { Real::INFINITY } else { 0.0 },
];
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias =
((angle - ang_limits[1]).max(0.0) - (ang_limits[0] - angle).max(0.0)) * erp_inv_dt;
let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac;
JointVelocityConstraint {
joint_id,
mj_lambda1: body1.mj_lambda,
mj_lambda2: body2.mj_lambda,
im1: body1.im,
im2: body2.im,
impulse: 0.0,
impulse_bounds,
lin_jac: na::zero(),
ang_jac1,
ang_jac2,
inv_lhs: 0.0, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: 0.0,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
}
}
#[cfg(feature = "dim3")]
pub fn limit_angular_coupled_ground(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; 1],
body1: &SolverBody<Real, 1>,
body2: &SolverBody<Real, 1>,
limited_coupled_axes: u8,
limits: &[JointLimits<Real>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<Real, 1> {
// NOTE: right now, this only supports exactly 2 coupled axes.
let ang_coupled_axes = limited_coupled_axes >> DIM;
assert_eq!(ang_coupled_axes.count_ones(), 2);
let not_coupled_index = ang_coupled_axes.trailing_ones() as usize;
let axis1 = self.basis.column(not_coupled_index).into_owned();
let axis2 = self.basis2.column(not_coupled_index).into_owned();
let rot = Rotation::rotation_between(&axis1, &axis2).unwrap_or_else(Rotation::identity);
let (ang_jac, angle) = rot
.axis_angle()
.map(|(axis, angle)| (axis.into_inner(), angle))
.unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0));
let mut ang_limits = [0.0, 0.0];
for k in 0..3 {
if (ang_coupled_axes & (1 << k)) != 0 {
let limit = &limits[DIM + k];
ang_limits[0] += limit.min * limit.min;
ang_limits[1] += limit.max * limit.max;
}
}
ang_limits[0] = ang_limits[0].sqrt();
ang_limits[1] = ang_limits[1].sqrt();
let min_enabled = angle <= ang_limits[0];
let max_enabled = ang_limits[1] <= angle;
let impulse_bounds = [
if min_enabled { -Real::INFINITY } else { 0.0 },
if max_enabled { Real::INFINITY } else { 0.0 },
];
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel;
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias =
((angle - ang_limits[1]).max(0.0) - (ang_limits[0] - angle).max(0.0)) * erp_inv_dt;
let ang_jac2 = body2.sqrt_ii * ang_jac;
JointVelocityGroundConstraint {
joint_id,
mj_lambda2: body2.mj_lambda,
im2: body2.im,
impulse: 0.0,
impulse_bounds,
lin_jac: na::zero(),
ang_jac2,
inv_lhs: 0.0, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: 0.0,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
writeback_id,
}
}
}