use criterion::{black_box, criterion_group, criterion_main, Criterion};
use k::joint::Range;
use na::RealField;
use nalgebra as na;
use std::{f64::consts::PI, sync::Arc, thread};
const THREADS: usize = 4;
fn generate_random_joint_angles_from_limits<T>(limits: &[Option<k::joint::Range<T>>]) -> Vec<T>
where
T: RealField,
{
limits
.iter()
.map(|range| match range {
Some(range) => {
(range.max.clone() - range.min.clone()) * na::convert(rand::random())
+ range.min.clone()
}
None => na::convert::<f64, T>(rand::random::<f64>() - 0.5) * na::convert(2.0 * PI),
})
.collect()
}
fn bench_rctree(c: &mut Criterion) {
let chain = k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap();
let limits = chain
.iter_joints()
.map(|j| j.limits)
.collect::<Vec<Option<Range<f64>>>>();
let angles = generate_random_joint_angles_from_limits(&limits);
c.bench_function("bench_rctree", |b| {
b.iter(|| {
chain.set_joint_positions(&angles).unwrap();
let _trans = chain.update_transforms();
assert_eq!(_trans.len(), 13);
});
});
}
fn bench_rctree_set_joints(c: &mut Criterion) {
let chain = k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap();
let limits = chain
.iter_joints()
.map(|j| j.limits)
.collect::<Vec<Option<Range<f64>>>>();
let angles = generate_random_joint_angles_from_limits(&limits);
c.bench_function("bench_rctree_set_joints", |b| {
b.iter(|| {
chain.set_joint_positions(&angles).unwrap();
});
});
}
fn bench_rctree_get_joints(c: &mut Criterion) {
let chain = k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap();
c.bench_function("bench_rctree_get_joints", |b| {
b.iter(|| {
black_box(chain.joint_positions());
});
});
}
fn bench_rctree_concurrent_set_joints(c: &mut Criterion) {
let chain = Arc::new(k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap());
let limits = chain
.iter_joints()
.map(|j| j.limits)
.collect::<Vec<Option<Range<f64>>>>();
let angles = Arc::new(generate_random_joint_angles_from_limits(&limits));
c.bench_function("bench_rctree_concurrent_set_joints_1000", |b| {
b.iter(|| {
let mut handles = vec![];
for _ in 0..THREADS {
let chain = chain.clone();
let angles = angles.clone();
handles.push(thread::spawn(move || {
for _ in 0..1000 {
chain.set_joint_positions(&angles).unwrap();
}
}));
}
for handle in handles {
handle.join().unwrap();
}
});
});
}
fn bench_rctree_concurrent_get_joints(c: &mut Criterion) {
let chain = Arc::new(k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap());
c.bench_function("bench_rctree_concurrent_get_joints_1000", |b| {
b.iter(|| {
let mut handles = vec![];
for _ in 0..THREADS {
let chain = chain.clone();
handles.push(thread::spawn(move || {
for _ in 0..1000 {
black_box(chain.joint_positions());
}
}));
}
for handle in handles {
handle.join().unwrap();
}
});
});
}
fn bench_rctree_concurrent_set_get_joints(c: &mut Criterion) {
let chain = Arc::new(k::Chain::<f64>::from_urdf_file("urdf/sample.urdf").unwrap());
let limits = chain
.iter_joints()
.map(|j| j.limits)
.collect::<Vec<Option<Range<f64>>>>();
let angles = Arc::new(generate_random_joint_angles_from_limits(&limits));
c.bench_function("bench_rctree_concurrent_set_get_joints_1000", |b| {
b.iter(|| {
let mut handles = vec![];
for _ in 0..THREADS / 2 {
let chain = chain.clone();
handles.push(thread::spawn(move || {
for _ in 0..1000 {
black_box(chain.joint_positions());
}
}));
}
for _ in 0..THREADS / 2 {
let chain = chain.clone();
let angles = angles.clone();
handles.push(thread::spawn(move || {
for _ in 0..1000 {
chain.set_joint_positions(&angles).unwrap();
}
}));
}
for handle in handles {
handle.join().unwrap();
}
});
});
}
criterion_group!(
benches,
bench_rctree,
bench_rctree_get_joints,
bench_rctree_set_joints,
bench_rctree_concurrent_set_joints,
bench_rctree_concurrent_get_joints,
bench_rctree_concurrent_set_get_joints,
);
criterion_main!(benches);