arcos_kdl/
forward_diff_kinematics.rs

1// Copyright (c) 2019 Autonomous Robots and Cognitive Systems Laboratory
2// Author: Daniel Garcia-Vaglio <degv364@gmail.com>
3//
4// This program is free software: you can redistribute it and/or modify
5// it under the terms of the GNU General Public License as published by
6// the Free Software Foundation, either version 3 of the License, or
7// (at your option) any later version.
8//
9// This program is distributed in the hope that it will be useful,
10// but WITHOUT ANY WARRANTY; without even the implied warranty of
11// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12// GNU General Public License for more details.
13//
14// You should have received a copy of the GNU General Public License
15// along with this program. If not, see <http://www.gnu.org/licenses/>.
16
17use crate::chains::Chain;
18use crate::geometry::{Frame, Introspection, Twist};
19use crate::joint::JointType;
20
21/// Calculate the global twist of a segment given the global pose and twist
22/// of the previous segment, and the local pose and twist of the current
23/// segment
24pub fn reference_frame(
25    total_twist: Twist,
26    segment_twist: Twist,
27    total_pose: Frame,
28    segment_pose: Frame,
29) -> Twist {
30    let rotation = total_pose.get_rotation();
31    let rot = rotation * segment_twist.fixed_rows::<3>(3);
32    let tmp = rotation * segment_pose.get_translation();
33    let trans =
34        rotation * segment_twist.fixed_rows::<3>(0) + total_twist.fixed_rows::<3>(3).cross(&tmp);
35
36    Twist::new(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2])
37}
38
39/// Solver for forward differential kinematics. Calculate the velocity at any
40/// given joint from the current joint configuration and rate.
41#[derive(Clone, Debug)]
42pub struct ForwardDiffKinematicsSolver {
43    chain: Chain,
44}
45
46impl ForwardDiffKinematicsSolver {
47    /// Create a default empty solver
48    pub fn default() -> Self {
49        Self {
50            chain: Chain::default(),
51        }
52    }
53
54    /// Create a new solver from a given chain
55    pub fn new(in_chain: Chain) -> Self {
56        Self { chain: in_chain }
57    }
58
59    /// Solver for forward differential kinematics at an arbitrary segment.\
60    /// Computes the twist at the segment given the current chain state.
61    ///
62    /// `chain`: Kinematic chain to use by the solver\
63    /// `angles`: vector with the state at each joint\
64    /// `qdots`: vector with the differential state at each joint\
65    /// `segment_index`: at which segment to compute the velocity\
66    pub fn solve_at_segment(
67        &self,
68        angles: &Vec<f64>,
69        qdots: &Vec<f64>,
70        segment_index: usize,
71    ) -> Twist {
72        let num_joints = self.chain.get_num_joints();
73        let num_segments = self.chain.get_num_segments();
74        if num_joints != angles.len() {
75            panic!(
76                "Got {} angles, but the chain has {} joints",
77                angles.len(),
78                num_joints
79            );
80        }
81
82        if num_joints != qdots.len() {
83            panic!(
84                "Got {} angle differentials, but the chain has {} joints",
85                qdots.len(),
86                num_joints
87            );
88        }
89
90        if segment_index >= num_segments {
91            panic!(
92                "Asked for pose at segment {}, but the chain has {} segments",
93                segment_index, num_segments
94            );
95        }
96        let mut counter = 0;
97        let mut result_pose = Frame::identity();
98        let mut result_twist = Twist::zeros();
99
100        for i in 0..segment_index + 1 {
101            let segment = self.chain.get_segment(i);
102            let pose = segment.pose(angles[counter]);
103            let twist = segment.twist(angles[counter], qdots[counter]);
104            result_twist += reference_frame(result_twist, twist, result_pose, pose);
105            result_pose *= pose;
106            if segment.get_joint_type() != JointType::NoJoint {
107                counter += 1;
108            }
109        }
110        result_twist
111    }
112
113    /// Solver for forward differential kinematics at the end-effector\
114    /// Uses the arbitrary segment solver with end-effector as default segment
115    pub fn solve(&self, angles: &Vec<f64>, qdots: &Vec<f64>) -> Twist {
116        self.solve_at_segment(angles, qdots, self.chain.get_num_segments() - 1)
117    }
118}
119
120#[cfg(test)]
121mod test {
122    use crate::chains::tests::create_testing_chain;
123    use crate::forward_diff_kinematics::ForwardDiffKinematicsSolver;
124    use crate::geometry::{get_twist_error, Twist};
125
126    #[test]
127    fn base_twist_correct() {
128        let chain = create_testing_chain();
129        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
130        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
131        let solver = ForwardDiffKinematicsSolver::new(chain);
132        let base_twist = solver.solve_at_segment(&angles, &qdots, 0);
133        assert_eq!(base_twist, Twist::zeros())
134    }
135
136    #[test]
137    fn end_twist_correct() {
138        let chain = create_testing_chain();
139        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
140        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
141        let solver = ForwardDiffKinematicsSolver::new(chain);
142        let end_twist = solver.solve(&angles, &qdots);
143        let error = get_twist_error(
144            end_twist,
145            Twist::new(-0.423878, -1.01074, 0.123592, -2.28297, 0.209941, 1.80897),
146        );
147        assert!(error < 0.00001)
148    }
149
150    #[test]
151    fn middle_twist_correct() {
152        let chain = create_testing_chain();
153        let angles = vec![0.1, -0.95, 0.57, 0.68, -0.27, 0.39, 0.47];
154        let qdots = vec![1.0, -0.65, 0.87, 0.01, -0.41, -0.98, 0.24];
155        let solver = ForwardDiffKinematicsSolver::new(chain);
156        let middle_twist = solver.solve_at_segment(&angles, &qdots, 4);
157        let error = get_twist_error(
158            middle_twist,
159            Twist::new(-0.344006, 0.591387, 0.0751308, -1.1035, -0.0710371, 1.45781),
160        );
161        assert!(error < 0.00001)
162    }
163}
arcos_kdl/forward_diff_kinematics.rs

arcos_kdl/
forward_diff_kinematics.rs
