arcos_kdl/
segment.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::geometry::{get_frame_error, invert_frame, new_ref_point, Frame, Introspection, Twist};
18use crate::joint::{Joint, JointType};
19use serde::{Deserialize, Serialize};
20
21/// This is a placeholder, as we are interested only in kinematics, we are not
22/// implementing dynamics yet.
23pub type RigidBodyInertia = f64;
24
25/// Implementation of a kinematic chian segment. This is the joint together with
26/// the link.
27///
28/// It is represented by the joint, a frame that transforms from the joint to the
29/// end of the link, and an inertia parameter.
30#[derive(Copy, Clone, Debug, Deserialize, Serialize)]
31pub struct Segment {
32    joint: Joint,
33    f_tip: Frame,
34    inertia: RigidBodyInertia,
35}
36
37impl Segment {
38    /// Default segment, with the default joint and no link.
39    pub fn default() -> Segment {
40        Segment {
41            joint: Joint::default(),
42            f_tip: Frame::identity(),
43            inertia: 0.0,
44        }
45    }
46
47    /// Constructs a segment from the given `Joint`, link transformation
48    /// and inertia
49    pub fn new(joint: Joint, f_tip: Frame, inertia: RigidBodyInertia) -> Segment {
50        let frame = invert_frame(joint.pose(0.0)) * f_tip;
51        Segment {
52            joint: joint,
53            f_tip: frame,
54            inertia: inertia,
55        }
56    }
57
58    /// Creates a new segment with the given joint
59    pub fn set_joint(&self, new_joint: Joint) -> Segment {
60        Segment {
61            joint: new_joint,
62            f_tip: self.f_tip,
63            inertia: self.inertia,
64        }
65    }
66
67    /// Creates a new segment with the given link transformation
68    pub fn set_tip_frame(&self, new_f_tip: Frame) -> Segment {
69        let frame = invert_frame(self.joint.pose(0.0)) * new_f_tip;
70        Segment {
71            joint: self.joint,
72            f_tip: frame,
73            inertia: self.inertia,
74        }
75    }
76
77    /// Creates a new segment with the given inertia
78    ///
79    /// Note that right now this method is useless because dynamics are not
80    /// yet implemented
81    pub fn set_inertia(&self, new_inertia: RigidBodyInertia) -> Segment {
82        Segment {
83            joint: self.joint,
84            f_tip: self.f_tip,
85            inertia: new_inertia,
86        }
87    }
88
89    /// Returns the type of the internal joint
90    pub fn get_joint_type(&self) -> JointType {
91        self.joint.get_joint_type()
92    }
93
94    /// Returns the link transfrom
95    pub fn get_link_transform(&self) -> Frame {
96        self.f_tip
97    }
98
99    /// Get the pose at the tip of the segment given the state of the joint
100    pub fn pose(&self, joint_state: f64) -> Frame {
101        self.joint.pose(joint_state) * self.f_tip
102    }
103
104    /// Get the twist at the tip of the segment given the state of the joint
105    pub fn twist(&self, joint_state: f64, qdot: f64) -> Twist {
106        new_ref_point(
107            self.joint.twist(qdot),
108            self.joint.pose(joint_state).get_rotation() * self.f_tip.get_translation(),
109        )
110    }
111}
112
113impl PartialEq for Segment {
114    fn eq(&self, other: &Self) -> bool {
115        let threshold = 0.0000001;
116        let similar_tf = get_frame_error(self.f_tip, other.f_tip) < threshold;
117        (self.joint == other.joint) && (self.inertia == other.inertia) && similar_tf
118    }
119}
120
121#[cfg(test)]
122mod tests {
123    use crate::geometry::{get_frame_error, get_twist_error, Frame, Twist};
124    use crate::joint::{Joint, JointType};
125    use crate::segment::Segment;
126    use nalgebra::geometry::{Isometry3, Translation3, UnitQuaternion};
127
128    #[test]
129    fn calculate_pose() {
130        let joint = Joint::default()
131            .set_type(JointType::RotZ)
132            .set_offset(2.36)
133            .set_scale(3.5);
134        let f_tip = Isometry3::from_parts(
135            Translation3::new(0.1, -2.0, 0.5),
136            UnitQuaternion::from_euler_angles(1.0, 0.5, -0.4),
137        )
138        .to_homogeneous();
139        let joint_state = 2.1;
140        let segment = Segment::new(joint, f_tip, 0.0);
141        let result = Frame::new(
142            0.689601, -0.0171617, 0.723987, 1.79963, 0.542773, 0.674079, -0.501015, -0.878265,
143            -0.479426, 0.73846, 0.47416, 0.5, 0.0, 0.0, 0.0, 1.0,
144        );
145        let error = get_frame_error(result, segment.pose(joint_state));
146        assert!(error < 0.00001);
147    }
148
149    #[test]
150    fn calculate_twist() {
151        let joint = Joint::default()
152            .set_type(JointType::RotZ)
153            .set_offset(2.36)
154            .set_scale(3.5);
155        let f_tip = Isometry3::from_parts(
156            Translation3::new(0.1, -2.0, 0.5),
157            UnitQuaternion::from_euler_angles(1.0, 0.5, -0.4),
158        )
159        .to_homogeneous();
160        let joint_state = 2.1;
161        let qdot = 0.3;
162        let segment = Segment::new(joint, f_tip, 0.0);
163        let result = Twist::new(0.92217, 1.88961, 0.0, 0.0, 0.0, 1.05);
164        let error = get_twist_error(result, segment.twist(joint_state, qdot));
165        assert!(error < 0.001);
166    }
167}
arcos_kdl/segment.rs

arcos_kdl/
segment.rs
