arcos_kdl/
chains.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::joint::JointType;
18use crate::segment::Segment;
19use nalgebra::DMatrix;
20use serde::{Deserialize, Serialize};
21
22/// Implementation of kinematic chains
23///
24/// segments: a vector with the segments of the chain\
25/// num_joints: number of joints (segments with movable joints)\
26/// num_ind_joints: Number of joints with independent movement\
27/// num_unlocked_joints: Num of enabled/unlocked joints\
28/// locked_joints: vector with bools indicating if the joint is locked\
29/// coupling_matrix: matrix correlating the joints with their couples\
30#[derive(Clone, Debug, Deserialize, Serialize)]
31pub struct Chain {
32    segments: Vec<Segment>,
33    num_joints: usize,
34    num_ind_joints: usize,
35    num_unlocked_joints: usize,
36    locked_joints: Vec<bool>,
37    coupling_matrix: DMatrix<f64>,
38}
39
40impl Chain {
41    /// Default empty chain
42    pub fn default() -> Chain {
43        Chain {
44            segments: Vec::new(),
45            num_joints: 0,
46            num_ind_joints: 0,
47            num_unlocked_joints: 0,
48            locked_joints: vec![],
49            coupling_matrix: DMatrix::identity(0, 0),
50        }
51    }
52
53    /// Returns the total number of segments
54    pub fn get_num_segments(&self) -> usize {
55        self.segments.len()
56    }
57
58    /// Returns the number of segments with moveable joints
59    pub fn get_num_joints(&self) -> usize {
60        self.num_joints
61    }
62
63    /// Returns the number of Independent moveable joints
64    pub fn get_num_ind_joints(&self) -> usize {
65        self.num_ind_joints
66    }
67
68    /// Returns the coupling matrix
69    pub fn get_coupling_matrix(&self) -> DMatrix<f64> {
70        self.coupling_matrix.clone()
71    }
72
73    /// Add a new semgment at the end of the kinematic chain\
74    /// (Where the end is where the end-effector would be)
75    pub fn add_segment(&mut self, segment: Segment) {
76        self.segments.push(segment);
77        if segment.get_joint_type() != JointType::NoJoint {
78            self.num_joints += 1;
79            self.locked_joints.push(false);
80            self.num_unlocked_joints += 1;
81            self.num_ind_joints = self.num_unlocked_joints;
82            self.coupling_matrix = DMatrix::identity(self.num_unlocked_joints, self.num_ind_joints);
83        }
84    }
85
86    /// Append an entire chain at the end of this chain
87    pub fn add_chain(&mut self, mut chain: Chain) {
88        self.segments.append(&mut chain.segments);
89        self.num_joints += chain.num_joints;
90    }
91
92    /// Get the segment at the index-th position
93    pub fn get_segment(&self, index: usize) -> Segment {
94        self.segments[index]
95    }
96
97    /// Get the vector with lock information
98    pub fn get_locked_joints(&self) -> Vec<bool> {
99        self.locked_joints.clone()
100    }
101
102    /// Set coupling information, locked joints and coupling relations
103    pub fn set_coupling(
104        &mut self,
105        new_locked_joints: Vec<bool>,
106        new_coupling_matrix: DMatrix<f64>,
107    ) {
108        let new_num_unlocked_joints = new_locked_joints.iter().filter(|&n| !(*n)).count();
109        if new_coupling_matrix.nrows() != new_num_unlocked_joints {
110            panic!(
111                "Wrong size of coupling arguments. Matrix has {} rows,
112                   but the vector says there should be {}",
113                new_coupling_matrix.nrows(),
114                new_num_unlocked_joints
115            );
116        }
117        self.locked_joints = new_locked_joints;
118        self.num_unlocked_joints = new_num_unlocked_joints;
119        self.num_ind_joints = new_coupling_matrix.ncols();
120        self.coupling_matrix = new_coupling_matrix;
121    }
122}
123
124impl PartialEq for Chain {
125    fn eq(&self, other: &Self) -> bool {
126        (self.segments == other.segments)
127            && (self.num_joints == other.num_joints)
128            && (self.num_ind_joints == other.num_ind_joints)
129            && (self.num_unlocked_joints == other.num_unlocked_joints)
130            && (self.coupling_matrix == other.coupling_matrix)
131    }
132}
133
134#[cfg(test)]
135/// Testing module for chains
136pub mod tests {
137    use crate::chains::Chain;
138    use crate::geometry::{EulerBuild, Frame};
139    use crate::joint::{Joint, JointType};
140    use crate::segment::Segment;
141
142    /// Create a chain for testing purposes, it is something similar to the
143    /// kinematic chain of KUKA LWR4PLUS
144    pub fn create_testing_chain() -> Chain {
145        let mut chain = Chain::default();
146        // Base segment
147        chain.add_segment(Segment::new(
148            Joint::default().set_type(JointType::NoJoint),
149            Frame::from_translation_euler(0.1, -0.2, 0.3, -0.1, -0.2, 0.3),
150            0.0,
151        ));
152        // Segment 0
153        chain.add_segment(Segment::new(
154            Joint::default().set_type(JointType::RotZ),
155            Frame::from_translation_euler(0.2, 0.5, 0.1, 0.58, -1.2, 2.3),
156            0.0,
157        ));
158        // Segment 1
159        chain.add_segment(Segment::new(
160            Joint::default().set_type(JointType::RotZ),
161            Frame::from_translation_euler(0.02, 0.3, -0.81, 3.0, -1.7, 1.3),
162            0.0,
163        ));
164        // Segment 2
165        chain.add_segment(Segment::new(
166            Joint::default().set_type(JointType::RotZ),
167            Frame::from_translation_euler(0.4, -0.27, 0.19, 1.57, -1.57, 0.0),
168            0.0,
169        ));
170        // Segment 3
171        chain.add_segment(Segment::new(
172            Joint::default().set_type(JointType::RotZ),
173            Frame::from_translation_euler(0.24, -0.18, 0.16, 0.01, 0.0, -0.01),
174            0.0,
175        ));
176        // Segment 4
177        chain.add_segment(Segment::new(
178            Joint::default().set_type(JointType::RotZ),
179            Frame::from_translation_euler(1.0, -0.5, -0.01, 0.05, -1.02, 0.34),
180            0.0,
181        ));
182        // Segment 5
183        chain.add_segment(Segment::new(
184            Joint::default().set_type(JointType::RotZ),
185            Frame::from_translation_euler(0.02, 0.58, -0.87, 0.95, -1.74, 0.68),
186            0.0,
187        ));
188        // Segment 6
189        chain.add_segment(Segment::new(
190            Joint::default().set_type(JointType::RotZ),
191            Frame::from_translation_euler(0.2, 0.1, -0.1, -0.76, -0.94, 0.25),
192            0.0,
193        ));
194        chain
195    }
196}
arcos_kdl/chains.rs

arcos_kdl/
chains.rs
