k/
funcs.rs

Help
1use super::chain::*;
2use super::joint::*;
3use na::{DMatrix, RealField, Vector3};
4use nalgebra as na;
5use simba::scalar::SubsetOf;
6
7/// Calculate Jacobian of the serial chain (manipulator).
8pub fn jacobian<T>(arm: &SerialChain<T>) -> DMatrix<T>
9where
10    T: RealField + SubsetOf<f64>,
11{
12    let dof = arm.dof();
13    let t_n = arm.end_transform();
14    arm.update_transforms();
15    let p_n = t_n.translation;
16    let jacobi_vec = arm
17        .iter_joints()
18        .map(|joint| {
19            let t_i = joint.world_transform().unwrap();
20            match &joint.joint_type {
21                JointType::Linear { axis } => {
22                    let p_i = t_i.rotation * axis;
23                    [
24                        p_i[0].clone(),
25                        p_i[1].clone(),
26                        p_i[2].clone(),
27                        na::zero(),
28                        na::zero(),
29                        na::zero(),
30                    ]
31                }
32                JointType::Rotational { axis } => {
33                    let p_i = t_i.translation;
34                    let a_i = t_i.rotation * axis;
35                    let dp_i = a_i.cross(&(p_n.clone().vector - p_i.vector));
36                    [
37                        dp_i[0].clone(),
38                        dp_i[1].clone(),
39                        dp_i[2].clone(),
40                        a_i[0].clone(),
41                        a_i[1].clone(),
42                        a_i[2].clone(),
43                    ]
44                }
45                JointType::Fixed => panic!("impossible, bug of jacobian"),
46            }
47        })
48        .collect::<Vec<_>>();
49    // Pi: a_i x (p_n - Pi)
50    // wi: a_i
51    DMatrix::from_fn(6, dof, |r, c| jacobi_vec[c][r].clone())
52}
53
54/// Calculate the center of mass of the chain
55///
56/// ```
57/// use k::*;
58/// use k::link::*;
59///
60/// let j0 = NodeBuilder::new()
61///     .translation(Translation3::new(0.0, 1.0, 0.0))
62///     .into_node();
63/// let j1 = NodeBuilder::new()
64///     .translation(Translation3::new(0.0, 0.0, 1.0))
65///     .into_node();
66/// j0.set_link(Some(LinkBuilder::new().inertial(Inertial::from_mass(1.0)).finalize()));
67/// j1.set_link(Some(LinkBuilder::new().inertial(Inertial::from_mass(4.0)).finalize()));
68/// j1.set_parent(&j0);
69/// let tree = Chain::from_root(j0);
70/// let com1 = center_of_mass(&tree);
71/// ```
72pub fn center_of_mass<T>(chain: &Chain<T>) -> Vector3<T>
73where
74    T: RealField + SubsetOf<f64>,
75{
76    let mut total_mass = T::zero();
77    let mut com = Vector3::zeros();
78
79    chain.update_transforms();
80    chain.iter().for_each(|node| {
81        if let Some(trans) = node.world_transform() {
82            if let Some(ref link) = *node.link() {
83                let inertia_trans = trans * link.inertial.origin().translation.clone();
84                com += inertia_trans.translation.vector * link.inertial.mass.clone();
85                total_mass += link.inertial.mass.clone();
86            }
87        }
88    });
89    com / total_mass
90}
91
92#[cfg(test)]
93mod tests {
94    use super::*;
95    use crate::link::*;
96    use crate::node::*;
97    use na::*;
98    #[cfg(target_family = "wasm")]
99    use wasm_bindgen_test::wasm_bindgen_test as test;
100
101    #[test]
102    fn test_update_center_of_mass() {
103        let j0 = NodeBuilder::new()
104            .translation(Translation3::new(0.0, 1.0, 0.0))
105            .into_node();
106        let j1 = NodeBuilder::new()
107            .translation(Translation3::new(0.0, 0.0, 1.0))
108            .joint_type(JointType::Rotational {
109                axis: Vector3::y_axis(),
110            })
111            .into_node();
112        j0.set_link(Some(
113            LinkBuilder::new()
114                .name("l0")
115                .inertial(Inertial::new(Isometry3::identity(), 1.0, Matrix3::zeros()))
116                .finalize(),
117        ));
118        let mut i1 = Inertial::new(Isometry3::identity(), 4.0, Matrix3::zeros());
119        i1.set_origin(Isometry3::from_parts(
120            Translation3::new(0.0, 0.0, 1.0),
121            UnitQuaternion::identity(),
122        ));
123        j1.set_link(Some(LinkBuilder::new().name("l1").inertial(i1).finalize()));
124        j1.set_parent(&j0);
125        let tree = Chain::from_root(j0);
126        let com1 = center_of_mass(&tree);
127        assert!((com1.x - 0.0f64).abs() < f64::EPSILON);
128        assert!((com1.y - 1.0f64).abs() < f64::EPSILON);
129        assert!((com1.z - 1.6f64).abs() < f64::EPSILON);
130        j1.set_joint_position(0.5).unwrap();
131        let com2 = center_of_mass(&tree);
132        assert!((com2.x - 0.383540).abs() < 0.0001);
133        assert!((com2.y - 1.0f64).abs() < f64::EPSILON);
134        assert!((com2.z - 1.502066).abs() < 0.0001);
135    }
136}
k/funcs.rs

k/
funcs.rs
