franka/model.rs
1// Copyright (c) 2021 Marco Boneberger
2// Licensed under the EUPL-1.2-or-later
3
4//! Contains model library types.
5use nalgebra::Matrix4;
6
7use crate::model::model_library::ModelLibrary;
8use crate::robot::robot_state::RobotState;
9use crate::FrankaResult;
10use std::fmt;
11use std::path::Path;
12
13pub(crate) mod library_downloader;
14mod model_library;
15
16/// Enumerates the seven joints, the flange, and the end effector of a robot.
17pub enum Frame {
18 Joint1,
19 Joint2,
20 Joint3,
21 Joint4,
22 Joint5,
23 Joint6,
24 Joint7,
25 Flange,
26 EndEffector,
27 Stiffness,
28}
29
30impl fmt::Display for Frame {
31 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
32 match self {
33 Frame::Joint1 => {
34 write!(f, "Joint 1")
35 }
36 Frame::Joint2 => {
37 write!(f, "Joint 2")
38 }
39 Frame::Joint3 => {
40 write!(f, "Joint 3")
41 }
42 Frame::Joint4 => {
43 write!(f, "Joint 4")
44 }
45 Frame::Joint5 => {
46 write!(f, "Joint 5")
47 }
48 Frame::Joint6 => {
49 write!(f, "Joint 6")
50 }
51 Frame::Joint7 => {
52 write!(f, "Joint 7")
53 }
54 Frame::Flange => {
55 write!(f, "Flange")
56 }
57 Frame::EndEffector => {
58 write!(f, "End-Effector")
59 }
60 Frame::Stiffness => {
61 write!(f, "Stiffness")
62 }
63 }
64 }
65}
66
67/// Calculates poses of joints and dynamic properties of the robot.
68pub struct Model {
69 library: ModelLibrary,
70}
71
72#[allow(non_snake_case)]
73impl Model {
74 /// Creates a new Model instance from the shared object of the Model for offline usage.
75 ///
76 /// If you just want to use the model to control the Robot you should use
77 /// [`Robot::load_model`](`crate::Robot::load_model`).
78 ///
79 /// If you do not have the model you can use the download_model example to download the model
80 /// # Arguments
81 /// * `model_filename` - Path to the model.
82 /// * `libm_filename` - Path to libm.so.6 . You probably do not need to specify the path as it
83 /// it should be detected automatically. However if you get panics for unresolved symbols or libm could
84 /// not be found you have to specify the path. On most Ubuntu systems it is in
85 /// ```text
86 /// /lib/x86_64-linux-gnu/libm.so.6
87 /// ```
88 /// It maybe has a different name on your machine but it is not the "libm.so". You can
89 /// verify that it is the correct libm by checking:
90 /// ```bash
91 /// nm -D /lib/x86_64-linux-gnu/libm.so.6 | grep sincos
92 /// ```
93 /// # Errors
94 /// * ModelException
95 /// # libm FAQ
96 /// What is libm? - Libm is the Math library of the C Standard Library. It is what you get when
97 /// you include <math.h> in C
98 ///
99 /// Why do we need it? - Because the model is not self contained and relies on some functions from libm
100 ///
101 /// How does the libfranka embed libm? They are not including <math.h> - Apparently it gets somehow included when using \<array> ¯\_(ツ)_/¯
102 pub fn new<S: AsRef<Path>>(
103 model_filename: S,
104 libm_filename: Option<&Path>,
105 ) -> FrankaResult<Self> {
106 Ok(Model {
107 library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?,
108 })
109 }
110 /// Gets the 4x4 pose matrix for the given frame in base frame.
111 ///
112 /// The pose is represented as a 4x4 matrix in column-major format.
113 /// # Arguments
114 /// * `frame` - The desired frame.
115 /// * `q` - Joint position.
116 /// * `F_T_EE` - End effector in flange frame.
117 /// * `EE_T_K` - Stiffness frame K in the end effector frame.
118 /// # Return
119 /// Vectorized 4x4 pose matrix, column-major.
120 pub fn pose(
121 &self,
122 frame: &Frame,
123 q: &[f64; 7],
124 F_T_EE: &[f64; 16],
125 EE_T_K: &[f64; 16],
126 ) -> [f64; 16] {
127 let mut output = [0.; 16];
128 match frame {
129 Frame::Joint1 => self.library.joint1(q, &mut output),
130 Frame::Joint2 => self.library.joint2(q, &mut output),
131 Frame::Joint3 => self.library.joint3(q, &mut output),
132 Frame::Joint4 => self.library.joint4(q, &mut output),
133 Frame::Joint5 => self.library.joint5(q, &mut output),
134 Frame::Joint6 => self.library.joint6(q, &mut output),
135 Frame::Joint7 => self.library.joint7(q, &mut output),
136 Frame::Flange => self.library.flange(q, &mut output),
137 Frame::EndEffector => self.library.ee(q, F_T_EE, &mut output),
138 Frame::Stiffness => {
139 let tmp: Matrix4<f64> =
140 Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K);
141 let mut stiffness_f_t_ee = [0.; 16];
142 tmp.iter()
143 .enumerate()
144 .for_each(|(i, &x)| stiffness_f_t_ee[i] = x);
145 self.library.ee(q, &stiffness_f_t_ee, &mut output)
146 }
147 }
148 output
149 }
150 /// Gets the 4x4 pose matrix for the given frame in base frame.
151 ///
152 /// The pose is represented as a 4x4 matrix in column-major format.
153 /// # Arguments
154 /// * `frame` - The desired frame.
155 /// * `robot_state` - State from which the pose should be calculated.
156 /// # Return
157 /// Vectorized 4x4 pose matrix, column-major.
158 pub fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] {
159 self.pose(
160 frame,
161 &robot_state.q,
162 &robot_state.F_T_EE,
163 &robot_state.EE_T_K,
164 )
165 }
166 /// Gets the 6x7 Jacobian for the given frame, relative to that frame.
167 ///
168 /// The Jacobian is represented as a 6x7 matrix in column-major format.
169 /// # Arguments
170 /// * `frame` - The desired frame.
171 /// * `q` - Joint position.
172 /// * `F_T_EE` - End effector in flange frame.
173 /// * `EE_T_K` - Stiffness frame K in the end effector frame.
174 /// # Return
175 /// Vectorized 6x7 Jacobian, column-major.
176 pub fn body_jacobian(
177 &self,
178 frame: &Frame,
179 q: &[f64; 7],
180 F_T_EE: &[f64; 16],
181 EE_T_K: &[f64; 16],
182 ) -> [f64; 42] {
183 let mut output = [0.; 42];
184 match frame {
185 Frame::Joint1 => self.library.body_jacobian_joint1(&mut output),
186 Frame::Joint2 => self.library.body_jacobian_joint2(q, &mut output),
187 Frame::Joint3 => self.library.body_jacobian_joint3(q, &mut output),
188 Frame::Joint4 => self.library.body_jacobian_joint4(q, &mut output),
189 Frame::Joint5 => self.library.body_jacobian_joint5(q, &mut output),
190 Frame::Joint6 => self.library.body_jacobian_joint6(q, &mut output),
191 Frame::Joint7 => self.library.body_jacobian_joint7(q, &mut output),
192 Frame::Flange => self.library.body_jacobian_flange(q, &mut output),
193 Frame::EndEffector => self.library.body_jacobian_ee(q, F_T_EE, &mut output),
194 Frame::Stiffness => {
195 let tmp: Matrix4<f64> =
196 Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K);
197 let mut stiffness_f_t_ee = [0.; 16];
198 tmp.iter()
199 .enumerate()
200 .for_each(|(i, &x)| stiffness_f_t_ee[i] = x);
201 self.library
202 .body_jacobian_ee(q, &stiffness_f_t_ee, &mut output)
203 }
204 }
205 output
206 }
207
208 /// Gets the 6x7 Jacobian for the given frame, relative to that frame.
209 ///
210 /// The Jacobian is represented as a 6x7 matrix in column-major format.
211 /// # Arguments
212 /// * `frame` - The desired frame.
213 /// * `robot_state` - State from which the pose should be calculated.
214 /// # Return
215 /// Vectorized 6x7 Jacobian, column-major.
216 pub fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] {
217 self.body_jacobian(
218 frame,
219 &robot_state.q,
220 &robot_state.F_T_EE,
221 &robot_state.EE_T_K,
222 )
223 }
224 /// Gets the 6x7 Jacobian for the given joint relative to the base frame.
225 ///
226 /// The Jacobian is represented as a 6x7 matrix in column-major format.
227 /// # Arguments
228 /// * `frame` - The desired frame.
229 /// * `q` - Joint position.
230 /// * `F_T_EE` - End effector in flange frame.
231 /// * `EE_T_K` - Stiffness frame K in the end effector frame.
232 /// # Return
233 /// Vectorized 6x7 Jacobian, column-major.
234 pub fn zero_jacobian(
235 &self,
236 frame: &Frame,
237 q: &[f64; 7],
238 F_T_EE: &[f64; 16],
239 EE_T_K: &[f64; 16],
240 ) -> [f64; 42] {
241 let mut output = [0.; 42];
242 match frame {
243 Frame::Joint1 => self.library.zero_jacobian_joint1(&mut output),
244 Frame::Joint2 => self.library.zero_jacobian_joint2(q, &mut output),
245 Frame::Joint3 => self.library.zero_jacobian_joint3(q, &mut output),
246 Frame::Joint4 => self.library.zero_jacobian_joint4(q, &mut output),
247 Frame::Joint5 => self.library.zero_jacobian_joint5(q, &mut output),
248 Frame::Joint6 => self.library.zero_jacobian_joint6(q, &mut output),
249 Frame::Joint7 => self.library.zero_jacobian_joint7(q, &mut output),
250 Frame::Flange => self.library.zero_jacobian_flange(q, &mut output),
251 Frame::EndEffector => self.library.zero_jacobian_ee(q, F_T_EE, &mut output),
252 Frame::Stiffness => {
253 let tmp: Matrix4<f64> =
254 Matrix4::from_column_slice(F_T_EE) * Matrix4::from_column_slice(EE_T_K);
255 let mut stiffness_f_t_ee = [0.; 16];
256 tmp.iter()
257 .enumerate()
258 .for_each(|(i, &x)| stiffness_f_t_ee[i] = x);
259 self.library
260 .zero_jacobian_ee(q, &stiffness_f_t_ee, &mut output)
261 }
262 }
263 output
264 }
265 /// Gets the 6x7 Jacobian for the given joint relative to the base frame.
266 ///
267 /// The Jacobian is represented as a 6x7 matrix in column-major format.
268 /// # Arguments
269 /// * `frame` - The desired frame.
270 /// * `robot_state` - State from which the pose should be calculated.
271 /// # Return
272 /// Vectorized 6x7 Jacobian, column-major.
273 pub fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] {
274 self.zero_jacobian(
275 frame,
276 &robot_state.q,
277 &robot_state.F_T_EE,
278 &robot_state.EE_T_K,
279 )
280 }
281 /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2].
282 /// # Arguments
283 /// * `q` - Joint position.
284 /// * `I_total` - Inertia of the attached total load including end effector, relative to
285 /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2].
286 /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\].
287 /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\].
288 /// # Return
289 /// Vectorized 7x7 mass matrix, column-major.
290 pub fn mass(
291 &self,
292 q: &[f64; 7],
293 I_total: &[f64; 9],
294 m_total: f64,
295 F_x_Ctotal: &[f64; 3],
296 ) -> [f64; 49] {
297 let mut output = [0.; 49];
298 self.library
299 .mass(q, I_total, &m_total, F_x_Ctotal, &mut output);
300 output
301 }
302 /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2].
303 /// # Arguments
304 /// * `robot_state` - State from which the mass matrix should be calculated.
305 /// # Return
306 /// Vectorized 7x7 mass matrix, column-major.
307 pub fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] {
308 self.mass(
309 &robot_state.q,
310 &robot_state.I_total,
311 robot_state.m_total,
312 &robot_state.F_x_Ctotal,
313 )
314 }
315 /// Calculates the Coriolis force vector (state-space equation):
316 /// , in \[Nm\].
317 /// # Arguments
318 /// * `q` - Joint position.
319 /// * `dq` - Joint velocity.
320 /// * `I_total` - Inertia of the attached total load including end effector, relative to
321 /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2].
322 /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\].
323 /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\].
324 /// # Return
325 /// Coriolis force vector.
326 pub fn coriolis(
327 &self,
328 q: &[f64; 7],
329 dq: &[f64; 7],
330 I_total: &[f64; 9],
331 m_total: f64,
332 F_x_Ctotal: &[f64; 3],
333 ) -> [f64; 7] {
334 let mut output = [0.; 7];
335 self.library
336 .coriolis(q, dq, I_total, &m_total, F_x_Ctotal, &mut output);
337 output
338 }
339
340 /// Calculates the Coriolis force vector (state-space equation):
341 /// , in \[Nm\].
342 /// # Arguments
343 /// * `robot_state` - State from which the Coriolis force vector should be calculated.
344 /// # Return
345 /// Coriolis force vector.
346 pub fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] {
347 self.coriolis(
348 &robot_state.q,
349 &robot_state.dq,
350 &robot_state.I_load,
351 robot_state.m_total,
352 &robot_state.F_x_Ctotal,
353 )
354 }
355 /// Calculates the gravity vector. Unit: \[Nm\].
356 /// # Arguments
357 /// * `q` - Joint position.
358 /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\].
359 /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\].
360 /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2]
361 /// Default to [0.0, 0.0, -9.81].
362 /// # Return
363 /// Gravity vector.
364 pub fn gravity<'a, Grav: Into<Option<&'a [f64; 3]>>>(
365 &self,
366 q: &[f64; 7],
367 m_total: f64,
368 F_x_Ctotal: &[f64; 3],
369 gravity_earth: Grav,
370 ) -> [f64; 7] {
371 let gravity_earth = gravity_earth.into().unwrap_or(&[0., 0., -9.81]);
372 let mut output = [0.; 7];
373 self.library
374 .gravity(q, gravity_earth, &m_total, F_x_Ctotal, &mut output);
375 output
376 }
377 /// Calculates the gravity vector. Unit: \[Nm\].
378 /// # Arguments
379 /// * `robot_state` - State from which the gravity vector should be calculated.
380 /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2].
381 /// By default `gravity_earth` will be calculated from [`RobotState::O_ddP_O`](`crate::RobotState::O_ddP_O`).
382 /// # Return
383 /// Gravity vector.
384 pub fn gravity_from_state<'a, Grav: Into<Option<&'a [f64; 3]>>>(
385 &self,
386 robot_state: &RobotState,
387 gravity_earth: Grav,
388 ) -> [f64; 7] {
389 self.gravity(
390 &robot_state.q,
391 robot_state.m_total,
392 &robot_state.F_x_Ctotal,
393 gravity_earth.into().unwrap_or(&robot_state.O_ddP_O),
394 )
395 }
396}