bevy_urdf 0.4.3

Import robots from URDF files and run simulation with rapier.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

use std::collections::HashMap;

#[derive(Debug, Clone)]
pub struct LinkTransform {
    pub position: Vector3<f32>,
    pub rotation: UnitQuaternion<f32>,
}

use nalgebra::{Point3, Quaternion, UnitQuaternion, Vector3};

// reinit_vector3 and reinit_quaternion reinitialize nalgebra types because k and urdf use different versions of nalgebra
pub fn reinit_vector3<T: Copy>(old_vec: &impl AsRef<[T; 3]>) -> Vector3<T> {
    let components = old_vec.as_ref();
    Vector3::new(components[0], components[1], components[2])
}

pub fn reinit_quaternion<T: Copy + nalgebra::RealField>(
    w: T,
    x: T,
    y: T,
    z: T,
) -> UnitQuaternion<T> {
    // Create quaternion from components (w, x, y, z)
    let quat = Quaternion::new(w, x, y, z);
    UnitQuaternion::from_quaternion(quat)
}

pub fn get_link_transforms(
    urdf_robot: &mut urdf_rs::Robot,
    isometry: nalgebra::Isometry<f32, nalgebra::Unit<nalgebra::Quaternion<f32>>, 3>,
) -> Result<HashMap<String, LinkTransform>, k::Error> {
    // Convert URDF to kinematic chain

    let robot: k::Chain<f32> = urdf_robot.clone().into();

    // Compute forward kinematics
    robot.update_transforms();

    let mut transforms = HashMap::new();

    // Get transform for each link
    for (_i, link) in robot.iter().enumerate() {
        let world_transform = link.world_transform().unwrap();
        let link_name = &link.link().clone().unwrap().name;

        // Extract position
        let position = reinit_vector3(&world_transform.translation.vector);
        let position = Point3::from(position);
        let position = isometry.transform_point(&position).coords;

        let rotation = reinit_quaternion(
            world_transform.rotation.w,
            world_transform.rotation.i,
            world_transform.rotation.j,
            world_transform.rotation.k,
        );
        let rotation = isometry.rotation * rotation;

        // urdf_robot.links[i].inertial.origin.xyz =
        // Vec3([position.x as f64, position.y as f64, position.z as f64]);
        // let rpy = rotation.euler_angles();
        // urdf_robot.links[i].inertial.origin.rpy = Vec3([rpy.0 as f64, rpy.1 as f64, rpy.2 as f64]);

        transforms.insert(link_name.clone(), LinkTransform { position, rotation });
    }

    Ok(transforms)
}