├── .github └── workflows │ └── rust.yml ├── .gitignore ├── .gitlab-ci.yml ├── Cargo.toml ├── LICENSE ├── README.md ├── examples ├── cartesian_impedance_control.rs ├── communication_test.rs ├── download_model.rs ├── echo_robot_state.rs ├── generate_cartesian_pose_motion.rs ├── generate_cartesian_velocity_motion.rs ├── generate_consecutive_motions.rs ├── generate_elbow_motion.rs ├── generate_joint_position_motion.rs ├── generate_joint_velocity_motion.rs ├── grasp_object.rs ├── mirror_robot.rs └── print_joint_poses.rs ├── rustfmt.toml └── src ├── exception.rs ├── gripper.rs ├── gripper ├── gripper_state.rs └── types.rs ├── lib.rs ├── model.rs ├── model ├── library_downloader.rs └── model_library.rs ├── network.rs ├── robot.rs ├── robot ├── control_loop.rs ├── control_tools.rs ├── control_types.rs ├── error.rs ├── errors.rs ├── logger.rs ├── low_pass_filter.rs ├── motion_generator_traits.rs ├── rate_limiting.rs ├── robot_control.rs ├── robot_impl.rs ├── robot_state.rs ├── service_types.rs ├── types.rs └── virtual_wall_cuboid.rs └── utils.rs /.github/workflows/rust.yml: -------------------------------------------------------------------------------- 1 | name: Rust 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | env: 10 | CARGO_TERM_COLOR: always 11 | 12 | jobs: 13 | build: 14 | 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - uses: actions/checkout@v2 19 | - name: Build 20 | run: cargo build --all-features --verbose 21 | - name: Check Format 22 | run: cargo fmt -- --check 23 | - name: Clippy 24 | run: cargo clippy --all-features -- -D warnings 25 | - name: Run tests 26 | run: cargo test --all-features --verbose -- --test-threads=1 27 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | Cargo.lock 3 | /.idea -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - build 3 | 4 | rust-latest: 5 | stage: build 6 | image: rust 7 | before_script: 8 | - rustup component add rustfmt 9 | - rustup component add clippy 10 | script: 11 | - cargo fmt -- --check 12 | - cargo clippy --all-features -- -D warnings 13 | - cargo build --all-features --verbose 14 | - cargo test --all-features --verbose -- --test-threads=1 15 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "libfranka-rs" 3 | version = "0.9.0" 4 | authors = ["Marco Boneberger "] 5 | edition = "2018" 6 | license = "EUPL-1.2" 7 | description = "Library to control Franka Emika robots" 8 | categories = ["science::robotics"] 9 | keywords = ["franka", "emika", "real-time", "panda", "libfranka"] 10 | repository = "https://github.com/marcbone/libfranka-rs" 11 | readme = "README.md" 12 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 13 | 14 | [lib] 15 | name = "franka" 16 | path = "src/lib.rs" 17 | 18 | [[example]] 19 | name = "cartesian_impedance_control" 20 | path = "examples/cartesian_impedance_control.rs" 21 | 22 | [[example]] 23 | name = "communication_test" 24 | path = "examples/communication_test.rs" 25 | 26 | [[example]] 27 | name = "download_model" 28 | path = "examples/download_model.rs" 29 | 30 | [[example]] 31 | name = "echo_robot_state" 32 | path = "examples/echo_robot_state.rs" 33 | 34 | [[example]] 35 | name = "generate_cartesian_pose_motion" 36 | path = "examples/generate_cartesian_pose_motion.rs" 37 | 38 | [[example]] 39 | name = "generate_cartesian_velocity_motion" 40 | path = "examples/generate_cartesian_velocity_motion.rs" 41 | 42 | [[example]] 43 | name = "generate_consecutive_motions" 44 | path = "examples/generate_consecutive_motions.rs" 45 | 46 | [[example]] 47 | name = "generate_elbow_motion" 48 | path = "examples/generate_elbow_motion.rs" 49 | 50 | [[example]] 51 | name = "generate_joint_position_motion" 52 | path = "examples/generate_joint_position_motion.rs" 53 | 54 | [[example]] 55 | name = "generate_joint_velocity_motion" 56 | path = "examples/generate_joint_velocity_motion.rs" 57 | 58 | [[example]] 59 | name = "grasp_object" 60 | path = "examples/grasp_object.rs" 61 | 62 | [[example]] 63 | name = "mirror_robot" 64 | path = "examples/mirror_robot.rs" 65 | 66 | [[example]] 67 | name = "print_joint_poses" 68 | path = "examples/print_joint_poses.rs" 69 | 70 | [profile.dev] 71 | opt-level = 3 72 | 73 | [dependencies] 74 | serde = { version = "1.0", features = ["derive"] } 75 | bincode = "1.3" 76 | serde_repr = "0.1" 77 | libc = "0.2" 78 | nix = "0.20.0" 79 | mio = { version = "0.7", features = ["os-poll", "tcp", "udp"] } 80 | num-derive = "0.3" 81 | num-traits = "0.2" 82 | nalgebra = "0.23" 83 | thiserror = "1.0" 84 | libloading = "0.7.0" 85 | 86 | [dev-dependencies] 87 | clap = { version = "3.1.7", features = ["derive"] } 88 | mockall = "0.9.1" 89 | float_extras = "0.1.6" 90 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | EUROPEAN UNION PUBLIC LICENCE v. 1.2 2 | EUPL © the European Union 2007, 2016 3 | 4 | This European Union Public Licence (the ‘EUPL’) applies to the Work (as defined 5 | below) which is provided under the terms of this Licence. Any use of the Work, 6 | other than as authorised under this Licence is prohibited (to the extent such 7 | use is covered by a right of the copyright holder of the Work). 8 | 9 | The Work is provided under the terms of this Licence when the Licensor (as 10 | defined below) has placed the following notice immediately following the 11 | copyright notice for the Work: 12 | 13 | Licensed under the EUPL 14 | 15 | or has expressed by any other means his willingness to license under the EUPL. 16 | 17 | 1. Definitions 18 | 19 | In this Licence, the following terms have the following meaning: 20 | 21 | - ‘The Licence’: this Licence. 22 | 23 | - ‘The Original Work’: the work or software distributed or communicated by the 24 | Licensor under this Licence, available as Source Code and also as Executable 25 | Code as the case may be. 26 | 27 | - ‘Derivative Works’: the works or software that could be created by the 28 | Licensee, based upon the Original Work or modifications thereof. This Licence 29 | does not define the extent of modification or dependence on the Original Work 30 | required in order to classify a work as a Derivative Work; this extent is 31 | determined by copyright law applicable in the country mentioned in Article 15. 32 | 33 | - ‘The Work’: the Original Work or its Derivative Works. 34 | 35 | - ‘The Source Code’: the human-readable form of the Work which is the most 36 | convenient for people to study and modify. 37 | 38 | - ‘The Executable Code’: any code which has generally been compiled and which is 39 | meant to be interpreted by a computer as a program. 40 | 41 | - ‘The Licensor’: the natural or legal person that distributes or communicates 42 | the Work under the Licence. 43 | 44 | - ‘Contributor(s)’: any natural or legal person who modifies the Work under the 45 | Licence, or otherwise contributes to the creation of a Derivative Work. 46 | 47 | - ‘The Licensee’ or ‘You’: any natural or legal person who makes any usage of 48 | the Work under the terms of the Licence. 49 | 50 | - ‘Distribution’ or ‘Communication’: any act of selling, giving, lending, 51 | renting, distributing, communicating, transmitting, or otherwise making 52 | available, online or offline, copies of the Work or providing access to its 53 | essential functionalities at the disposal of any other natural or legal 54 | person. 55 | 56 | 2. Scope of the rights granted by the Licence 57 | 58 | The Licensor hereby grants You a worldwide, royalty-free, non-exclusive, 59 | sublicensable licence to do the following, for the duration of copyright vested 60 | in the Original Work: 61 | 62 | - use the Work in any circumstance and for all usage, 63 | - reproduce the Work, 64 | - modify the Work, and make Derivative Works based upon the Work, 65 | - communicate to the public, including the right to make available or display 66 | the Work or copies thereof to the public and perform publicly, as the case may 67 | be, the Work, 68 | - distribute the Work or copies thereof, 69 | - lend and rent the Work or copies thereof, 70 | - sublicense rights in the Work or copies thereof. 71 | 72 | Those rights can be exercised on any media, supports and formats, whether now 73 | known or later invented, as far as the applicable law permits so. 74 | 75 | In the countries where moral rights apply, the Licensor waives his right to 76 | exercise his moral right to the extent allowed by law in order to make effective 77 | the licence of the economic rights here above listed. 78 | 79 | The Licensor grants to the Licensee royalty-free, non-exclusive usage rights to 80 | any patents held by the Licensor, to the extent necessary to make use of the 81 | rights granted on the Work under this Licence. 82 | 83 | 3. Communication of the Source Code 84 | 85 | The Licensor may provide the Work either in its Source Code form, or as 86 | Executable Code. If the Work is provided as Executable Code, the Licensor 87 | provides in addition a machine-readable copy of the Source Code of the Work 88 | along with each copy of the Work that the Licensor distributes or indicates, in 89 | a notice following the copyright notice attached to the Work, a repository where 90 | the Source Code is easily and freely accessible for as long as the Licensor 91 | continues to distribute or communicate the Work. 92 | 93 | 4. Limitations on copyright 94 | 95 | Nothing in this Licence is intended to deprive the Licensee of the benefits from 96 | any exception or limitation to the exclusive rights of the rights owners in the 97 | Work, of the exhaustion of those rights or of other applicable limitations 98 | thereto. 99 | 100 | 5. Obligations of the Licensee 101 | 102 | The grant of the rights mentioned above is subject to some restrictions and 103 | obligations imposed on the Licensee. Those obligations are the following: 104 | 105 | Attribution right: The Licensee shall keep intact all copyright, patent or 106 | trademarks notices and all notices that refer to the Licence and to the 107 | disclaimer of warranties. The Licensee must include a copy of such notices and a 108 | copy of the Licence with every copy of the Work he/she distributes or 109 | communicates. The Licensee must cause any Derivative Work to carry prominent 110 | notices stating that the Work has been modified and the date of modification. 111 | 112 | Copyleft clause: If the Licensee distributes or communicates copies of the 113 | Original Works or Derivative Works, this Distribution or Communication will be 114 | done under the terms of this Licence or of a later version of this Licence 115 | unless the Original Work is expressly distributed only under this version of the 116 | Licence — for example by communicating ‘EUPL v. 1.2 only’. The Licensee 117 | (becoming Licensor) cannot offer or impose any additional terms or conditions on 118 | the Work or Derivative Work that alter or restrict the terms of the Licence. 119 | 120 | Compatibility clause: If the Licensee Distributes or Communicates Derivative 121 | Works or copies thereof based upon both the Work and another work licensed under 122 | a Compatible Licence, this Distribution or Communication can be done under the 123 | terms of this Compatible Licence. For the sake of this clause, ‘Compatible 124 | Licence’ refers to the licences listed in the appendix attached to this Licence. 125 | Should the Licensee's obligations under the Compatible Licence conflict with 126 | his/her obligations under this Licence, the obligations of the Compatible 127 | Licence shall prevail. 128 | 129 | Provision of Source Code: When distributing or communicating copies of the Work, 130 | the Licensee will provide a machine-readable copy of the Source Code or indicate 131 | a repository where this Source will be easily and freely available for as long 132 | as the Licensee continues to distribute or communicate the Work. 133 | 134 | Legal Protection: This Licence does not grant permission to use the trade names, 135 | trademarks, service marks, or names of the Licensor, except as required for 136 | reasonable and customary use in describing the origin of the Work and 137 | reproducing the content of the copyright notice. 138 | 139 | 6. Chain of Authorship 140 | 141 | The original Licensor warrants that the copyright in the Original Work granted 142 | hereunder is owned by him/her or licensed to him/her and that he/she has the 143 | power and authority to grant the Licence. 144 | 145 | Each Contributor warrants that the copyright in the modifications he/she brings 146 | to the Work are owned by him/her or licensed to him/her and that he/she has the 147 | power and authority to grant the Licence. 148 | 149 | Each time You accept the Licence, the original Licensor and subsequent 150 | Contributors grant You a licence to their contributions to the Work, under the 151 | terms of this Licence. 152 | 153 | 7. Disclaimer of Warranty 154 | 155 | The Work is a work in progress, which is continuously improved by numerous 156 | Contributors. It is not a finished work and may therefore contain defects or 157 | ‘bugs’ inherent to this type of development. 158 | 159 | For the above reason, the Work is provided under the Licence on an ‘as is’ basis 160 | and without warranties of any kind concerning the Work, including without 161 | limitation merchantability, fitness for a particular purpose, absence of defects 162 | or errors, accuracy, non-infringement of intellectual property rights other than 163 | copyright as stated in Article 6 of this Licence. 164 | 165 | This disclaimer of warranty is an essential part of the Licence and a condition 166 | for the grant of any rights to the Work. 167 | 168 | 8. Disclaimer of Liability 169 | 170 | Except in the cases of wilful misconduct or damages directly caused to natural 171 | persons, the Licensor will in no event be liable for any direct or indirect, 172 | material or moral, damages of any kind, arising out of the Licence or of the use 173 | of the Work, including without limitation, damages for loss of goodwill, work 174 | stoppage, computer failure or malfunction, loss of data or any commercial 175 | damage, even if the Licensor has been advised of the possibility of such damage. 176 | However, the Licensor will be liable under statutory product liability laws as 177 | far such laws apply to the Work. 178 | 179 | 9. Additional agreements 180 | 181 | While distributing the Work, You may choose to conclude an additional agreement, 182 | defining obligations or services consistent with this Licence. However, if 183 | accepting obligations, You may act only on your own behalf and on your sole 184 | responsibility, not on behalf of the original Licensor or any other Contributor, 185 | and only if You agree to indemnify, defend, and hold each Contributor harmless 186 | for any liability incurred by, or claims asserted against such Contributor by 187 | the fact You have accepted any warranty or additional liability. 188 | 189 | 10. Acceptance of the Licence 190 | 191 | The provisions of this Licence can be accepted by clicking on an icon ‘I agree’ 192 | placed under the bottom of a window displaying the text of this Licence or by 193 | affirming consent in any other similar way, in accordance with the rules of 194 | applicable law. Clicking on that icon indicates your clear and irrevocable 195 | acceptance of this Licence and all of its terms and conditions. 196 | 197 | Similarly, you irrevocably accept this Licence and all of its terms and 198 | conditions by exercising any rights granted to You by Article 2 of this Licence, 199 | such as the use of the Work, the creation by You of a Derivative Work or the 200 | Distribution or Communication by You of the Work or copies thereof. 201 | 202 | 11. Information to the public 203 | 204 | In case of any Distribution or Communication of the Work by means of electronic 205 | communication by You (for example, by offering to download the Work from a 206 | remote location) the distribution channel or media (for example, a website) must 207 | at least provide to the public the information requested by the applicable law 208 | regarding the Licensor, the Licence and the way it may be accessible, concluded, 209 | stored and reproduced by the Licensee. 210 | 211 | 12. Termination of the Licence 212 | 213 | The Licence and the rights granted hereunder will terminate automatically upon 214 | any breach by the Licensee of the terms of the Licence. 215 | 216 | Such a termination will not terminate the licences of any person who has 217 | received the Work from the Licensee under the Licence, provided such persons 218 | remain in full compliance with the Licence. 219 | 220 | 13. Miscellaneous 221 | 222 | Without prejudice of Article 9 above, the Licence represents the complete 223 | agreement between the Parties as to the Work. 224 | 225 | If any provision of the Licence is invalid or unenforceable under applicable 226 | law, this will not affect the validity or enforceability of the Licence as a 227 | whole. Such provision will be construed or reformed so as necessary to make it 228 | valid and enforceable. 229 | 230 | The European Commission may publish other linguistic versions or new versions of 231 | this Licence or updated versions of the Appendix, so far this is required and 232 | reasonable, without reducing the scope of the rights granted by the Licence. New 233 | versions of the Licence will be published with a unique version number. 234 | 235 | All linguistic versions of this Licence, approved by the European Commission, 236 | have identical value. Parties can take advantage of the linguistic version of 237 | their choice. 238 | 239 | 14. Jurisdiction 240 | 241 | Without prejudice to specific agreement between parties, 242 | 243 | - any litigation resulting from the interpretation of this License, arising 244 | between the European Union institutions, bodies, offices or agencies, as a 245 | Licensor, and any Licensee, will be subject to the jurisdiction of the Court 246 | of Justice of the European Union, as laid down in article 272 of the Treaty on 247 | the Functioning of the European Union, 248 | 249 | - any litigation arising between other parties and resulting from the 250 | interpretation of this License, will be subject to the exclusive jurisdiction 251 | of the competent court where the Licensor resides or conducts its primary 252 | business. 253 | 254 | 15. Applicable Law 255 | 256 | Without prejudice to specific agreement between parties, 257 | 258 | - this Licence shall be governed by the law of the European Union Member State 259 | where the Licensor has his seat, resides or has his registered office, 260 | 261 | - this licence shall be governed by Belgian law if the Licensor has no seat, 262 | residence or registered office inside a European Union Member State. 263 | 264 | Appendix 265 | 266 | ‘Compatible Licences’ according to Article 5 EUPL are: 267 | 268 | - GNU General Public License (GPL) v. 2, v. 3 269 | - GNU Affero General Public License (AGPL) v. 3 270 | - Open Software License (OSL) v. 2.1, v. 3.0 271 | - Eclipse Public License (EPL) v. 1.0 272 | - CeCILL v. 2.0, v. 2.1 273 | - Mozilla Public Licence (MPL) v. 2 274 | - GNU Lesser General Public Licence (LGPL) v. 2.1, v. 3 275 | - Creative Commons Attribution-ShareAlike v. 3.0 Unported (CC BY-SA 3.0) for 276 | works other than software 277 | - European Union Public Licence (EUPL) v. 1.1, v. 1.2 278 | - Québec Free and Open-Source Licence — Reciprocity (LiLiQ-R) or Strong 279 | Reciprocity (LiLiQ-R+). 280 | 281 | The European Commission may update this Appendix to later versions of the above 282 | licences without producing a new version of the EUPL, as long as they provide 283 | the rights granted in Article 2 of this Licence and protect the covered Source 284 | Code from exclusive appropriation. 285 | 286 | All other changes or additions to this Appendix require the production of a new 287 | EUPL version. 288 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![crates.io](https://img.shields.io/crates/v/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) 2 | ![GitHub Workflow Status](https://img.shields.io/github/workflow/status/marcbone/libfranka-rs/Rust) 3 | [![crates.io](https://img.shields.io/crates/l/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) 4 | [![crates.io](https://img.shields.io/crates/d/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) 5 | [![docs.rs](https://docs.rs/libfranka-rs/badge.svg)](https://docs.rs/libfranka-rs) 6 | # libfranka-rs 7 | libfranka-rs is an **unofficial** port of [libfranka](https://github.com/frankaemika/libfranka) written in Rust. 8 | This library can interact with research versions of Franka Emika Robots. 9 | The library aims to provide researchers the possibility to experiment with Rust within a real-time robotics 10 | application. 11 | 12 | **ALWAYS HAVE THE USER STOP BUTTON AT HAND WHILE CONTROLLING 13 | THE ROBOT!** 14 | 15 | ## Features 16 | * Real-time control of the robot 17 | * A libfranka-like API 18 | * Usage with Preempt_RT or stock Linux kernel 19 | * Usage of the gripper 20 | * Usage of the robot model 21 | * Download the robot model for offline usage 22 | * Ports of the libfranka examples to help you to get started 23 | * The functionality of the [example_commons](https://github.com/frankaemika/libfranka/blob/master/examples/examples_common.cpp) is directly part of the library, so you do not have to copy these files to your project 24 | * Direct Conversions from [nalgebra](https://nalgebra.org/) (Eigen3 equivalent) types into libfranka control types (JointPositions, CartesianPose, ...) 25 | * Proper error handling with Result types 26 | 27 | TODO: 28 | * Usage of the Model for anything else but Linux x86_64 29 | 30 | Not supported: 31 | * Windows (macOS could maybe work, but I have not tested it) 32 | * Vacuum Grippers (we do not have those, so I cannot test them) 33 | 34 | ## Example 35 | A small example for controlling joint positions. You can find more in the examples folder. 36 | ```rust 37 | use franka::{FrankaResult, JointPositions, MotionFinished, Robot, RobotState}; 38 | use std::f64::consts::PI; 39 | use std::time::Duration; 40 | fn main() -> FrankaResult<()> { 41 | let mut robot = Robot::new("robotik-bs.de", None, None)?; 42 | robot.set_default_behavior()?; 43 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 44 | println!("Press Enter to continue..."); 45 | std::io::stdin().read_line(&mut String::new()).unwrap(); 46 | 47 | // Set additional parameters always before the control loop, NEVER in the control loop! 48 | // Set collision behavior. 49 | robot.set_collision_behavior( 50 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 51 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 52 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 53 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 54 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 55 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 56 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 57 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 58 | )?; 59 | 60 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 61 | robot.joint_motion(0.5, &q_goal)?; 62 | println!("Finished moving to initial joint configuration."); 63 | let mut initial_position = JointPositions::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 64 | let mut time = 0.; 65 | let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { 66 | time += time_step.as_secs_f64(); 67 | if time == 0. { 68 | initial_position.q = state.q_d; 69 | } 70 | let mut out = JointPositions::new(initial_position.q); 71 | let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time)); 72 | out.q[3] += delta_angle; 73 | out.q[4] += delta_angle; 74 | out.q[6] += delta_angle; 75 | if time >= 5.0 { 76 | println!("Finished motion, shutting down example"); 77 | return out.motion_finished(); 78 | } 79 | out 80 | }; 81 | robot.control_joint_positions(callback, None, None, None) 82 | } 83 | ``` 84 | 85 | ## How to get started 86 | As it is a straight port, you may find the 87 | [Franka Control Interface Documentation](https://frankaemika.github.io/docs/index.html) helpful. 88 | 89 | ### With zero Rust knowledge 90 | If this is your first time using Rust, I recommend reading the [Rust Book](https://doc.rust-lang.org/stable/book/). 91 | 92 | If you have Rust installed and just want to play with the examples, you can also run: 93 | ```bash 94 | cargo install libfranka-rs --examples --version 0.9.0 95 | generate_joint_position_motion 96 | ``` 97 | 98 | If you are already familiar with the original libfranka examples. I suggest you take a look at the examples folder. 99 | The examples that are named like an original libfranka example are ports that stay as close to the original as possible, 100 | hoping that it makes your introduction into the Rust world as smooth as possible. 101 | 102 | 103 | 104 | ### With zero libfranka knowledge 105 | The [Franka Control Interface Documentation](https://frankaemika.github.io/docs/index.html) also includes a setup guide. 106 | You can skip the installation of libfranka as you will be using libfranka-rs. 107 | Take a look at the [Documentation](https://docs.rs/libfranka-rs) and the examples folder. You should run the 108 | communication_test example to verify that your setup is correct. 109 | 110 | ### How to use libfranka-rs 111 | If you want to use libfranka-rs in your project, you have to add 112 | ```text 113 | libfranka-rs = "0.9.0" 114 | ``` 115 | to your Cargo.toml file. 116 | libfranka-rs version numbers are structured as MAJOR.MINOR.PATCH. The Major and Minor versions match the original libfranka 117 | version numbers. That means for 0.8, your robot has to be at least on Firmware 4.0.0. Older firmware versions are not supported by 118 | libfranka-rs. You can find more information about system updates [here](https://frankaemika.github.io). 119 | 120 | ## Licence 121 | This library is copyrighted © 2021 Marco Boneberger 122 | 123 | 124 | Licensed under the EUPL, Version 1.2 or – as soon they will be approved by the European Commission - subsequent versions of the EUPL (the "Licence"); 125 | 126 | You may not use this work except in compliance with the Licence. 127 | You may obtain a copy of the Licence at: 128 | 129 | [https://joinup.ec.europa.eu/software/page/eupl](https://joinup.ec.europa.eu/software/page/eupl) 130 | 131 | Unless required by applicable law or agreed to in writing, software distributed under the Licence is distributed on an "AS IS" basis 132 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 133 | 134 | See the Licence for the specific language governing permissions and limitations under the Licence. 135 | -------------------------------------------------------------------------------- /examples/cartesian_impedance_control.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::Frame; 6 | use franka::FrankaResult; 7 | use franka::Robot; 8 | use franka::RobotState; 9 | use franka::Torques; 10 | use franka::{array_to_isometry, Matrix6x7, Vector7}; 11 | use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; 12 | use std::time::Duration; 13 | 14 | /// An example showing a simple cartesian impedance controller without inertia shaping 15 | /// that renders a spring damper system where the equilibrium is the initial configuration. 16 | /// After starting the controller try to push the robot around and try different stiffness levels. 17 | /// 18 | /// WARNING collision thresholds are set to high values. Make sure you have the user stop at hand! 19 | #[derive(Parser, Debug)] 20 | #[clap(author, version, name = "cartesian_impedance_control")] 21 | struct CommandLineArguments { 22 | /// IP-Address or hostname of the robot 23 | pub franka_ip: String, 24 | } 25 | 26 | fn main() -> FrankaResult<()> { 27 | let args = CommandLineArguments::parse(); 28 | let translational_stiffness = 150.; 29 | let rotational_stiffness = 10.; 30 | 31 | let mut stiffness: Matrix6 = Matrix6::zeros(); 32 | let mut damping: Matrix6 = Matrix6::zeros(); 33 | { 34 | let mut top_left_corner = stiffness.fixed_slice_mut::(0, 0); 35 | top_left_corner.copy_from(&(Matrix3::identity() * translational_stiffness)); 36 | let mut top_left_corner = damping.fixed_slice_mut::(0, 0); 37 | top_left_corner.copy_from(&(2. * f64::sqrt(translational_stiffness) * Matrix3::identity())); 38 | } 39 | { 40 | let mut bottom_right_corner = stiffness.fixed_slice_mut::(3, 3); 41 | bottom_right_corner.copy_from(&(Matrix3::identity() * rotational_stiffness)); 42 | let mut bottom_right_corner = damping.fixed_slice_mut::(3, 3); 43 | bottom_right_corner 44 | .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); 45 | } 46 | let mut robot = Robot::new(args.franka_ip.as_str(), None, None)?; 47 | let model = robot.load_model(true)?; 48 | 49 | // Set additional parameters always before the control loop, NEVER in the control loop! 50 | // Set collision behavior. 51 | robot.set_collision_behavior( 52 | [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6], [100.; 6], 53 | )?; 54 | robot.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; 55 | robot.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; 56 | let initial_state = robot.read_once()?; 57 | let initial_transform = array_to_isometry(&initial_state.O_T_EE); 58 | let position_d = initial_transform.translation.vector; 59 | let orientation_d = initial_transform.rotation; 60 | 61 | println!( 62 | "WARNING: Collision thresholds are set to high values. \ 63 | Make sure you have the user stop at hand!" 64 | ); 65 | println!("After starting try to push the robot and see how it reacts."); 66 | println!("Press Enter to continue..."); 67 | std::io::stdin().read_line(&mut String::new()).unwrap(); 68 | let result = robot.control_torques( 69 | |state: &RobotState, _step: &Duration| -> Torques { 70 | let coriolis: Vector7 = model.coriolis_from_state(&state).into(); 71 | let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, &state); 72 | let jacobian = Matrix6x7::from_column_slice(&jacobian_array); 73 | let _q = Vector7::from_column_slice(&state.q); 74 | let dq = Vector7::from_column_slice(&state.dq); 75 | let transform = array_to_isometry(&state.O_T_EE); 76 | let position = transform.translation.vector; 77 | let mut orientation = *transform.rotation.quaternion(); 78 | 79 | let mut error: Matrix6x1 = Matrix6x1::::zeros(); 80 | { 81 | let mut error_head = error.fixed_slice_mut::(0, 0); 82 | error_head.set_column(0, &(position - position_d)); 83 | } 84 | 85 | if orientation_d.coords.dot(&orientation.coords) < 0. { 86 | orientation.coords = -orientation.coords; 87 | } 88 | let orientation = UnitQuaternion::new_normalize(orientation); 89 | let error_quaternion: UnitQuaternion = orientation.inverse() * orientation_d; 90 | { 91 | let mut error_tail = error.fixed_slice_mut::(3, 0); 92 | error_tail.copy_from( 93 | &-(transform.rotation.to_rotation_matrix() 94 | * Vector3::new(error_quaternion.i, error_quaternion.j, error_quaternion.k)), 95 | ); 96 | } 97 | let tau_task: Vector7 = 98 | jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq)); 99 | let tau_d: Vector7 = tau_task + coriolis; 100 | 101 | tau_d.into() 102 | }, 103 | None, 104 | None, 105 | ); 106 | 107 | match result { 108 | Ok(_) => Ok(()), 109 | Err(e) => { 110 | eprintln!("{}", e); 111 | Ok(()) 112 | } 113 | } 114 | } 115 | -------------------------------------------------------------------------------- /examples/communication_test.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use clap::Parser; 4 | use franka::FrankaResult; 5 | use franka::Robot; 6 | use franka::RobotState; 7 | use franka::{MotionFinished, Torques}; 8 | use std::f64::consts::PI; 9 | use std::time::Duration; 10 | 11 | /// An example indicating the network performance. 12 | /// 13 | /// WARNING: Before executing this example, make sure there is enough space in front of the robot. 14 | #[derive(Parser, Debug)] 15 | #[clap(author, version, name = "communication_test")] 16 | struct CommandLineArguments { 17 | /// IP-Address or hostname of the robot 18 | pub franka_ip: String, 19 | } 20 | 21 | fn main() -> FrankaResult<()> { 22 | let args = CommandLineArguments::parse(); 23 | let mut robot = Robot::new(args.franka_ip.as_str(), None, None)?; 24 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 25 | 26 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 27 | println!("Press Enter to continue..."); 28 | std::io::stdin().read_line(&mut String::new()).unwrap(); 29 | 30 | robot.set_default_behavior()?; 31 | robot.set_collision_behavior( 32 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 33 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 34 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 35 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 36 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 37 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 38 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 39 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 40 | )?; 41 | 42 | robot.joint_motion(0.5, &q_goal)?; 43 | let zero_torques = Torques::new([0.; 7]); 44 | let mut time: u64 = 0; 45 | let mut counter: u64 = 0; 46 | let mut avg_success_rate = 0.; 47 | let mut min_success_rate = 1.; 48 | let mut max_success_rate = 0.; 49 | 50 | let callback = |state: &RobotState, time_step: &Duration| -> Torques { 51 | time += time_step.as_millis() as u64; 52 | if time == 0 { 53 | return zero_torques; 54 | } 55 | counter += 1; 56 | if counter % 100 == 0 { 57 | println!( 58 | "#{} Current success rate: {}", 59 | counter, state.control_command_success_rate 60 | ); 61 | } 62 | std::thread::sleep(Duration::from_micros(100)); 63 | avg_success_rate += state.control_command_success_rate; 64 | if state.control_command_success_rate > max_success_rate { 65 | max_success_rate = state.control_command_success_rate 66 | } 67 | if state.control_command_success_rate < min_success_rate { 68 | min_success_rate = state.control_command_success_rate 69 | } 70 | if time >= 10000 { 71 | return zero_torques.motion_finished(); 72 | } 73 | zero_torques 74 | }; 75 | robot.control_torques(callback, false, 1000.)?; 76 | 77 | avg_success_rate /= counter as f64; 78 | let lost_robot_states = time - counter; 79 | println!("#######################################################"); 80 | if lost_robot_states > 0 { 81 | println!( 82 | "The control loop did not get executed {} times in the", 83 | lost_robot_states 84 | ); 85 | println!( 86 | "last {} milliseconds! (lost {} robot_states)", 87 | time, lost_robot_states 88 | ); 89 | println!(); 90 | } 91 | println!("Control command success rate of {} samples: ", counter); 92 | println!("Max: {}", max_success_rate); 93 | println!("Avg: {}", avg_success_rate); 94 | println!("Min {}", min_success_rate); 95 | 96 | if avg_success_rate < 0.9 { 97 | println!(); 98 | println!("WARNING: THIS SETUP IS PROBABLY NOT SUFFICIENT FOR FCI!"); 99 | println!("PLEASE TRY OUT A DIFFERENT PC / NIC"); 100 | } else if avg_success_rate < 0.95 { 101 | println!(); 102 | println!("WARNING: MANY PACKETS GOT LOST!"); 103 | println!("PLEASE INSPECT YOUR SETUP AND FOLLOW ADVICE ON"); 104 | println!("https://frankaemika.github.io/docs/troubleshooting.html") 105 | } 106 | println!("#######################################################"); 107 | Ok(()) 108 | } 109 | -------------------------------------------------------------------------------- /examples/download_model.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::exception::FrankaException::ModelException; 6 | use franka::Robot; 7 | use franka::{FrankaResult, RealtimeConfig}; 8 | use std::fs; 9 | use std::path::PathBuf; 10 | 11 | /// Downloads the model for offline usage 12 | #[derive(Parser, Debug)] 13 | #[clap(author, version, name = "download_model")] 14 | struct CommandLineArguments { 15 | /// IP-Address or hostname of the robot 16 | pub franka_ip: String, 17 | 18 | /// Directory where the model should be downloaded 19 | #[clap(parse(from_os_str))] 20 | download_path: PathBuf, 21 | } 22 | 23 | fn main() -> FrankaResult<()> { 24 | let args: CommandLineArguments = CommandLineArguments::parse(); 25 | let mut path = args.download_path; 26 | path.push("model.so"); 27 | let mut robot = Robot::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; 28 | robot.load_model(true)?; 29 | fs::copy("/tmp/model.so", &path).map_err(|_| ModelException { 30 | message: "Could copy model to download location".to_string(), 31 | })?; 32 | println!("Model successfully downloaded to {:?}", &path); 33 | Ok(()) 34 | } 35 | -------------------------------------------------------------------------------- /examples/echo_robot_state.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::FrankaResult; 6 | use franka::Robot; 7 | use franka::RobotState; 8 | 9 | /// An example showing how to continuously read the robot state. 10 | #[derive(Parser, Debug)] 11 | #[clap(author, version, name = "echo_robot_state")] 12 | struct CommandLineArguments { 13 | /// IP-Address or hostname of the robot 14 | pub franka_ip: String, 15 | } 16 | 17 | fn main() -> FrankaResult<()> { 18 | let address = CommandLineArguments::parse(); 19 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 20 | let mut count = 0; 21 | robot.read(|robot_state: &RobotState| { 22 | // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but 23 | // should not be done in a control loop. 24 | println!("{:?}", robot_state); 25 | count += 1; 26 | count <= 100 27 | })?; 28 | println!("Done"); 29 | Ok(()) 30 | } 31 | -------------------------------------------------------------------------------- /examples/generate_cartesian_pose_motion.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::FrankaResult; 6 | use franka::Robot; 7 | use franka::RobotState; 8 | use franka::{CartesianPose, MotionFinished}; 9 | use std::f64::consts::PI; 10 | use std::time::Duration; 11 | 12 | /// An example showing how to generate a Cartesian motion. 13 | /// 14 | /// WARNING: Before executing this example, make sure there is enough space in front of the robot. 15 | #[derive(Parser, Debug)] 16 | #[clap(author, version, name = "generate_cartesian_pose_motion")] 17 | struct CommandLineArguments { 18 | /// IP-Address or hostname of the robot 19 | pub franka_ip: String, 20 | } 21 | 22 | fn main() -> FrankaResult<()> { 23 | let address = CommandLineArguments::parse(); 24 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 25 | robot.set_default_behavior()?; 26 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 27 | println!("Press Enter to continue..."); 28 | std::io::stdin().read_line(&mut String::new()).unwrap(); 29 | // Set additional parameters always before the control loop, NEVER in the control loop! 30 | // Set collision behavior. 31 | robot.set_collision_behavior( 32 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 33 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 34 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 35 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 36 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 37 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 38 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 39 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 40 | )?; 41 | 42 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 43 | robot.joint_motion(0.5, &q_goal)?; 44 | println!("Finished moving to initial joint configuration."); 45 | let mut initial_pose = CartesianPose::new([0.0; 16], None); 46 | let mut time = 0.; 47 | let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { 48 | time += time_step.as_secs_f64(); 49 | if time == 0. { 50 | initial_pose.O_T_EE = state.O_T_EE_c; 51 | } 52 | let radius = 0.3; 53 | let angle = PI / 4. * (1. - f64::cos(PI / 5. * time)); 54 | let delta_x = radius * f64::sin(angle); 55 | let delta_z = radius * (f64::cos(angle) - 1.); 56 | let mut out = CartesianPose::new(initial_pose.O_T_EE, None); 57 | 58 | out.O_T_EE[12] += delta_x; 59 | out.O_T_EE[14] += delta_z; 60 | if time >= 10.0 { 61 | println!("Finished motion, shutting down example"); 62 | return out.motion_finished(); 63 | } 64 | out 65 | }; 66 | robot.control_cartesian_pose(callback, None, None, None) 67 | } 68 | -------------------------------------------------------------------------------- /examples/generate_cartesian_velocity_motion.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::{CartesianVelocities, FrankaResult, MotionFinished, Robot, RobotState}; 6 | use std::f64::consts::PI; 7 | use std::time::Duration; 8 | 9 | /// An example showing how to generate a Cartesian velocity motion. 10 | /// 11 | /// WARNING: Before executing this example, make sure there is enough space in front of the robot. 12 | #[derive(Parser, Debug)] 13 | #[clap(author, version, name = "generate_cartesian_velocity_motion")] 14 | struct CommandLineArguments { 15 | /// IP-Address or hostname of the robot 16 | pub franka_ip: String, 17 | } 18 | 19 | fn main() -> FrankaResult<()> { 20 | let address = CommandLineArguments::parse(); 21 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 22 | robot.set_default_behavior()?; 23 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 24 | println!("Press Enter to continue..."); 25 | std::io::stdin().read_line(&mut String::new()).unwrap(); 26 | 27 | // Set additional parameters always before the control loop, NEVER in the control loop! 28 | // Set the joint impedance. 29 | robot.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; 30 | let lower_torque_thresholds_nominal = [25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.]; 31 | let upper_torque_thresholds_nominal = [35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0]; 32 | let lower_torque_thresholds_acceleration = lower_torque_thresholds_nominal; 33 | let upper_torque_thresholds_acceleration = upper_torque_thresholds_nominal; 34 | 35 | let lower_force_thresholds_nominal = [30.0, 30.0, 30.0, 25.0, 25.0, 25.0]; 36 | let upper_force_thresholds_nominal = [40.0, 40.0, 40.0, 35.0, 35.0, 35.0]; 37 | let lower_force_thresholds_acceleration = lower_force_thresholds_nominal; 38 | let upper_force_thresholds_acceleration = upper_force_thresholds_nominal; 39 | robot.set_collision_behavior( 40 | lower_torque_thresholds_acceleration, 41 | upper_torque_thresholds_acceleration, 42 | lower_torque_thresholds_nominal, 43 | upper_torque_thresholds_nominal, 44 | lower_force_thresholds_acceleration, 45 | upper_force_thresholds_acceleration, 46 | lower_force_thresholds_nominal, 47 | upper_force_thresholds_nominal, 48 | )?; 49 | 50 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 51 | robot.joint_motion(0.5, &q_goal)?; 52 | println!("Finished moving to initial joint configuration."); 53 | let time_max = 4.; 54 | let v_max = 0.1; 55 | let angle = PI / 4.; 56 | let mut time = 0.; 57 | let callback = |_state: &RobotState, period: &Duration| -> CartesianVelocities { 58 | time += period.as_secs_f64(); 59 | 60 | let cycle = f64::floor(f64::powf( 61 | -1.0, 62 | (time - float_extras::f64::fmod(time, time_max)) / time_max, 63 | )); 64 | 65 | let v = cycle * v_max / 2. * (1. - f64::cos(2. * PI / time_max * time)); 66 | let v_x = f64::cos(angle) * v; 67 | let v_z = -f64::sin(angle) * v; 68 | let output = CartesianVelocities::new([v_x, 0., v_z, 0., 0., 0.], None); 69 | if time >= 2. * time_max { 70 | println!("Finished motion, shutting down example"); 71 | return output.motion_finished(); 72 | } 73 | output 74 | }; 75 | robot.control_cartesian_velocities(callback, None, None, None) 76 | } 77 | -------------------------------------------------------------------------------- /examples/generate_consecutive_motions.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::exception::FrankaException; 6 | use franka::{FrankaResult, JointVelocities, MotionFinished, Robot, RobotState}; 7 | use std::f64::consts::PI; 8 | use std::time::Duration; 9 | 10 | /// An example showing how to execute consecutive motions with error recovery. 11 | /// 12 | /// WARNING: Before executing this example, make sure there is enough space in front and to the side 13 | /// of the robot. 14 | #[derive(Parser, Debug)] 15 | #[clap(author, version, name = "generate_consecutive_motions")] 16 | struct CommandLineArguments { 17 | /// IP-Address or hostname of the robot 18 | pub franka_ip: String, 19 | } 20 | 21 | fn main() -> FrankaResult<()> { 22 | let address = CommandLineArguments::parse(); 23 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 24 | robot.set_default_behavior()?; 25 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 26 | println!("Press Enter to continue..."); 27 | std::io::stdin().read_line(&mut String::new()).unwrap(); 28 | 29 | // Set additional parameters always before the control loop, NEVER in the control loop! 30 | // Set collision behavior. 31 | robot.set_collision_behavior( 32 | [10.0, 10.0, 9.0, 9.0, 6.0, 7.0, 6.0], 33 | [10.0, 10.0, 9.0, 9.0, 6.0, 7.0, 6.0], 34 | [10.0, 10.0, 9.0, 9.0, 6.0, 7.0, 6.0], 35 | [10.0, 10.0, 9.0, 9.0, 6.0, 7.0, 6.0], 36 | [10.0, 10.0, 10.0, 12.5, 12.5, 12.5], 37 | [10.0, 10.0, 10.0, 12.5, 12.5, 12.5], 38 | [10.0, 10.0, 10.0, 12.5, 12.5, 12.5], 39 | [10.0, 10.0, 10.0, 12.5, 12.5, 12.5], 40 | )?; 41 | 42 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 43 | robot.joint_motion(0.5, &q_goal)?; 44 | println!("Finished moving to initial joint configuration."); 45 | let mut time = 0.; 46 | let omega_max = 0.2; 47 | let time_max = 4.0; 48 | let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { 49 | time += time_step.as_secs_f64(); 50 | 51 | let cycle = f64::floor(f64::powf( 52 | -1.0, 53 | (time - float_extras::f64::fmod(time, time_max)) / time_max, 54 | )); 55 | 56 | let omega = cycle * omega_max / 2. * (1. - f64::cos(2. * PI / time_max * time)); 57 | let out = JointVelocities::new([0., 0., 0., omega, omega, omega, omega]); 58 | if time >= 2. * time_max { 59 | println!("Finished motion."); 60 | return out.motion_finished(); 61 | } 62 | out 63 | }; 64 | for _ in 0..5 { 65 | let result = robot.control_joint_velocities(callback, None, None, None); 66 | match result { 67 | Ok(_) => {} 68 | Err(e) => match e { 69 | FrankaException::ControlException { log: _, error: msg } => { 70 | println!("{}", msg); 71 | println!("Running error recovery..."); 72 | robot.automatic_error_recovery()?; 73 | } 74 | _ => { 75 | return Err(e); 76 | } 77 | }, 78 | } 79 | } 80 | Ok(()) 81 | } 82 | -------------------------------------------------------------------------------- /examples/generate_elbow_motion.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::{CartesianPose, FrankaResult, MotionFinished, Robot, RobotState}; 6 | use std::f64::consts::PI; 7 | use std::time::Duration; 8 | 9 | /// An example showing how to move the robot's elbow. 10 | /// 11 | /// WARNING: Before executing this example, make sure that the elbow has enough space to move. 12 | #[derive(Parser, Debug)] 13 | #[clap(author, version, name = "generate_elbow_motion")] 14 | struct CommandLineArguments { 15 | /// IP-Address or hostname of the robot 16 | pub franka_ip: String, 17 | } 18 | 19 | fn main() -> FrankaResult<()> { 20 | let address = CommandLineArguments::parse(); 21 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 22 | robot.set_default_behavior()?; 23 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 24 | println!("Press Enter to continue..."); 25 | std::io::stdin().read_line(&mut String::new()).unwrap(); 26 | // Set additional parameters always before the control loop, NEVER in the control loop! 27 | // Set collision behavior. 28 | robot.set_collision_behavior( 29 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 30 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 31 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 32 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 33 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 34 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 35 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 36 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 37 | )?; 38 | 39 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 40 | robot.joint_motion(0.5, &q_goal)?; 41 | println!("Finished moving to initial joint configuration."); 42 | let mut initial_pose = [0.; 16]; 43 | let mut initial_elbow = [0.; 2]; 44 | let mut time = 0.; 45 | let callback = |state: &RobotState, time_step: &Duration| -> CartesianPose { 46 | time += time_step.as_secs_f64(); 47 | if time == 0. { 48 | initial_pose = state.O_T_EE_c; 49 | initial_elbow = state.elbow_c; 50 | } 51 | let angle = PI / 10. * (1. - f64::cos(PI / 5. * time)); 52 | let mut elbow = initial_elbow; 53 | elbow[0] += angle; 54 | let out = CartesianPose::new(initial_pose, Some(elbow)); 55 | if time >= 10.0 { 56 | println!("Finished motion, shutting down example"); 57 | return out.motion_finished(); 58 | } 59 | out 60 | }; 61 | robot.control_cartesian_pose(callback, None, None, None) 62 | } 63 | -------------------------------------------------------------------------------- /examples/generate_joint_position_motion.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::{FrankaResult, JointPositions, MotionFinished, Robot, RobotState}; 6 | use std::f64::consts::PI; 7 | use std::time::Duration; 8 | 9 | /// An example showing how to generate a joint position motion. 10 | /// 11 | /// WARNING: Before executing this example, make sure there is enough space in front of the robot. 12 | #[derive(Parser, Debug)] 13 | #[clap(author, version, name = "generate_joint_position_motion")] 14 | struct CommandLineArguments { 15 | /// IP-Address or hostname of the robot 16 | pub franka_ip: String, 17 | } 18 | 19 | fn main() -> FrankaResult<()> { 20 | let address = CommandLineArguments::parse(); 21 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 22 | robot.set_default_behavior()?; 23 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 24 | println!("Press Enter to continue..."); 25 | std::io::stdin().read_line(&mut String::new()).unwrap(); 26 | 27 | // Set additional parameters always before the control loop, NEVER in the control loop! 28 | // Set collision behavior. 29 | robot.set_collision_behavior( 30 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 31 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 32 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 33 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 34 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 35 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 36 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 37 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 38 | )?; 39 | 40 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 41 | robot.joint_motion(0.5, &q_goal)?; 42 | println!("Finished moving to initial joint configuration."); 43 | let mut initial_position = JointPositions::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 44 | let mut time = 0.; 45 | let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { 46 | time += time_step.as_secs_f64(); 47 | if time == 0. { 48 | initial_position.q = state.q_d; 49 | } 50 | let mut out = JointPositions::new(initial_position.q); 51 | let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time)); 52 | out.q[3] += delta_angle; 53 | out.q[4] += delta_angle; 54 | out.q[6] += delta_angle; 55 | if time >= 5.0 { 56 | println!("Finished motion, shutting down example"); 57 | return out.motion_finished(); 58 | } 59 | out 60 | }; 61 | robot.control_joint_positions(callback, None, None, None) 62 | } 63 | -------------------------------------------------------------------------------- /examples/generate_joint_velocity_motion.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use clap::Parser; 5 | use franka::FrankaResult; 6 | use franka::Robot; 7 | use franka::RobotState; 8 | use franka::{JointVelocities, MotionFinished}; 9 | use std::f64::consts::PI; 10 | use std::time::Duration; 11 | 12 | /// An example showing how to generate a joint velocity motion. 13 | /// 14 | /// WARNING: Before executing this example, make sure there is enough space in front of the robot. 15 | #[derive(Parser, Debug)] 16 | #[clap(author, version, name = "generate_joint_velocity_motion")] 17 | struct CommandLineArguments { 18 | /// IP-Address or hostname of the robot 19 | pub franka_ip: String, 20 | } 21 | 22 | fn main() -> FrankaResult<()> { 23 | let address = CommandLineArguments::parse(); 24 | let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; 25 | robot.set_default_behavior()?; 26 | println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); 27 | println!("Press Enter to continue..."); 28 | std::io::stdin().read_line(&mut String::new()).unwrap(); 29 | 30 | // Set additional parameters always before the control loop, NEVER in the control loop! 31 | // Set collision behavior. 32 | robot.set_collision_behavior( 33 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 34 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 35 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 36 | [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 37 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 38 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 39 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 40 | [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 41 | )?; 42 | 43 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 44 | robot.joint_motion(0.5, &q_goal)?; 45 | println!("Finished moving to initial joint configuration."); 46 | let mut time = 0.; 47 | let omega_max = 1.0; 48 | let time_max = 1.0; 49 | let callback = move |_state: &RobotState, time_step: &Duration| -> JointVelocities { 50 | time += time_step.as_secs_f64(); 51 | 52 | let cycle = f64::floor(f64::powf( 53 | -1.0, 54 | (time - float_extras::f64::fmod(time, time_max)) / time_max, 55 | )); 56 | 57 | let omega = cycle * omega_max / 2. * (1. - f64::cos(2. * PI / time_max * time)); 58 | let out = JointVelocities::new([0., 0., 0., omega, omega, omega, omega]); 59 | if time >= 2. * time_max { 60 | println!("Finished motion, shutting down example"); 61 | return out.motion_finished(); 62 | } 63 | out 64 | }; 65 | robot.control_joint_velocities(callback, None, None, None) 66 | } 67 | -------------------------------------------------------------------------------- /examples/grasp_object.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use clap::Parser; 4 | use franka::{FrankaResult, Gripper}; 5 | use std::time::Duration; 6 | 7 | /// An example showing how to control FRANKA's gripper. 8 | #[derive(Parser, Debug)] 9 | #[clap(author, version, name = "grasp_object")] 10 | struct CommandLineArguments { 11 | /// IP-Address or hostname of the gripper 12 | pub gripper_hostname: String, 13 | /// Perform homing before grasping to calibrate the gripper 14 | #[clap(long)] 15 | pub homing: bool, 16 | /// Width of the object in meter 17 | pub object_width: f64, 18 | } 19 | 20 | fn main() -> FrankaResult<()> { 21 | let args: CommandLineArguments = CommandLineArguments::parse(); 22 | let mut gripper = Gripper::new(args.gripper_hostname.as_str())?; 23 | if args.homing { 24 | gripper.homing()?; 25 | } 26 | let state = gripper.read_once()?; 27 | if state.max_width < args.object_width { 28 | eprintln!("Object is too large for the current fingers on the gripper."); 29 | std::process::exit(-1); 30 | } 31 | gripper.grasp(args.object_width, 0.1, 60., None, None)?; 32 | std::thread::sleep(Duration::from_secs(3)); 33 | let state = gripper.read_once()?; 34 | if !state.is_grasped { 35 | eprintln!("Object lost"); 36 | std::process::exit(-1); 37 | } 38 | println!("Grasped object, will release it now."); 39 | gripper.stop()?; 40 | Ok(()) 41 | } 42 | -------------------------------------------------------------------------------- /examples/mirror_robot.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use clap::Parser; 4 | use core::f64::consts::PI; 5 | use franka::exception::FrankaResult; 6 | use franka::model::Frame; 7 | use franka::robot::control_types::Torques; 8 | use franka::robot::Robot; 9 | use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; 10 | use franka::Matrix7; 11 | use franka::RobotState; 12 | use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; 13 | use std::sync::mpsc::channel; 14 | use std::time::Duration; 15 | 16 | /// An example where one robot is guided by the user and the other robot acts as a mirror. In this 17 | /// case mirror does not mean equal joint positions. Instead, it acts like a real physical mirror that 18 | /// stands in front of the robot (mirrored cartesian poses). Hand guide the end-effector of the user robot 19 | /// and see how the other robot mirrors this pose. 20 | /// WARNING: Before executing this example, make sure there is enough space in between the robots. 21 | #[derive(Parser, Debug)] 22 | #[clap(author, version, name = "mirror_robot")] 23 | struct CommandLineArguments { 24 | /// IP-Address or hostname of the robot which the user can hand guide 25 | #[clap(long)] 26 | pub franka_ip_user: String, 27 | /// IP-Address or hostname of the robot which mirrors the movement 28 | #[clap(long)] 29 | pub franka_ip_mirror: String, 30 | } 31 | 32 | const NULLSPACE_TORQUE_SCALING: f64 = 5.; 33 | fn main() -> FrankaResult<()> { 34 | let args = CommandLineArguments::parse(); 35 | let translational_stiffness = 400.; 36 | let rotational_stiffness = 50.; 37 | 38 | let mut stiffness: Matrix6 = Matrix6::zeros(); 39 | let mut damping: Matrix6 = Matrix6::zeros(); 40 | { 41 | let mut top_left_corner = stiffness.fixed_slice_mut::(0, 0); 42 | top_left_corner.copy_from(&(Matrix3::identity() * translational_stiffness)); 43 | let mut top_left_corner = damping.fixed_slice_mut::(0, 0); 44 | top_left_corner.copy_from(&(2. * f64::sqrt(translational_stiffness) * Matrix3::identity())); 45 | } 46 | { 47 | let mut bottom_right_corner = stiffness.fixed_slice_mut::(3, 3); 48 | bottom_right_corner.copy_from(&(Matrix3::identity() * rotational_stiffness)); 49 | let mut bottom_right_corner = damping.fixed_slice_mut::(3, 3); 50 | bottom_right_corner 51 | .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); 52 | } 53 | let mut robot_user = Robot::new(args.franka_ip_user.as_str(), None, None)?; 54 | let mut robot_mirror = Robot::new(args.franka_ip_mirror.as_str(), None, None)?; 55 | let model = robot_mirror.load_model(true)?; 56 | robot_mirror.set_collision_behavior( 57 | [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6], [100.; 6], 58 | )?; 59 | robot_mirror.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; 60 | robot_mirror.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; 61 | robot_user.set_collision_behavior( 62 | [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6], [100.; 6], 63 | )?; 64 | robot_user.set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])?; 65 | robot_user.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; 66 | let initial_state = robot_user.read_once()?; 67 | let initial_transform = array_to_isometry(&initial_state.O_T_EE); 68 | let mut desired_state = initial_state.O_T_EE; 69 | let mut position_d = initial_transform.translation.vector; 70 | let mut orientation_d = initial_transform.rotation; 71 | let (sender, receiver) = channel(); 72 | 73 | println!( 74 | "WARNING: Collision thresholds are set to high values. \ 75 | Make sure you have both user stops at hand!" 76 | ); 77 | println!("After starting try to guide the user robot by hand and see how the other reacts."); 78 | println!("Press Enter to continue..."); 79 | std::io::stdin().read_line(&mut String::new()).unwrap(); 80 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 81 | let thread = std::thread::spawn(|| { 82 | let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 83 | robot_user.joint_motion(0.1, &q_goal).unwrap(); 84 | robot_user 85 | }); 86 | 87 | robot_mirror.joint_motion(0.1, &q_goal)?; 88 | let mut robot_user = thread.join().unwrap(); 89 | 90 | std::thread::spawn(move || { 91 | robot_user.control_torques( 92 | move |state, _step| -> Torques { 93 | sender.send(state.O_T_EE).unwrap(); 94 | Torques::new([0.; 7]) 95 | }, 96 | None, 97 | None, 98 | ) 99 | }); 100 | 101 | robot_mirror.control_torques( 102 | |state: &RobotState, _step: &Duration| -> Torques { 103 | let home: Vector7 = q_goal.into(); 104 | let coriolis: Vector7 = model.coriolis_from_state(&state).into(); 105 | let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, &state); 106 | let jacobian = Matrix6x7::from_column_slice(&jacobian_array); 107 | let q = Vector7::from_column_slice(&state.q); 108 | let dq = Vector7::from_column_slice(&state.dq); 109 | let transform = array_to_isometry(&state.O_T_EE); 110 | let position = transform.translation.vector; 111 | let mut orientation = *transform.rotation.quaternion(); 112 | 113 | desired_state = receiver 114 | .recv_timeout(Duration::from_micros(100)) 115 | .unwrap_or(desired_state); 116 | let desired_transform = array_to_isometry(&desired_state); 117 | position_d = desired_transform.translation.vector; 118 | position_d.y = -position_d.y; 119 | orientation_d = desired_transform.rotation; 120 | let quaternion = Quaternion::::new( 121 | orientation_d.w, 122 | -orientation_d.i, 123 | orientation_d.j, 124 | -orientation_d.k, 125 | ); 126 | orientation_d = UnitQuaternion::from_quaternion(quaternion); 127 | let mut error: Matrix6x1 = Matrix6x1::::zeros(); 128 | { 129 | let mut error_head = error.fixed_slice_mut::(0, 0); 130 | error_head.set_column(0, &(position - position_d)); 131 | } 132 | 133 | if orientation_d.coords.dot(&orientation.coords) < 0. { 134 | orientation.coords = -orientation.coords; 135 | } 136 | let orientation = UnitQuaternion::new_normalize(orientation); 137 | let error_quaternion: UnitQuaternion = orientation.inverse() * orientation_d; 138 | { 139 | let mut error_tail = error.fixed_slice_mut::(3, 0); 140 | error_tail.copy_from( 141 | &-(transform.rotation.to_rotation_matrix() 142 | * Vector3::new(error_quaternion.i, error_quaternion.j, error_quaternion.k)), 143 | ); 144 | } 145 | let nullspace_projection = Matrix7::identity() 146 | - jacobian.transpose() * jacobian.transpose().pseudo_inverse(0.001).unwrap(); 147 | let tau_nullspace = nullspace_projection * NULLSPACE_TORQUE_SCALING * (home - q); 148 | let tau_task: Vector7 = 149 | jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq)); 150 | let tau_d: Vector7 = tau_task + tau_nullspace + coriolis; 151 | 152 | tau_d.into() 153 | }, 154 | None, 155 | None, 156 | ) 157 | } 158 | -------------------------------------------------------------------------------- /examples/print_joint_poses.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use clap::Parser; 4 | use franka::{Frame, FrankaResult, RealtimeConfig, Robot}; 5 | use nalgebra::Matrix4; 6 | 7 | /// An example showing how to use the model library that prints the transformation 8 | /// matrix of each joint with respect to the base frame. 9 | #[derive(Parser, Debug)] 10 | #[clap(author, version, name = "print_joint_poses")] 11 | struct CommandLineArguments { 12 | /// IP-Address or hostname of the robot 13 | pub franka_ip: String, 14 | } 15 | 16 | fn main() -> FrankaResult<()> { 17 | let args = CommandLineArguments::parse(); 18 | let mut robot = Robot::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; 19 | let model = robot.load_model(false)?; 20 | let state = robot.read_once()?; 21 | let frames = vec![ 22 | Frame::Joint1, 23 | Frame::Joint2, 24 | Frame::Joint3, 25 | Frame::Joint4, 26 | Frame::Joint5, 27 | Frame::Joint6, 28 | Frame::Joint7, 29 | Frame::Flange, 30 | Frame::EndEffector, 31 | ]; 32 | for frame in frames { 33 | let pose = Matrix4::from_column_slice(&model.pose_from_state(&frame, &state)); 34 | println!("{} {}", frame, pose); 35 | } 36 | Ok(()) 37 | } 38 | -------------------------------------------------------------------------------- /rustfmt.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/marcbone/libfranka-rs/e7841fbd7a17fcbcc52c9e1d59837360dd0a99b2/rustfmt.toml -------------------------------------------------------------------------------- /src/exception.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains exception and Result definitions 5 | use crate::robot::logger::Record; 6 | use thiserror::Error; 7 | 8 | /// Represents all kind of errors which correspond to the franka::Exception in the C++ version of 9 | /// this library 10 | #[derive(Error, Debug)] 11 | pub enum FrankaException { 12 | /// ControlException is thrown if an error occurs during motion generation or torque control. 13 | /// The exception holds a vector with the last received robot states. The number of recorded 14 | /// states can be configured in the Robot constructor. 15 | #[error("{error}")] 16 | ControlException { 17 | /// Vector of states and commands logged just before the exception occurred. 18 | log: Option>, 19 | /// Explanatory string. 20 | error: String, 21 | }, 22 | 23 | /// IncompatibleVersionException is thrown if the robot does not support this version of libfranka-rs. 24 | #[error("Incompatible library version: Robot has version {server_version:?} and libfranka-rs has {library_version:?}")] 25 | IncompatibleLibraryVersionError { 26 | /// Control's protocol version. 27 | server_version: u16, 28 | /// libfranka-rs protocol version. 29 | library_version: u16, 30 | }, 31 | 32 | /// NoMotionGeneratorRunningError is thrown when trying to send a motion command without an motion generator running. 33 | #[error("Trying to send motion command, but no motion generator running!")] 34 | NoMotionGeneratorRunningError, 35 | 36 | /// NoControllerRunningError is thrown when trying to send a control command without an controller running. 37 | #[error("Trying to send control command, but no controller generator running!")] 38 | NoControllerRunningError, 39 | /// PartialCommandError when trying to send a partial command. 40 | #[error("Trying to send partial command!")] 41 | PartialCommandError, 42 | 43 | /// NetworkException is thrown if a connection to the robot cannot be established, or when a timeout occurs. 44 | #[error("{message:?}")] 45 | NetworkException { message: String }, 46 | 47 | /// CommandException is thrown if an error occurs during command execution. 48 | #[error("{message:?}")] 49 | CommandException { message: String }, 50 | 51 | /// ModelException is thrown if an error occurs when loading the model library 52 | #[error("{message:?}")] 53 | ModelException { message: String }, 54 | 55 | /// RealTimeException is thrown if the real-time priority cannot be set 56 | #[error("{message:?}")] 57 | RealTimeException { message: String }, 58 | } 59 | 60 | /// creates a CommandException from a static string slice 61 | pub(crate) fn create_command_exception(message: &'static str) -> FrankaException { 62 | FrankaException::CommandException { 63 | message: message.to_string(), 64 | } 65 | } 66 | 67 | /// Result type which can have FrankaException as Error 68 | pub type FrankaResult = Result; 69 | // wait for https://github.com/rust-lang/rust/issues/43301 to be closed 70 | // impl Termination for FrankaResult<()> { 71 | // fn report(self) -> i32 { 72 | // return match self { 73 | // Ok(_) => {ExitCode::SUCCESS.report();} 74 | // Err(e) => { 75 | // eprintln!("{}",e); 76 | // ExitCode::FAILURE.report(); 77 | // } 78 | // } 79 | // } 80 | // } 81 | -------------------------------------------------------------------------------- /src/gripper/gripper_state.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains the franka::GripperState type. 5 | 6 | use crate::gripper::types::GripperStateIntern; 7 | use serde::Deserialize; 8 | use serde::Serialize; 9 | use std::time::Duration; 10 | 11 | /// Describes the gripper state. 12 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 13 | pub struct GripperState { 14 | /// Current gripper opening width. Unit: \[m\]. 15 | pub width: f64, 16 | 17 | /// Maximum gripper opening width. 18 | /// This parameter is estimated by homing the gripper. 19 | /// After changing the gripper fingers, a homing needs to be done. Unit: \[m\]. 20 | /// 21 | /// See [`Gripper::homing()`](`crate::Gripper::homing`) 22 | pub max_width: f64, 23 | 24 | /// Indicates whether an object is currently grasped. 25 | pub is_grasped: bool, 26 | 27 | /// Current gripper temperature. Unit: [°C]. 28 | pub temperature: u16, 29 | 30 | /// Strictly monotonically increasing timestamp since robot start. 31 | pub time: Duration, 32 | } 33 | 34 | impl From for GripperState { 35 | fn from(intern: GripperStateIntern) -> Self { 36 | GripperState { 37 | width: intern.width, 38 | max_width: intern.max_width, 39 | is_grasped: intern.is_grasped, 40 | temperature: intern.temperature, 41 | time: Duration::from_millis(intern.message_id as u64), 42 | } 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /src/gripper/types.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use std::fmt::Debug; 5 | 6 | use serde::Deserialize; 7 | use serde::Serialize; 8 | use serde_repr::{Deserialize_repr, Serialize_repr}; 9 | 10 | use crate::network::MessageCommand; 11 | use std::time::Duration; 12 | 13 | pub static VERSION: u16 = 3; 14 | pub static COMMAND_PORT: u16 = 1338; 15 | 16 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 17 | #[repr(u16)] 18 | pub enum Command { 19 | Connect, 20 | Homing, 21 | Grasp, 22 | Move, 23 | Stop, 24 | } 25 | 26 | #[derive(Serialize_repr, Deserialize_repr, Debug)] 27 | #[repr(u16)] 28 | pub enum Status { 29 | Success, 30 | Fail, 31 | Unsuccessful, 32 | Aborted, 33 | } 34 | 35 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 36 | #[repr(packed)] 37 | pub struct GripperStateIntern { 38 | pub message_id: u32, 39 | pub width: f64, 40 | pub max_width: f64, 41 | pub is_grasped: bool, 42 | pub temperature: u16, 43 | } 44 | 45 | impl GripperStateIntern { 46 | pub fn get_time(&self) -> Duration { 47 | Duration::from_millis(self.message_id as u64) 48 | } 49 | } 50 | 51 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 52 | #[repr(packed)] 53 | pub struct CommandHeader { 54 | pub command: Command, 55 | pub command_id: u32, 56 | pub size: u32, 57 | } 58 | 59 | impl CommandHeader { 60 | pub fn new(command: Command, command_id: u32, size: u32) -> CommandHeader { 61 | CommandHeader { 62 | command, 63 | command_id, 64 | size, 65 | } 66 | } 67 | } 68 | 69 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 70 | #[repr(packed)] 71 | pub struct ConnectRequest { 72 | pub version: u16, 73 | pub udp_port: u16, 74 | } 75 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 76 | #[repr(packed)] 77 | pub struct MoveRequest { 78 | width: f64, 79 | speed: f64, 80 | } 81 | 82 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 83 | #[repr(packed)] 84 | pub struct GraspEpsilon { 85 | inner: f64, 86 | outer: f64, 87 | } 88 | 89 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 90 | #[repr(packed)] 91 | pub struct GraspRequest { 92 | width: f64, 93 | epsilon: GraspEpsilon, 94 | speed: f64, 95 | force: f64, 96 | } 97 | 98 | impl ConnectRequest { 99 | pub fn new(udp_port: u16) -> Self { 100 | ConnectRequest { 101 | version: VERSION, 102 | udp_port, 103 | } 104 | } 105 | } 106 | 107 | impl MoveRequest { 108 | pub fn new(width: f64, speed: f64) -> Self { 109 | MoveRequest { width, speed } 110 | } 111 | } 112 | 113 | impl GraspRequest { 114 | pub fn new(width: f64, speed: f64, force: f64, epsilon_inner: f64, epsilon_outer: f64) -> Self { 115 | GraspRequest { 116 | width, 117 | epsilon: GraspEpsilon { 118 | inner: epsilon_inner, 119 | outer: epsilon_outer, 120 | }, 121 | speed, 122 | force, 123 | } 124 | } 125 | } 126 | 127 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 128 | #[repr(packed)] 129 | pub struct ConnectRequestWithHeader { 130 | pub header: CommandHeader, 131 | pub request: ConnectRequest, 132 | } 133 | 134 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 135 | #[repr(packed)] 136 | pub struct MoveRequestWithHeader { 137 | pub header: CommandHeader, 138 | pub request: MoveRequest, 139 | } 140 | 141 | pub type HomingRequestWithHeader = CommandHeader; 142 | pub type StopRequestWithHeader = CommandHeader; 143 | 144 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 145 | #[repr(packed)] 146 | pub struct GraspRequestWithHeader { 147 | pub header: CommandHeader, 148 | pub request: GraspRequest, 149 | } 150 | 151 | impl MessageCommand for CommandHeader { 152 | fn get_command_message_id(&self) -> u32 { 153 | self.command_id 154 | } 155 | } 156 | 157 | impl MessageCommand for ConnectRequestWithHeader { 158 | fn get_command_message_id(&self) -> u32 { 159 | self.header.get_command_message_id() 160 | } 161 | } 162 | 163 | impl MessageCommand for MoveRequestWithHeader { 164 | fn get_command_message_id(&self) -> u32 { 165 | self.header.get_command_message_id() 166 | } 167 | } 168 | 169 | impl MessageCommand for GraspRequestWithHeader { 170 | fn get_command_message_id(&self) -> u32 { 171 | self.header.get_command_message_id() 172 | } 173 | } 174 | 175 | #[derive(Serialize, Deserialize, Debug)] 176 | pub struct ConnectResponse { 177 | pub header: CommandHeader, 178 | pub status: Status, 179 | pub version: u16, 180 | } 181 | 182 | #[derive(Serialize, Deserialize, Debug)] 183 | pub struct MoveResponse { 184 | pub header: CommandHeader, 185 | pub status: Status, 186 | } 187 | 188 | pub type GraspResponse = MoveResponse; 189 | pub type HomingResponse = MoveResponse; 190 | pub type StopResponse = HomingResponse; 191 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! # libfranka-rs 5 | //! libfranka-rs is a library to control [Franka Emika](https://franka.de) research robots. 6 | //! It is an unofficial port of [libfranka](https://github.com/frankaemika/libfranka). 7 | //! 8 | //! **ALWAYS HAVE THE USER STOP BUTTON AT 9 | //! HAND WHILE CONTROLLING THE ROBOT!** 10 | //! 11 | //! 12 | //! ## Design 13 | //! The design of this library is very similar to the original library. All files are 14 | //! named like their libfranka counterparts and contain the same functionality apart from some 15 | //! exceptions. 16 | //! 17 | //! The library is divided into three main Modules: 18 | //! * [gripper](`crate::gripper`) - contains everything which is only needed for controlling the gripper. 19 | //! * [model](`crate::model`) - contains everything needed for using the model. 20 | //! * [robot](`crate::robot`) - contains everything which is only needed for controlling the robot. 21 | //! 22 | //! # Example: 23 | //!```no_run 24 | //! use std::time::Duration; 25 | //! use std::f64::consts::PI; 26 | //! use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; 27 | //! fn main() -> FrankaResult<()> { 28 | //! let mut robot = Robot::new("robotik-bs.de", None, None)?; 29 | //! robot.set_default_behavior()?; 30 | //! robot.set_collision_behavior([20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 31 | //! [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0], 32 | //! [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], 33 | //! [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], [20.0, 20.0, 20.0, 25.0, 25.0, 25.0])?; 34 | //! let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 35 | //! robot.joint_motion(0.5, &q_goal)?; 36 | //! let mut initial_position = JointPositions::new([0.0; 7]); 37 | //! let mut time = 0.; 38 | //! let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { 39 | //! time += time_step.as_secs_f64(); 40 | //! if time == 0. { 41 | //! initial_position.q = state.q_d; 42 | //! } 43 | //! let mut out = JointPositions::new(initial_position.q); 44 | //! let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time)); 45 | //! out.q[3] += delta_angle; 46 | //! out.q[4] += delta_angle; 47 | //! out.q[6] += delta_angle; 48 | //! if time >= 5.0 { 49 | //! return out.motion_finished(); 50 | //! } 51 | //! out 52 | //! }; 53 | //! robot.control_joint_positions(callback, None, None, None) 54 | //! } 55 | //! ``` 56 | //! 57 | //! The main function returns a FrankaResult<()> which means that it returns either Ok(()) 58 | //! or an Error of type FrankaException which correspond to the C++ exceptions in libfranka. 59 | //! 60 | //! ```no_run 61 | //! # use std::time::Duration; 62 | //! # use std::f64::consts::PI; 63 | //! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; 64 | //! # fn main() -> FrankaResult<()> { 65 | //! let mut robot = Robot::new("robotik-bs.de", None, None)?; 66 | //! # Ok(()) 67 | //! # } 68 | //! ``` 69 | //! connects with the robot. You can either provide an IP Address or a hostname. The other options 70 | //! are for setting the RealtimeConfig and the logger size. But we are happy with the defaults so 71 | //! we set them to none. With the "?" we forward eventual errors like "Connection refused" for example 72 | //! 73 | //! 74 | //! ```ignore 75 | //! robot.set_default_behavior()?; 76 | //! robot.set_collision_behavior(...)?; 77 | //! ``` 78 | //! this specifies the default collision behavior, joint impedance and Cartesian impedance 79 | //! 80 | //! ```no_run 81 | //! # use std::time::Duration; 82 | //! # use std::f64::consts::PI; 83 | //! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; 84 | //! # fn main() -> FrankaResult<()> { 85 | //! # let mut robot = Robot::new("robotik-bs.de", None, None)?; 86 | //! let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; 87 | //! robot.joint_motion(0.5, &q_goal)?; 88 | //! # Ok(()) 89 | //! # } 90 | //! ``` 91 | //! These lines specify a joint position goal and moves the robot at 50% of its max speed towards 92 | //! the goal. In this case we use it to bring the robot into his home position 93 | //! 94 | //! ```no_run 95 | //! # use franka::JointPositions; 96 | //! let mut initial_position = JointPositions::new([0.0; 7]); 97 | //! ``` 98 | //! Here we define a variable for the initial joint position. You cannot set the right values 99 | //! here because they will change until you are in the control loop. Therefore it is necessary 100 | //! specify them at the first time the control loop is executed like here: 101 | //! ```no_run 102 | //! # use franka::{JointPositions, RobotState}; 103 | //! # let mut initial_position = JointPositions::new([0.0; 7]); 104 | //! # let time = 0.; 105 | //! # let state = RobotState::default(); 106 | //! if time == 0. { 107 | //! initial_position.q = state.q_d; 108 | //! } 109 | //! ``` 110 | //! 111 | //! ```no_run 112 | //! # use franka::JointPositions; 113 | //! # use std::f64::consts::PI; 114 | //! # let time = 0.; 115 | //! # let delta_angle = 0.; 116 | //! # let mut initial_position = JointPositions::new([0.0; 7]); 117 | //! let mut out = JointPositions::new(initial_position.q); 118 | //! let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time)); 119 | //! out.q[3] += delta_angle; 120 | //! out.q[4] += delta_angle; 121 | //! out.q[6] += delta_angle; 122 | //! ``` 123 | //! Here we define our desired JointPositions. It is important that we provide a smooth signal 124 | //! to the robot, therefore we use a cosine function. Without it we would get a CommandException 125 | //! while running. 126 | //! 127 | //! ```no_run 128 | //! # use franka::{JointPositions, MotionFinished}; 129 | //! # fn joint_positions() -> JointPositions { 130 | //! # let time = 0.; 131 | //! # let mut out = JointPositions::new([0.;7]); 132 | //! 133 | //! if time >= 5.0 { 134 | //! return out.motion_finished(); 135 | //! } 136 | //! out 137 | //! # } 138 | //! ``` 139 | //! Until 5 seconds have passed we just return our JointPositions. As we want to stop the control 140 | //! loop after 5 seconds. We tell the robot that this is our last command and that we want to exit 141 | //! the control loop. 142 | //! 143 | //! 144 | //! All of this we put inside our closure (they are called lambda-functions in C++) callback 145 | //! ```ignore 146 | //! let callback = |state: &RobotState, time_step: &Duration| -> JointPositions {...} 147 | //! ``` 148 | //! Our callback Takes a immutable Reference to a RobotState and a immutable reference to the passed time 149 | //! since the callback was executed the last time (the time is zero at the first call of the function) 150 | //! and returns JointPositions. 151 | //! 152 | //! With this callback we can now control the joint positions of the robot: 153 | //! ```no_run 154 | //! # use std::time::Duration; 155 | //! # use std::f64::consts::PI; 156 | //! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; 157 | //! # fn main() -> FrankaResult<()> { 158 | //! # let mut robot = Robot::new("robotik-bs.de", None, None)?; 159 | //! # let callback = |state: &RobotState, time_step: &Duration| -> JointPositions {JointPositions::new([0.;7])}; 160 | //! robot.control_joint_positions(callback, None, None, None) 161 | //! # } 162 | //! ``` 163 | //! There are optional arguments for specifying the Controller mode, rate limiting and the cutoff frequency. 164 | //! As we are happy with the defaults we set them to None. Note that this function returns a FrankaResult. 165 | //! As it is the last statement of the main method we do not have to forward the error with a "?". 166 | pub mod exception; 167 | pub mod gripper; 168 | mod network; 169 | pub mod robot; 170 | 171 | pub mod model; 172 | pub mod utils; 173 | 174 | pub use exception::FrankaResult; 175 | pub use gripper::gripper_state::GripperState; 176 | pub use gripper::Gripper; 177 | pub use model::Frame; 178 | pub use model::Model; 179 | pub use robot::control_types::*; 180 | pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; 181 | pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; 182 | pub use robot::robot_state::RobotState; 183 | pub use robot::Robot; 184 | pub use utils::*; 185 | -------------------------------------------------------------------------------- /src/model.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains model library types. 5 | use nalgebra::Matrix4; 6 | 7 | use crate::model::model_library::ModelLibrary; 8 | use crate::robot::robot_state::RobotState; 9 | use crate::FrankaResult; 10 | use std::fmt; 11 | use std::path::Path; 12 | 13 | pub(crate) mod library_downloader; 14 | mod model_library; 15 | 16 | /// Enumerates the seven joints, the flange, and the end effector of a robot. 17 | pub enum Frame { 18 | Joint1, 19 | Joint2, 20 | Joint3, 21 | Joint4, 22 | Joint5, 23 | Joint6, 24 | Joint7, 25 | Flange, 26 | EndEffector, 27 | Stiffness, 28 | } 29 | 30 | impl 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. 68 | pub struct Model { 69 | library: ModelLibrary, 70 | } 71 | 72 | #[allow(non_snake_case)] 73 | impl 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 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 - Apparently it gets somehow included when using \ ¯\_(ツ)_/¯ 102 | pub fn new>( 103 | model_filename: S, 104 | libm_filename: Option<&Path>, 105 | ) -> FrankaResult { 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 = 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 = 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 = 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 | /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), 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 | /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), 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>>( 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>>( 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 | } 397 | -------------------------------------------------------------------------------- /src/model/library_downloader.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use crate::exception::FrankaException::ModelException; 4 | use crate::network::Network; 5 | use crate::robot::service_types::{ 6 | LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, 7 | LoadModelLibraryResponse, LoadModelLibrarySystem, RobotCommandEnum, 8 | }; 9 | use crate::FrankaResult; 10 | use std::fmt; 11 | use std::fmt::Display; 12 | use std::fmt::Formatter; 13 | use std::fs::File; 14 | use std::io::Write; 15 | use std::mem::size_of; 16 | use std::path::Path; 17 | 18 | pub struct LibraryDownloader {} 19 | 20 | impl LibraryDownloader { 21 | pub fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { 22 | if cfg!(all(target_os = "linux", target_arch = "x86_64")) { 23 | let command = LoadModelLibraryRequestWithHeader { 24 | header: network.create_header_for_robot( 25 | RobotCommandEnum::LoadModelLibrary, 26 | size_of::(), 27 | ), 28 | request: LoadModelLibraryRequest { 29 | architecture: LoadModelLibraryArchitecture::X64, 30 | system: LoadModelLibrarySystem::Linux, 31 | }, 32 | }; 33 | let command_id: u32 = network.tcp_send_request(command); 34 | let mut buffer = Vec::::new(); 35 | let _response: LoadModelLibraryResponse = 36 | network.tcp_blocking_receive_load_library_response(command_id, &mut buffer)?; 37 | let mut file = File::create(download_path).map_err(|_| ModelException { 38 | message: "Error writing model to disk:".to_string(), 39 | })?; 40 | file.write(&buffer).map_err(|_| ModelException { 41 | message: "Error writing model to disk:".to_string(), 42 | })?; 43 | Ok(()) 44 | } else { 45 | Err(ModelException { 46 | message: 47 | "Your platform is not yet supported for Downloading models. Please use Linux on\ 48 | x86_64 for now" 49 | .to_string(), 50 | }) 51 | } 52 | } 53 | } 54 | 55 | #[derive(Debug)] 56 | pub struct UnsupportedPlatform {} 57 | 58 | impl std::error::Error for UnsupportedPlatform {} 59 | 60 | impl Display for UnsupportedPlatform { 61 | fn fmt(&self, f: &mut Formatter) -> fmt::Result { 62 | writeln!( 63 | f, 64 | "Your platform is not yet supported for Downloading models. Please use Linux on\ 65 | x86_64 for now" 66 | ) 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /src/model/model_library.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use crate::exception::FrankaException; 4 | use crate::exception::FrankaException::ModelException; 5 | use crate::FrankaResult; 6 | use libc::c_double; 7 | use libloading as lib; 8 | use libloading::os::unix::Library; 9 | use libloading::os::unix::Symbol; 10 | use std::path::Path; 11 | 12 | #[allow(non_snake_case, dead_code)] 13 | pub struct ModelLibrary { 14 | libm: Library, 15 | lib_model: Library, 16 | M_NE: Symbol< 17 | unsafe extern "C" fn( 18 | *const c_double, 19 | *const c_double, 20 | c_double, 21 | *const c_double, 22 | *mut c_double, 23 | ), 24 | >, 25 | Ji_J_J1: Symbol, 26 | Ji_J_J2: Symbol, 27 | Ji_J_J3: Symbol, 28 | Ji_J_J4: Symbol, 29 | Ji_J_J5: Symbol, 30 | Ji_J_J6: Symbol, 31 | Ji_J_J7: Symbol, 32 | Ji_J_J8: Symbol, 33 | Ji_J_J9: Symbol, 34 | 35 | O_J_J1: Symbol, 36 | O_J_J2: Symbol, 37 | O_J_J3: Symbol, 38 | O_J_J4: Symbol, 39 | O_J_J5: Symbol, 40 | O_J_J6: Symbol, 41 | O_J_J7: Symbol, 42 | O_J_J8: Symbol, 43 | O_J_J9: Symbol, 44 | 45 | O_T_J1: Symbol, 46 | O_T_J2: Symbol, 47 | O_T_J3: Symbol, 48 | O_T_J4: Symbol, 49 | O_T_J5: Symbol, 50 | O_T_J6: Symbol, 51 | O_T_J7: Symbol, 52 | O_T_J8: Symbol, 53 | O_T_J9: Symbol, 54 | 55 | c_NE: Symbol< 56 | unsafe extern "C" fn( 57 | *const c_double, 58 | *const c_double, 59 | *const c_double, 60 | c_double, 61 | *const c_double, 62 | *mut c_double, 63 | ), 64 | >, 65 | g_NE: Symbol< 66 | unsafe extern "C" fn( 67 | *const c_double, 68 | *const c_double, 69 | c_double, 70 | *const c_double, 71 | *mut c_double, 72 | ), 73 | >, 74 | } 75 | 76 | #[allow(non_snake_case, dead_code)] 77 | impl ModelLibrary { 78 | pub fn new(model_filename: &Path, libm_filename: Option<&Path>) -> FrankaResult { 79 | let libm_filename = match libm_filename { 80 | Some(filename) => filename, 81 | None => Path::new("libm.so.6"), 82 | }; 83 | unsafe { 84 | let libm = lib::os::unix::Library::open( 85 | Some(libm_filename), 86 | libc::RTLD_NOW | libc::RTLD_GLOBAL, 87 | ) 88 | .map_err(|_| FrankaException::ModelException { 89 | message: "lifbranka-rs: Can not open libm library".to_string(), 90 | })?; 91 | let lib_model = lib::os::unix::Library::open(Some(model_filename), libc::RTLD_NOW) 92 | .map_err(|_| FrankaException::ModelException { 93 | message: "lifbranka-rs: Can not open model library".to_string(), 94 | })?; 95 | 96 | let M_NE = ModelLibrary::get_symbol(&lib_model, "M_NE")?; 97 | let Ji_J_J1 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J1")?; 98 | let Ji_J_J2 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J2")?; 99 | let Ji_J_J3 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J3")?; 100 | let Ji_J_J4 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J4")?; 101 | let Ji_J_J5 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J5")?; 102 | let Ji_J_J6 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J6")?; 103 | let Ji_J_J7 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J7")?; 104 | let Ji_J_J8 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J8")?; 105 | let Ji_J_J9 = ModelLibrary::get_symbol(&lib_model, "Ji_J_J9")?; 106 | 107 | let O_J_J1 = ModelLibrary::get_symbol(&lib_model, "O_J_J1")?; 108 | let O_J_J2 = ModelLibrary::get_symbol(&lib_model, "O_J_J2")?; 109 | let O_J_J3 = ModelLibrary::get_symbol(&lib_model, "O_J_J3")?; 110 | let O_J_J4 = ModelLibrary::get_symbol(&lib_model, "O_J_J4")?; 111 | let O_J_J5 = ModelLibrary::get_symbol(&lib_model, "O_J_J5")?; 112 | let O_J_J6 = ModelLibrary::get_symbol(&lib_model, "O_J_J6")?; 113 | let O_J_J7 = ModelLibrary::get_symbol(&lib_model, "O_J_J7")?; 114 | let O_J_J8 = ModelLibrary::get_symbol(&lib_model, "O_J_J8")?; 115 | let O_J_J9 = ModelLibrary::get_symbol(&lib_model, "O_J_J9")?; 116 | 117 | let O_T_J1 = ModelLibrary::get_symbol(&lib_model, "O_T_J1")?; 118 | let O_T_J2 = ModelLibrary::get_symbol(&lib_model, "O_T_J2")?; 119 | let O_T_J3 = ModelLibrary::get_symbol(&lib_model, "O_T_J3")?; 120 | let O_T_J4 = ModelLibrary::get_symbol(&lib_model, "O_T_J4")?; 121 | let O_T_J5 = ModelLibrary::get_symbol(&lib_model, "O_T_J5")?; 122 | let O_T_J6 = ModelLibrary::get_symbol(&lib_model, "O_T_J6")?; 123 | let O_T_J7 = ModelLibrary::get_symbol(&lib_model, "O_T_J7")?; 124 | let O_T_J8 = ModelLibrary::get_symbol(&lib_model, "O_T_J8")?; 125 | let O_T_J9 = ModelLibrary::get_symbol(&lib_model, "O_T_J9")?; 126 | 127 | let c_NE = ModelLibrary::get_symbol(&lib_model, "c_NE")?; 128 | let g_NE = ModelLibrary::get_symbol(&lib_model, "g_NE")?; 129 | 130 | Ok(ModelLibrary { 131 | libm, 132 | lib_model, 133 | M_NE, 134 | Ji_J_J1, 135 | Ji_J_J2, 136 | Ji_J_J3, 137 | Ji_J_J4, 138 | Ji_J_J5, 139 | Ji_J_J6, 140 | Ji_J_J7, 141 | Ji_J_J8, 142 | Ji_J_J9, 143 | 144 | O_J_J1, 145 | O_J_J2, 146 | O_J_J3, 147 | O_J_J4, 148 | O_J_J5, 149 | O_J_J6, 150 | O_J_J7, 151 | O_J_J8, 152 | O_J_J9, 153 | 154 | O_T_J1, 155 | O_T_J2, 156 | O_T_J3, 157 | O_T_J4, 158 | O_T_J5, 159 | O_T_J6, 160 | O_T_J7, 161 | O_T_J8, 162 | O_T_J9, 163 | 164 | c_NE, 165 | g_NE, 166 | }) 167 | } 168 | } 169 | pub fn mass( 170 | &self, 171 | q: &[f64; 7], 172 | I_load: &[f64; 9], 173 | m_load: &f64, 174 | F_x_Cload: &[f64; 3], 175 | m_ne: &mut [f64; 49], 176 | ) { 177 | unsafe { 178 | (self.M_NE)( 179 | q.as_ptr(), 180 | I_load.as_ptr(), 181 | *m_load, 182 | F_x_Cload.as_ptr(), 183 | m_ne.as_mut_ptr(), 184 | ) 185 | } 186 | } 187 | pub fn body_jacobian_joint1(&self, b_Ji_J_J1: &mut [f64; 42]) { 188 | unsafe { (self.Ji_J_J1)(b_Ji_J_J1.as_mut_ptr()) } 189 | } 190 | pub fn body_jacobian_joint2(&self, q: &[f64; 7], b_Ji_J_J2: &mut [f64; 42]) { 191 | unsafe { (self.Ji_J_J2)(q.as_ptr(), b_Ji_J_J2.as_mut_ptr()) } 192 | } 193 | pub fn body_jacobian_joint3(&self, q: &[f64; 7], b_Ji_J_J3: &mut [f64; 42]) { 194 | unsafe { (self.Ji_J_J3)(q.as_ptr(), b_Ji_J_J3.as_mut_ptr()) } 195 | } 196 | pub fn body_jacobian_joint4(&self, q: &[f64; 7], b_Ji_J_J4: &mut [f64; 42]) { 197 | unsafe { (self.Ji_J_J4)(q.as_ptr(), b_Ji_J_J4.as_mut_ptr()) } 198 | } 199 | pub fn body_jacobian_joint5(&self, q: &[f64; 7], b_Ji_J_J5: &mut [f64; 42]) { 200 | unsafe { (self.Ji_J_J5)(q.as_ptr(), b_Ji_J_J5.as_mut_ptr()) } 201 | } 202 | pub fn body_jacobian_joint6(&self, q: &[f64; 7], b_Ji_J_J6: &mut [f64; 42]) { 203 | unsafe { (self.Ji_J_J6)(q.as_ptr(), b_Ji_J_J6.as_mut_ptr()) } 204 | } 205 | pub fn body_jacobian_joint7(&self, q: &[f64; 7], b_Ji_J_J7: &mut [f64; 42]) { 206 | unsafe { (self.Ji_J_J7)(q.as_ptr(), b_Ji_J_J7.as_mut_ptr()) } 207 | } 208 | pub fn body_jacobian_flange(&self, q: &[f64; 7], b_Ji_J_J8: &mut [f64; 42]) { 209 | unsafe { (self.Ji_J_J8)(q.as_ptr(), b_Ji_J_J8.as_mut_ptr()) } 210 | } 211 | pub fn body_jacobian_ee(&self, q: &[f64; 7], F_T_EE: &[f64; 16], b_Ji_J_J9: &mut [f64; 42]) { 212 | unsafe { (self.Ji_J_J9)(q.as_ptr(), F_T_EE.as_ptr(), b_Ji_J_J9.as_mut_ptr()) } 213 | } 214 | 215 | pub fn zero_jacobian_joint1(&self, b_O_J_J1: &mut [f64; 42]) { 216 | unsafe { (self.O_J_J1)(b_O_J_J1.as_mut_ptr()) } 217 | } 218 | pub fn zero_jacobian_joint2(&self, q: &[f64; 7], b_O_J_J2: &mut [f64; 42]) { 219 | unsafe { (self.O_J_J2)(q.as_ptr(), b_O_J_J2.as_mut_ptr()) } 220 | } 221 | pub fn zero_jacobian_joint3(&self, q: &[f64; 7], b_O_J_J3: &mut [f64; 42]) { 222 | unsafe { (self.O_J_J3)(q.as_ptr(), b_O_J_J3.as_mut_ptr()) } 223 | } 224 | pub fn zero_jacobian_joint4(&self, q: &[f64; 7], b_O_J_J4: &mut [f64; 42]) { 225 | unsafe { (self.O_J_J4)(q.as_ptr(), b_O_J_J4.as_mut_ptr()) } 226 | } 227 | pub fn zero_jacobian_joint5(&self, q: &[f64; 7], b_O_J_J5: &mut [f64; 42]) { 228 | unsafe { (self.O_J_J5)(q.as_ptr(), b_O_J_J5.as_mut_ptr()) } 229 | } 230 | pub fn zero_jacobian_joint6(&self, q: &[f64; 7], b_O_J_J6: &mut [f64; 42]) { 231 | unsafe { (self.O_J_J6)(q.as_ptr(), b_O_J_J6.as_mut_ptr()) } 232 | } 233 | pub fn zero_jacobian_joint7(&self, q: &[f64; 7], b_O_J_J7: &mut [f64; 42]) { 234 | unsafe { (self.O_J_J7)(q.as_ptr(), b_O_J_J7.as_mut_ptr()) } 235 | } 236 | pub fn zero_jacobian_flange(&self, q: &[f64; 7], b_O_J_J8: &mut [f64; 42]) { 237 | unsafe { (self.O_J_J8)(q.as_ptr(), b_O_J_J8.as_mut_ptr()) } 238 | } 239 | pub fn zero_jacobian_ee(&self, q: &[f64; 7], F_T_EE: &[f64; 16], b_O_J_J9: &mut [f64; 42]) { 240 | unsafe { (self.O_J_J9)(q.as_ptr(), F_T_EE.as_ptr(), b_O_J_J9.as_mut_ptr()) } 241 | } 242 | 243 | pub fn joint1(&self, q: &[f64; 7], b_O_T_J1: &mut [f64; 16]) { 244 | unsafe { (self.O_T_J1)(q.as_ptr(), b_O_T_J1.as_mut_ptr()) } 245 | } 246 | pub fn joint2(&self, q: &[f64; 7], b_O_T_J2: &mut [f64; 16]) { 247 | unsafe { (self.O_T_J2)(q.as_ptr(), b_O_T_J2.as_mut_ptr()) } 248 | } 249 | pub fn joint3(&self, q: &[f64; 7], b_O_T_J3: &mut [f64; 16]) { 250 | unsafe { (self.O_T_J3)(q.as_ptr(), b_O_T_J3.as_mut_ptr()) } 251 | } 252 | pub fn joint4(&self, q: &[f64; 7], b_O_T_J4: &mut [f64; 16]) { 253 | unsafe { (self.O_T_J4)(q.as_ptr(), b_O_T_J4.as_mut_ptr()) } 254 | } 255 | pub fn joint5(&self, q: &[f64; 7], b_O_T_J5: &mut [f64; 16]) { 256 | unsafe { (self.O_T_J5)(q.as_ptr(), b_O_T_J5.as_mut_ptr()) } 257 | } 258 | pub fn joint6(&self, q: &[f64; 7], b_O_T_J6: &mut [f64; 16]) { 259 | unsafe { (self.O_T_J6)(q.as_ptr(), b_O_T_J6.as_mut_ptr()) } 260 | } 261 | pub fn joint7(&self, q: &[f64; 7], b_O_T_J7: &mut [f64; 16]) { 262 | unsafe { (self.O_T_J7)(q.as_ptr(), b_O_T_J7.as_mut_ptr()) } 263 | } 264 | pub fn flange(&self, q: &[f64; 7], b_O_T_J8: &mut [f64; 16]) { 265 | unsafe { (self.O_T_J8)(q.as_ptr(), b_O_T_J8.as_mut_ptr()) } 266 | } 267 | pub fn ee(&self, q: &[f64; 7], F_T_EE: &[f64; 16], b_O_T_J9: &mut [f64; 16]) { 268 | unsafe { (self.O_T_J9)(q.as_ptr(), F_T_EE.as_ptr(), b_O_T_J9.as_mut_ptr()) } 269 | } 270 | 271 | pub fn coriolis( 272 | &self, 273 | q: &[f64; 7], 274 | qd: &[f64; 7], 275 | I_load: &[f64; 9], 276 | m_load: &f64, 277 | F_x_Cload: &[f64; 3], 278 | c_NE: &mut [f64; 7], 279 | ) { 280 | unsafe { 281 | (self.c_NE)( 282 | q.as_ptr(), 283 | qd.as_ptr(), 284 | I_load.as_ptr(), 285 | *m_load, 286 | F_x_Cload.as_ptr(), 287 | c_NE.as_mut_ptr(), 288 | ) 289 | } 290 | } 291 | pub fn gravity( 292 | &self, 293 | q: &[f64; 7], 294 | g_earth: &[f64; 3], 295 | m_load: &f64, 296 | F_x_Cload: &[f64; 3], 297 | g_NE: &mut [f64; 7], 298 | ) { 299 | unsafe { 300 | (self.g_NE)( 301 | q.as_ptr(), 302 | g_earth.as_ptr(), 303 | *m_load, 304 | F_x_Cload.as_ptr(), 305 | g_NE.as_mut_ptr(), 306 | ) 307 | } 308 | } 309 | fn get_symbol(lib: &Library, symbol: &str) -> FrankaResult> { 310 | unsafe { 311 | lib.get(symbol.as_bytes()).map_err(|_| ModelException { 312 | message: format!("libfranka-rs: Symbol cannot be found: {}", symbol), 313 | }) 314 | } 315 | } 316 | } 317 | 318 | #[cfg(test)] 319 | mod test { 320 | use crate::exception::FrankaException; 321 | use crate::model::ModelLibrary; 322 | use std::path::Path; 323 | 324 | #[test] 325 | fn can_generate_model_exception() { 326 | match ModelLibrary::new(Path::new("/dev/null"), None) { 327 | Err(FrankaException::ModelException { message: _ }) => {} 328 | _ => panic!(), 329 | }; 330 | } 331 | 332 | #[test] 333 | #[ignore] 334 | fn m_ne_test() { 335 | let model = 336 | ModelLibrary::new(Path::new("/home/marco/franka_model/model.so"), None).unwrap(); 337 | let c = 5.; 338 | let a = [0., 0., 0., 0., 0., 0., 0.]; 339 | let b = [0.; 9]; 340 | let d = [0., 0., 0.]; 341 | let mut e = [0.; 49]; 342 | model.mass(&a, &b, &c, &d, &mut e); 343 | println!("{}", e[0]); 344 | } 345 | 346 | #[test] 347 | #[allow(non_snake_case)] 348 | #[ignore] 349 | fn coriolis_test() { 350 | let model = 351 | ModelLibrary::new(Path::new("/home/marco/franka_model/model.so"), None).unwrap(); 352 | let mut c_NE = [0.; 7]; 353 | let q = [0.; 7]; 354 | let qd = [0.; 7]; 355 | let I_load = [0.; 9]; 356 | let m_load = 0.; 357 | let F_x_Cload = [0.; 3]; 358 | model.coriolis(&q, &qd, &I_load, &m_load, &F_x_Cload, &mut c_NE); 359 | } 360 | } 361 | -------------------------------------------------------------------------------- /src/network.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #![allow(dead_code)] 4 | 5 | extern crate libc; 6 | extern crate nix; 7 | 8 | use std::collections::HashMap; 9 | use std::error::Error; 10 | use std::fmt::Debug; 11 | use std::io::{Read, Write}; 12 | use std::mem::size_of; 13 | use std::net::TcpStream as StdTcpStream; 14 | use std::net::{IpAddr, SocketAddr, ToSocketAddrs}; 15 | use std::os::unix::io::AsRawFd; 16 | use std::str::FromStr; 17 | use std::time::Duration; 18 | 19 | use mio::net::{TcpStream, UdpSocket}; 20 | use mio::{Events, Interest, Poll, Token}; 21 | 22 | use nix::sys::socket::setsockopt; 23 | use nix::sys::socket::sockopt::{KeepAlive, TcpKeepCount, TcpKeepIdle, TcpKeepInterval}; 24 | 25 | use serde::de::DeserializeOwned; 26 | use serde::Serialize; 27 | 28 | use crate::exception::{FrankaException, FrankaResult}; 29 | use crate::gripper; 30 | use crate::gripper::types::CommandHeader; 31 | use crate::robot::service_types::{ 32 | LoadModelLibraryResponse, LoadModelLibraryStatus, RobotCommandEnum, RobotCommandHeader, 33 | }; 34 | 35 | const CLIENT: Token = Token(1); 36 | 37 | pub enum NetworkType { 38 | Robot, 39 | Gripper, 40 | } 41 | 42 | pub trait MessageCommand { 43 | fn get_command_message_id(&self) -> u32; 44 | } 45 | 46 | pub struct Network { 47 | network_type: NetworkType, 48 | tcp_socket: TcpStream, 49 | udp_socket: UdpSocket, 50 | udp_server_address: SocketAddr, 51 | udp_port: u16, 52 | udp_timeout: Duration, 53 | pub command_id: u32, 54 | pending_response: Vec, 55 | pending_response_offset: usize, 56 | pending_response_len: usize, 57 | pending_command_id: u32, 58 | received_responses: HashMap>, 59 | poll_read: Poll, 60 | events: Events, 61 | poll_read_udp: Poll, 62 | events_udp: Events, 63 | } 64 | 65 | impl Network { 66 | pub fn new( 67 | network_type: NetworkType, 68 | franka_address: &str, 69 | franka_port: u16, 70 | ) -> Result> { 71 | let address_str: String = format!("{}:{}", franka_address, franka_port); 72 | let sock_address = address_str.to_socket_addrs().unwrap().next().unwrap(); 73 | let mut tcp_socket = TcpStream::from_std(StdTcpStream::connect(sock_address)?); 74 | let fd = tcp_socket.as_raw_fd(); 75 | 76 | setsockopt(fd, KeepAlive, &true)?; 77 | setsockopt(fd, TcpKeepIdle, &1)?; 78 | setsockopt(fd, TcpKeepCount, &3)?; 79 | setsockopt(fd, TcpKeepInterval, &1)?; 80 | 81 | let udp_timeout = Duration::from_secs(1); // TODO: offer in constructor 82 | let ip_addr = IpAddr::from_str("0.0.0.0")?; 83 | let udp_server_address = SocketAddr::new(ip_addr, 0); 84 | 85 | let mut udp_socket = UdpSocket::bind(udp_server_address)?; 86 | let udp_port = udp_socket.local_addr()?.port(); 87 | 88 | let command_id = 0; 89 | let pending_response: Vec = Vec::new(); 90 | let pending_response_offset = 0; 91 | let pending_command_id = 0; 92 | let received_responses = HashMap::new(); 93 | let poll_read = Poll::new()?; 94 | poll_read 95 | .registry() 96 | .register(&mut tcp_socket, CLIENT, Interest::READABLE)?; 97 | let poll_read_udp = Poll::new()?; 98 | poll_read_udp 99 | .registry() 100 | .register(&mut udp_socket, CLIENT, Interest::READABLE)?; 101 | let events = Events::with_capacity(128); 102 | let events_udp = Events::with_capacity(1); 103 | Ok(Network { 104 | network_type, 105 | tcp_socket, 106 | udp_socket, 107 | udp_server_address, 108 | udp_port, 109 | udp_timeout, 110 | command_id, 111 | pending_response, 112 | pending_response_offset, 113 | pending_command_id, 114 | pending_response_len: 0, 115 | received_responses, 116 | poll_read, 117 | events, 118 | poll_read_udp, 119 | events_udp, 120 | }) 121 | } 122 | pub fn create_header_for_gripper( 123 | &mut self, 124 | command: gripper::types::Command, 125 | size: usize, 126 | ) -> CommandHeader { 127 | let header = gripper::types::CommandHeader::new(command, self.command_id, size as u32); 128 | self.command_id += 1; 129 | header 130 | } 131 | pub fn create_header_for_robot( 132 | &mut self, 133 | command: RobotCommandEnum, 134 | size: usize, 135 | ) -> RobotCommandHeader { 136 | let header = RobotCommandHeader::new(command, self.command_id, size as u32); 137 | self.command_id += 1; 138 | header 139 | } 140 | 141 | pub fn tcp_send_request( 142 | &mut self, 143 | request: T, 144 | ) -> u32 { 145 | let encoded_request = serialize(&request); 146 | self.tcp_socket.write_all(&encoded_request).unwrap(); 147 | request.get_command_message_id() 148 | } 149 | /// Blocks until a Response message with the given command ID has been received and returns this 150 | /// response. 151 | /// 152 | /// # Arguments 153 | /// * `command_id` - Expected command ID of the Response. 154 | pub fn tcp_blocking_receive_response( 155 | &mut self, 156 | command_id: u32, 157 | ) -> T { 158 | let response_bytes = self.wait_for_response_to_arrive(&command_id); 159 | deserialize(&response_bytes) 160 | } 161 | /// Blocks until a LoadModelLibraryResponse message with the given command ID has been received 162 | /// and returns this LoadModelLibraryResponse. 163 | /// # Arguments 164 | /// * `command_id` - Expected command ID of the Response. 165 | /// * `buffer` - variable-length data for the expected LoadModelLibraryResponse message (if 166 | /// any has been received) is copied into it. 167 | /// 168 | /// # Error 169 | /// * [`ModelException`](`crate::exception::FrankaException::ModelException`) - if the 170 | /// model could not be downloaded successfully. 171 | /// 172 | pub fn tcp_blocking_receive_load_library_response( 173 | &mut self, 174 | command_id: u32, 175 | buffer: &mut Vec, 176 | ) -> FrankaResult { 177 | let response_bytes = self.wait_for_response_to_arrive(&command_id); 178 | let lib_response: LoadModelLibraryResponse = 179 | deserialize(&response_bytes[0..size_of::()]); 180 | match lib_response.status { 181 | LoadModelLibraryStatus::Success => {} 182 | LoadModelLibraryStatus::Error => { 183 | return Err(FrankaException::ModelException { 184 | message: "libfranka-rs: Server reports error when loading model library." 185 | .to_string(), 186 | }); 187 | } 188 | } 189 | assert_ne!( 190 | lib_response.header.size as usize, 191 | size_of::() 192 | ); 193 | buffer.append(&mut Vec::from( 194 | &response_bytes[size_of::()..], 195 | )); 196 | Ok(lib_response) 197 | } 198 | /// Tries to receive a Response message with the given command ID (non-blocking). 199 | /// 200 | /// # Arguments 201 | /// * `command_id` - Expected command ID of the response 202 | /// * `handler` - Callback to be invoked if the expected response has been received. 203 | /// 204 | /// # Return 205 | /// * true - if everything worked as expected 206 | /// * false - if the message could not be received 207 | /// 208 | /// # Error 209 | /// * [`FrankaException`](`crate::exception::FrankaException`) - if the handler returns an exception 210 | pub fn tcp_receive_response( 211 | &mut self, 212 | command_id: u32, 213 | handler: F, 214 | ) -> Result 215 | where 216 | F: FnOnce(T) -> Result<(), FrankaException>, 217 | T: DeserializeOwned + Debug + 'static, 218 | { 219 | self.tcp_read_from_buffer(Duration::from_micros(0)); 220 | let message = self.received_responses.get(&command_id); 221 | if message.is_none() { 222 | return Ok(false); 223 | } 224 | if message.unwrap().len() != size_of::() { 225 | panic!("libfranka-rs: Incorrect TCP message size."); 226 | } 227 | let message: T = deserialize(message.unwrap()); 228 | let result = handler(message); 229 | match result { 230 | Ok(_) => { 231 | self.received_responses.remove(&command_id); 232 | Ok(true) 233 | } 234 | Err(e) => Err(e), 235 | } 236 | } 237 | 238 | fn wait_for_response_to_arrive(&mut self, command_id: &u32) -> Vec { 239 | let mut response_bytes: Option> = None; 240 | while response_bytes == None { 241 | { 242 | self.tcp_read_from_buffer(Duration::from_millis(10)); 243 | response_bytes = self.received_responses.remove(command_id); 244 | } 245 | std::thread::yield_now(); 246 | } 247 | response_bytes.unwrap() 248 | } 249 | pub fn udp_receive(&mut self) -> Option { 250 | // TODO replace Vec with array when https://github.com/rust-lang/rust/issues/43408 251 | // is fixed 252 | let mut buffer: Vec = vec![0; size_of::()]; 253 | let available_bytes = self.udp_socket.peek(&mut buffer).ok()?; 254 | if available_bytes >= size_of::() { 255 | let object: Option = match self.udp_blocking_receive() { 256 | Ok(o) => Some(o), 257 | Err(_) => None, 258 | }; 259 | return object; 260 | } 261 | None 262 | } 263 | pub fn udp_blocking_receive( 264 | &mut self, 265 | ) -> FrankaResult { 266 | self.poll_read_udp 267 | .poll(&mut self.events_udp, Some(self.udp_timeout)) 268 | .unwrap(); 269 | for event in self.events_udp.iter() { 270 | match event.token() { 271 | CLIENT => { 272 | if event.is_readable() { 273 | let mut buffer: Vec = vec![0; size_of::()]; 274 | let read_bytes_and_address = self.udp_socket.recv_from(&mut buffer); 275 | while self.udp_socket.recv_from(&mut buffer).is_ok() {} 276 | let read_bytes = match read_bytes_and_address { 277 | Ok(res) => { 278 | self.udp_server_address = res.1; 279 | res.0 280 | } 281 | Err(e) => { 282 | return Err(FrankaException::NetworkException { 283 | message: e.to_string(), 284 | }); 285 | } 286 | }; 287 | if read_bytes != size_of::() { 288 | return Err(FrankaException::NetworkException { message: format!("UDP object could not be received: object has {} bytes but it should have {} bytes", read_bytes, size_of::()) }); 289 | } 290 | return Ok(deserialize(&buffer)); 291 | } 292 | } 293 | _ => unreachable!(), 294 | } 295 | } 296 | Err(FrankaException::NetworkException { 297 | message: "libfranka-rs: UDP receive: timeout".to_string(), 298 | }) 299 | } 300 | pub fn udp_send( 301 | &mut self, 302 | data: &T, 303 | ) -> FrankaResult<()> { 304 | let bytes_send = self 305 | .udp_socket 306 | .send_to(&serialize(data), self.udp_server_address) 307 | .map_err(|e| FrankaException::NetworkException { 308 | message: e.to_string(), 309 | })?; 310 | if bytes_send != size_of::() { 311 | return Err(FrankaException::NetworkException { 312 | message: "libfranka-rs: UDP object could not be send".to_string(), 313 | }); 314 | } 315 | Ok(()) 316 | } 317 | 318 | fn tcp_read_from_buffer(&mut self, timeout: Duration) { 319 | self.poll_read 320 | .poll(&mut self.events, Some(timeout)) 321 | .unwrap(); 322 | for event in self.events.iter() { 323 | match event.token() { 324 | CLIENT => { 325 | if event.is_readable() { 326 | let mut buffer = [0_u8; 70000]; 327 | let available_bytes = self.tcp_socket.peek(&mut buffer); 328 | let available_bytes = match available_bytes { 329 | Ok(a) => a, 330 | Err(e) => { 331 | eprintln!("{}", e); 332 | return; 333 | } 334 | }; 335 | 336 | if self.pending_response.is_empty() { 337 | let header_mem_size = match self.network_type { 338 | NetworkType::Gripper => size_of::(), 339 | NetworkType::Robot => size_of::(), 340 | }; 341 | if available_bytes >= header_mem_size { 342 | let mut header_bytes: Vec = vec![0; header_mem_size]; 343 | self.tcp_socket.read_exact(&mut header_bytes).unwrap(); 344 | self.pending_response.append(&mut header_bytes.clone()); 345 | self.pending_response_offset = header_mem_size as usize; 346 | match self.network_type { 347 | NetworkType::Gripper => { 348 | let header: CommandHeader = deserialize(&header_bytes); 349 | self.pending_response_len = header.size as usize; 350 | self.pending_command_id = header.command_id; 351 | } 352 | NetworkType::Robot => { 353 | let header: RobotCommandHeader = deserialize(&header_bytes); 354 | self.pending_response_len = header.size as usize; 355 | self.pending_command_id = header.command_id; 356 | } 357 | }; 358 | } 359 | } 360 | if !self.pending_response.is_empty() && available_bytes > 0 { 361 | let number_of_bytes_to_read = usize::min( 362 | available_bytes, 363 | self.pending_response_len - self.pending_response_offset as usize, 364 | ); 365 | let mut response_buffer: Vec = vec![0; number_of_bytes_to_read]; 366 | self.tcp_socket.read_exact(&mut response_buffer).unwrap(); 367 | self.pending_response.append(&mut response_buffer); 368 | self.pending_response_offset += number_of_bytes_to_read; 369 | if self.pending_response_offset == self.pending_response_len as usize { 370 | self.received_responses 371 | .insert(self.pending_command_id, self.pending_response.clone()); 372 | self.pending_response.clear(); 373 | self.pending_response_offset = 0; 374 | self.pending_command_id = 0; 375 | self.pending_response_len = 0; 376 | } 377 | } 378 | } 379 | if event.is_writable() { 380 | eprintln!("There should not be any writable events") 381 | } 382 | } 383 | _ => unreachable!(), 384 | } 385 | } 386 | } 387 | pub fn get_udp_port(&self) -> u16 { 388 | self.udp_port 389 | } 390 | } 391 | 392 | fn serialize(s: &T) -> Vec { 393 | bincode::serialize(s).unwrap() 394 | } 395 | 396 | fn deserialize(encoded: &[u8]) -> T { 397 | bincode::deserialize(encoded).unwrap() 398 | } 399 | 400 | #[cfg(test)] 401 | mod tests { 402 | use crate::network::{deserialize, serialize}; 403 | use crate::robot::types::RobotStateIntern; 404 | 405 | #[test] 406 | fn can_serialize_and_deserialize() { 407 | let state = RobotStateIntern::dummy(); 408 | let state2: RobotStateIntern = deserialize(&serialize(&state)); 409 | assert_eq!(state, state2); 410 | } 411 | } 412 | -------------------------------------------------------------------------------- /src/robot/control_loop.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use std::fmt::Debug; 4 | use std::time::Duration; 5 | 6 | use crate::exception::{FrankaException, FrankaResult}; 7 | use crate::robot::control_tools::{ 8 | has_realtime_kernel, set_current_thread_to_highest_scheduler_priority, 9 | }; 10 | use crate::robot::control_types::{ControllerMode, Finishable, RealtimeConfig, Torques}; 11 | use crate::robot::low_pass_filter::{low_pass_filter, MAX_CUTOFF_FREQUENCY}; 12 | use crate::robot::motion_generator_traits::MotionGeneratorTrait; 13 | use crate::robot::rate_limiting::{limit_rate_torques, DELTA_T, MAX_TORQUE_RATE}; 14 | use crate::robot::robot_control::RobotControl; 15 | use crate::robot::robot_state::RobotState; 16 | use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; 17 | use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; 18 | 19 | type ControlCallback<'b> = &'b mut dyn FnMut(&RobotState, &Duration) -> Torques; 20 | pub struct ControlLoop< 21 | 'a, 22 | 'b, 23 | T: RobotControl, 24 | U: Finishable + Debug + MotionGeneratorTrait, 25 | F: FnMut(&RobotState, &Duration) -> U, 26 | > { 27 | pub default_deviation: MoveDeviation, 28 | robot: &'a mut T, 29 | motion_callback: F, 30 | control_callback: Option>, 31 | limit_rate: bool, 32 | cutoff_frequency: f64, 33 | pub motion_id: u32, 34 | } 35 | 36 | impl< 37 | 'a, 38 | 'b, 39 | T: RobotControl, 40 | U: Finishable + Debug + MotionGeneratorTrait, 41 | F: FnMut(&RobotState, &Duration) -> U, 42 | > ControlLoop<'a, 'b, T, U, F> 43 | { 44 | pub fn new( 45 | robot: &'a mut T, 46 | control_callback: ControlCallback<'b>, 47 | motion_callback: F, 48 | limit_rate: bool, 49 | cutoff_frequency: f64, 50 | ) -> FrankaResult { 51 | let mut control_loop = ControlLoop::new_intern( 52 | robot, 53 | motion_callback, 54 | Some(control_callback), 55 | limit_rate, 56 | cutoff_frequency, 57 | )?; 58 | control_loop.motion_id = control_loop.robot.start_motion( 59 | MoveControllerMode::ExternalController, 60 | U::get_motion_generator_mode(), 61 | &control_loop.default_deviation, 62 | &control_loop.default_deviation, 63 | )?; 64 | Ok(control_loop) 65 | } 66 | pub fn from_control_mode( 67 | robot: &'a mut T, 68 | control_mode: ControllerMode, 69 | motion_callback: F, 70 | limit_rate: bool, 71 | cutoff_frequency: f64, 72 | ) -> FrankaResult { 73 | let mode: MoveControllerMode = match control_mode { 74 | ControllerMode::JointImpedance => MoveControllerMode::JointImpedance, 75 | ControllerMode::CartesianImpedance => MoveControllerMode::CartesianImpedance, 76 | }; 77 | let mut control_loop = 78 | ControlLoop::new_intern(robot, motion_callback, None, limit_rate, cutoff_frequency)?; 79 | control_loop.motion_id = control_loop.robot.start_motion( 80 | mode, 81 | U::get_motion_generator_mode(), 82 | &control_loop.default_deviation, 83 | &control_loop.default_deviation, 84 | )?; 85 | Ok(control_loop) 86 | } 87 | fn new_intern( 88 | robot: &'a mut T, 89 | motion_callback: F, 90 | control_callback: Option>, 91 | limit_rate: bool, 92 | cutoff_frequency: f64, 93 | ) -> FrankaResult { 94 | let enforce_real_time = robot.realtime_config() == RealtimeConfig::Enforce; 95 | let control_loop = ControlLoop { 96 | default_deviation: MoveDeviation { 97 | translation: 10., 98 | rotation: 3.12, 99 | elbow: 2. * std::f64::consts::PI, 100 | }, 101 | robot, 102 | motion_callback, 103 | control_callback, 104 | limit_rate, 105 | cutoff_frequency, 106 | motion_id: 0, 107 | }; 108 | if enforce_real_time { 109 | if has_realtime_kernel() { 110 | set_current_thread_to_highest_scheduler_priority()?; 111 | } else { 112 | return Err(FrankaException::RealTimeException { 113 | message: "libfranka-rs: Running kernel does not have realtime capabilities." 114 | .to_string(), 115 | }); 116 | } 117 | } 118 | Ok(control_loop) 119 | } 120 | /// this is the function which is operator() in c++ 121 | pub fn run(&mut self) -> FrankaResult<()> { 122 | match self.do_loop() { 123 | Ok(_) => Ok(()), 124 | Err(error) => { 125 | self.robot.cancel_motion(self.motion_id); 126 | Err(error) 127 | } 128 | } 129 | } 130 | 131 | fn do_loop(&mut self) -> FrankaResult<()> { 132 | let mut robot_state = self.robot.update(None, None)?; 133 | self.robot 134 | .throw_on_motion_error(&robot_state, self.motion_id)?; 135 | let mut previous_time = robot_state.time; 136 | let mut motion_command = 137 | MotionGeneratorCommand::new([0.; 7], [0.; 7], [0.; 16], [0.; 6], [0.; 2]); 138 | if self.control_callback.is_some() { 139 | let mut control_command = ControllerCommand { tau_J_d: [0.; 7] }; 140 | while self.spin_motion( 141 | &robot_state, 142 | &(robot_state.time - previous_time), 143 | &mut motion_command, 144 | ) && self.spin_control( 145 | &robot_state, 146 | &(robot_state.time - previous_time), 147 | &mut control_command, 148 | ) { 149 | previous_time = robot_state.time; 150 | robot_state = self 151 | .robot 152 | .update(Some(&motion_command), Some(&control_command))?; 153 | self.robot 154 | .throw_on_motion_error(&robot_state, self.motion_id)?; 155 | } 156 | self.robot.finish_motion( 157 | self.motion_id, 158 | Some(&motion_command), 159 | Some(&control_command), 160 | )?; 161 | } else { 162 | while self.spin_motion( 163 | &robot_state, 164 | &(robot_state.time - previous_time), 165 | &mut motion_command, 166 | ) { 167 | previous_time = robot_state.time; 168 | robot_state = self.robot.update(Some(&motion_command), None)?; 169 | self.robot 170 | .throw_on_motion_error(&robot_state, self.motion_id)?; 171 | } 172 | self.robot 173 | .finish_motion(self.motion_id, Some(&motion_command), None)?; 174 | } 175 | Ok(()) 176 | } 177 | 178 | fn spin_control( 179 | &mut self, 180 | robot_state: &RobotState, 181 | time_step: &Duration, 182 | command: &mut ControllerCommand, 183 | ) -> bool { 184 | let mut control_output: Torques = 185 | (self.control_callback.as_mut().unwrap())(robot_state, time_step); 186 | if self.cutoff_frequency < MAX_CUTOFF_FREQUENCY { 187 | for i in 0..7 { 188 | control_output.tau_J[i] = low_pass_filter( 189 | DELTA_T, 190 | control_output.tau_J[i], 191 | robot_state.tau_J_d[i], 192 | self.cutoff_frequency, 193 | ); 194 | } 195 | } 196 | if self.limit_rate { 197 | control_output.tau_J = limit_rate_torques( 198 | &MAX_TORQUE_RATE, 199 | &control_output.tau_J, 200 | &robot_state.tau_J_d, 201 | ); 202 | } 203 | command.tau_J_d = control_output.tau_J; 204 | command.tau_J_d.iter().for_each(|x| assert!(x.is_finite())); 205 | !control_output.is_finished() 206 | } 207 | fn spin_motion( 208 | &mut self, 209 | robot_state: &RobotState, 210 | time_step: &Duration, 211 | command: &mut MotionGeneratorCommand, 212 | ) -> bool { 213 | let motion_output = (self.motion_callback)(robot_state, time_step); 214 | motion_output.convert_motion(robot_state, command, self.cutoff_frequency, self.limit_rate); 215 | !motion_output.is_finished() 216 | } 217 | } 218 | -------------------------------------------------------------------------------- /src/robot/control_tools.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use crate::exception::FrankaException; 5 | use crate::FrankaResult; 6 | use std::path::Path; 7 | 8 | /// Determines whether the current OS kernel is a realtime kernel. 9 | /// 10 | /// On Linux, this checks for the existence of `/sys/kernel/realtime`. 11 | pub fn has_realtime_kernel() -> bool { 12 | Path::new("/sys/kernel/realtime").exists() 13 | } 14 | 15 | /// Sets the current thread to the highest possible scheduler priority. 16 | /// 17 | /// # Errors 18 | /// * RealtimeException if realtime priority cannot be set for the current thread. 19 | /// 20 | /// If the method returns an Error please check your /etc/security/limits.conf file 21 | /// There should be a line like this: 22 | /// ```text 23 | ///marco - rtprio 99 24 | /// ``` 25 | pub fn set_current_thread_to_highest_scheduler_priority() -> FrankaResult<()> { 26 | unsafe { 27 | let max_priority = libc::sched_get_priority_max(libc::SCHED_FIFO); 28 | if max_priority == -1 { 29 | return Err(FrankaException::RealTimeException { 30 | message: "libfranka-rs: unable to get maximum possible thread priority".to_string(), 31 | }); 32 | } 33 | let thread_param = libc::sched_param { 34 | // In the original libfranka the priority is set to the maximum priority (99 in this 35 | // case). However, we will set the priority 1 lower as 36 | // https://rt.wiki.kernel.org/index.php/HOWTO:_Build_an_RT-application recommends 37 | sched_priority: max_priority - 1, 38 | }; 39 | if libc::pthread_setschedparam(libc::pthread_self(), libc::SCHED_FIFO, &thread_param) != 0 { 40 | return Err(FrankaException::RealTimeException { 41 | message: "libfranka-rs: unable to set realtime scheduling".to_string(), 42 | }); 43 | } 44 | // The original libfranka does not use mlock. However, we use it to prevent our memory from 45 | // being swapped. 46 | if libc::mlockall(libc::MCL_CURRENT | libc::MCL_FUTURE) != 0 { 47 | return Err(FrankaException::RealTimeException { 48 | message: "libfranka-rs: unable to lock memory".to_string(), 49 | }); 50 | } 51 | } 52 | Ok(()) 53 | } 54 | 55 | /// Determines whether the given array represents a valid homogeneous transformation matrix. 56 | /// transform is represented as a 4x4 matrix in column-major format 57 | #[allow(clippy::float_cmp)] 58 | pub fn is_homogeneous_transformation(transform: &[f64; 16]) -> bool { 59 | const ORTHONORMAL_THRESHOLD: f64 = 1e-5; 60 | if transform[3] != 0.0 || transform[7] != 0.0 || transform[11] != 0.0 || transform[15] != 1.0 { 61 | return false; 62 | } 63 | for j in 0..3 { 64 | if f64::abs( 65 | f64::sqrt( 66 | transform[j * 4].powf(2.) 67 | + transform[j * 4 + 1].powf(2.) 68 | + transform[j * 4 + 2].powf(2.), 69 | ) - 1., 70 | ) > ORTHONORMAL_THRESHOLD 71 | { 72 | return false; 73 | } 74 | } 75 | for i in 0..3 { 76 | if f64::abs( 77 | f64::sqrt( 78 | transform[i].powf(2.) + transform[4 + i].powf(2.) + transform[2 * 4 + i].powf(2.), 79 | ) - 1., 80 | ) > ORTHONORMAL_THRESHOLD 81 | { 82 | return false; 83 | } 84 | } 85 | 86 | true 87 | } 88 | -------------------------------------------------------------------------------- /src/robot/error.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | //! Defines the errors that can occur while controlling the robot. 4 | use std::fmt::{Debug, Display, Formatter, Result}; 5 | 6 | use num_derive::{FromPrimitive, ToPrimitive}; 7 | use serde_repr::{Deserialize_repr, Serialize_repr}; 8 | 9 | /// Controlling errors of the robot. 10 | #[derive(Serialize_repr, Deserialize_repr, Debug, PartialEq, Copy, Clone)] 11 | #[repr(u64)] 12 | #[derive(FromPrimitive, ToPrimitive)] 13 | pub enum FrankaError { 14 | /// The robot moved past the joint limits. 15 | JointPositionLimitsViolation = 0, 16 | /// The robot moved past any of the virtual walls. 17 | CartesianPositionLimitsViolation = 1, 18 | /// The robot would have collided with itself 19 | SelfCollisionAvoidanceViolation = 2, 20 | /// The robot exceeded joint velocity limits. 21 | JointVelocityViolation = 3, 22 | /// The robot exceeded Cartesian velocity limits. 23 | CartesianVelocityViolation = 4, 24 | /// The robot exceeded safety threshold during force control. 25 | ForceControlSafetyViolation = 5, 26 | /// A collision was detected, i.e.\ the robot exceeded a torque threshold in a joint motion. 27 | JointReflex = 6, 28 | /// A collision was detected, i.e.\ the robot exceeded a torque threshold in a Cartesian motion. 29 | CartesianReflex = 7, 30 | /// Internal motion generator did not reach the goal pose. 31 | MaxGoalPoseDeviationViolation = 8, 32 | /// True if internal motion generator deviated from the path. 33 | MaxPathPoseDeviationViolation = 9, 34 | /// Cartesian velocity profile for internal motions was exceeded. 35 | CartesianVelocityProfileSafetyViolation = 10, 36 | /// An external joint position motion generator was started with a pose too far from the current pose. 37 | JointPositionMotionGeneratorStartPoseInvalid = 11, 38 | /// An external joint motion generator would move into a joint limit. 39 | JointMotionGeneratorPositionLimitsViolation = 12, 40 | /// An external joint motion generator exceeded velocity limits. 41 | JointMotionGeneratorVelocityLimitsViolation = 13, 42 | /// Commanded velocity in joint motion generators is discontinuous (target values are too far apart). 43 | JointMotionGeneratorVelocityDiscontinuity = 14, 44 | /// Commanded acceleration in joint motion generators is discontinuous (target values are too far apart). 45 | JointMotionGeneratorAccelerationDiscontinuity = 15, 46 | /// An external Cartesian position motion generator was started with a pose too far from the current pose. 47 | CartesianPositionMotionGeneratorStartPoseInvalid = 16, 48 | /// An external Cartesian motion generator would move into an elbow limit. 49 | CartesianMotionGeneratorElbowLimitViolation = 17, 50 | /// An external Cartesian motion generator would move with too high velocity. 51 | CartesianMotionGeneratorVelocityLimitsViolation = 18, 52 | /// Commanded velocity in Cartesian motion generators is discontinuous (target values are too far apart). 53 | CartesianMotionGeneratorVelocityDiscontinuity = 19, 54 | /// Commanded acceleration in Cartesian motion generators is discontinuous (target values are too far apart). 55 | CartesianMotionGeneratorAccelerationDiscontinuity = 20, 56 | /// Commanded elbow values in Cartesian motion generators are inconsistent. 57 | CartesianMotionGeneratorElbowSignInconsistent = 21, 58 | /// The first elbow value in Cartesian motion generators is too far from initial one. 59 | CartesianMotionGeneratorStartElbowInvalid = 22, 60 | /// The torque set by the external controller is discontinuous. 61 | ForceControllerDesiredForceToleranceViolation = 23, 62 | /// The start elbow sign was inconsistent. 63 | /// Applies only to motions started from Desk. 64 | StartElbowSignInconsistent = 24, 65 | /// Minimum network communication quality could not be held during a motion. 66 | CommunicationConstraintsViolation = 25, 67 | /// Commanded values would result in exceeding the power limit. 68 | PowerLimitViolation = 26, 69 | /// The joint position limits would be exceeded after IK calculation. 70 | CartesianMotionGeneratorJointPositionLimitsViolation = 27, 71 | /// The joint velocity limits would be exceeded after IK calculation. 72 | CartesianMotionGeneratorJointVelocityLimitsViolation = 28, 73 | /// The joint velocity in Cartesian motion generators is discontinuous after IK calculation 74 | CartesianMotionGeneratorJointVelocityDiscontinuity = 29, 75 | /// The joint acceleration in Cartesian motion generators is discontinuous after IK calculation. 76 | CartesianMotionGeneratorJointAccelerationDiscontinuity = 30, 77 | /// Cartesian pose is not a valid transformation matrix. 78 | CartesianPositionMotionGeneratorInvalidFrame = 31, 79 | /// The torque set by the external controller is discontinuous. 80 | ControllerTorqueDiscontinuity = 32, 81 | /// The robot is overloaded for the required motion. 82 | /// 83 | /// Applies only to motions started from Desk. 84 | JointP2PInsufficientTorqueForPlanning = 33, 85 | /// The measured torque signal is out of the safe range. 86 | TauJRangeViolation = 34, 87 | /// An instability is detected. 88 | InstabilityDetection = 35, 89 | /// The robot is in joint position limits violation error and the user guides the robot further towards the limit. 90 | JointMoveInWrongDirection = 36, 91 | /// The generated motion violates a joint limit. 92 | CartesianSplineViolation = 37, 93 | /// The generated motion violates a joint limit. 94 | JointViaPlanLimitViolation = 38, 95 | /// The gravity vector could not be initialized by measuring the base acceleration. 96 | BaseAccelerationInitializationTimeout = 39, 97 | /// Base acceleration O_ddP_O cannot be determined. 98 | BaseAccelerationInvalidReading = 40, 99 | } 100 | 101 | impl Display for FrankaError { 102 | fn fmt(&self, f: &mut Formatter) -> Result { 103 | write!(f, "{:?}", self) 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /src/robot/errors.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains the FrankaErrors type. 5 | use std::fmt; 6 | 7 | use crate::robot::error::FrankaError; 8 | use crate::robot::types::RoboErrorHelperStruct; 9 | use num_traits::FromPrimitive; 10 | 11 | /// Represent the Errors which were received from the robot. 12 | #[derive(Debug, Clone, Default)] 13 | pub struct FrankaErrors { 14 | /// contains all errors 15 | pub franka_errors: Vec, 16 | } 17 | 18 | pub enum FrankaErrorKind { 19 | Error, 20 | ReflexReason, 21 | } 22 | 23 | impl FrankaErrors { 24 | /// Creates a new set of errors. 25 | /// # Arguments 26 | /// * `errors` - a [RoboErrorHelperStruct](`crate::robot::types::RoboErrorHelperStruct`) containing 27 | /// all of the errors from [RobotStateIntern](`crate::robot::types::RobotStateIntern`) 28 | /// * `kind` - specifies if the errors are normal Errors or Reflex reasons 29 | pub(crate) fn new(errors: RoboErrorHelperStruct, kind: FrankaErrorKind) -> FrankaErrors { 30 | let error_array = match kind { 31 | FrankaErrorKind::Error => errors.combine_errors(), 32 | FrankaErrorKind::ReflexReason => errors.combine_reflex_reasons(), 33 | }; 34 | 35 | let franka_errors = error_array 36 | .iter() 37 | .enumerate() 38 | .filter(|(_i, x)| **x) 39 | .map(|(i, _x)| FrankaError::from_i32(i as i32).unwrap()) 40 | .collect::>(); 41 | FrankaErrors { franka_errors } 42 | } 43 | /// check if a specific error is received 44 | pub fn contains(&self, error: FrankaError) -> bool { 45 | self.franka_errors.contains(&error) 46 | } 47 | } 48 | 49 | impl fmt::Display for FrankaErrors { 50 | fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { 51 | write!(f, "[")?; 52 | let mut iter = self.franka_errors.iter().peekable(); 53 | while let Some(error) = iter.next() { 54 | match iter.peek() { 55 | Some(_) => { 56 | write!(f, "\"{}\", ", error)?; 57 | } 58 | None => { 59 | return write!(f, "\"{}\"]", error); 60 | } 61 | } 62 | } 63 | Ok(()) 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/robot/logger.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains the logging type definitions for [`ControlException`](`crate::exception::FrankaException::ControlException`) 5 | use crate::robot::control_types::{ 6 | CartesianPose, CartesianVelocities, JointPositions, JointVelocities, Torques, 7 | }; 8 | use crate::robot::robot_state::RobotState; 9 | use crate::robot::types::RobotCommand; 10 | use std::collections::VecDeque; 11 | 12 | /// Command sent to the robot. Structure used only for logging purposes. 13 | #[derive(Debug, Copy, Clone)] 14 | pub struct RobotCommandLog { 15 | /// sent joint positions. 16 | pub joint_positions: JointPositions, 17 | 18 | /// sent joint velocities. 19 | pub joint_velocities: JointVelocities, 20 | 21 | /// sent cartesian positions. 22 | pub cartesian_pose: CartesianPose, 23 | 24 | /// sent cartesian velocities. 25 | pub cartesian_velocities: CartesianVelocities, 26 | 27 | /// sent torques. 28 | pub torques: Torques, 29 | } 30 | 31 | /// One row of the log contains a robot command of timestamp n and a 32 | /// corresponding robot state of timestamp n+1. 33 | /// Provided by the [`ControlException`](`crate::exception::FrankaException::ControlException`). 34 | #[derive(Debug, Clone)] 35 | pub struct Record { 36 | /// Robot state of timestamp n+1. 37 | pub state: RobotState, 38 | /// Robot command of timestamp n, after rate limiting (if activated). 39 | pub command: RobotCommandLog, 40 | } 41 | 42 | impl Record { 43 | /// creates a string representation based on the debug formatter 44 | pub fn log(&self) -> String { 45 | format!("{:?}", self.clone()) 46 | } 47 | } 48 | 49 | pub(crate) struct Logger { 50 | states: VecDeque, 51 | commands: VecDeque, 52 | ring_front: usize, 53 | ring_size: usize, 54 | log_size: usize, 55 | } 56 | 57 | impl Logger { 58 | pub fn new(log_size: usize) -> Self { 59 | Logger { 60 | states: VecDeque::with_capacity(log_size), 61 | commands: VecDeque::with_capacity(log_size), 62 | ring_front: 0, 63 | ring_size: 0, 64 | log_size, 65 | } 66 | } 67 | pub fn log(&mut self, state: &RobotState, command: &RobotCommand) { 68 | self.states.push_back(state.clone()); 69 | self.commands.push_back(*command); 70 | self.ring_front = (self.ring_front + 1) % self.log_size; 71 | if self.ring_size == self.log_size { 72 | self.states.pop_front(); 73 | self.commands.pop_front(); 74 | } 75 | self.ring_size = usize::min(self.log_size, self.ring_size + 1); 76 | } 77 | pub fn flush(&mut self) -> Vec { 78 | let mut out: Vec = Vec::new(); 79 | for i in 0..self.ring_size { 80 | let index = (self.ring_front + i) % self.ring_size; 81 | let cmd = self.commands.get(index).unwrap(); 82 | let command = RobotCommandLog { 83 | joint_positions: JointPositions::new(cmd.motion.q_c), 84 | joint_velocities: JointVelocities::new(cmd.motion.dq_c), 85 | cartesian_pose: CartesianPose::new(cmd.motion.O_T_EE_c, None), 86 | cartesian_velocities: CartesianVelocities::new(cmd.motion.O_dP_EE_c, None), 87 | torques: Torques::new(cmd.control.tau_J_d), 88 | }; 89 | let record = Record { 90 | state: self.states.get(index).unwrap().clone(), 91 | command, 92 | }; 93 | out.push(record); 94 | } 95 | self.ring_front = 0; 96 | self.ring_size = 0; 97 | out 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /src/robot/low_pass_filter.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains functions for filtering signals with a low-pass filter. 5 | 6 | use crate::utils::array_to_isometry; 7 | use std::f64::consts::PI; 8 | 9 | /// Maximum cutoff frequency: 1000 Hz 10 | pub static MAX_CUTOFF_FREQUENCY: f64 = 1000.0; 11 | /// Default cutoff frequency: 100 Hz 12 | pub static DEFAULT_CUTOFF_FREQUENCY: f64 = 100.0; 13 | 14 | /// Applies a first-order low-pass filter 15 | /// 16 | /// # Arguments 17 | /// * `sample_time` - Sample time constant 18 | /// * `y` - Current value of the signal to be filtered 19 | /// * `y_last` - Value of the signal to be filtered in the previous time step 20 | /// * `cutoff_frequency` - Cutoff frequency of the low-pass filter 21 | /// # Panics 22 | /// This function panics if: 23 | /// * y is infinite or NaN. 24 | /// * y_last is infinite or NaN. 25 | /// * cutoff_frequency is zero, negative, infinite or NaN. 26 | /// * sample_time is negative, infinite or NaN. 27 | /// # Return 28 | /// Filtered value. 29 | pub fn low_pass_filter(sample_time: f64, y: f64, y_last: f64, cutoff_frequency: f64) -> f64 { 30 | assert!(sample_time.is_sign_positive() && sample_time.is_finite()); 31 | assert!(cutoff_frequency.is_sign_positive() && cutoff_frequency.is_finite()); 32 | assert!(y.is_finite() && y_last.is_finite()); 33 | let gain = sample_time / (sample_time + (1.0 / (2.0 * PI * cutoff_frequency))); 34 | gain * y + (1. - gain) * y_last 35 | } 36 | 37 | /// Applies a first-order low-pass filter to the translation and spherical linear interpolation 38 | /// to the rotation of a transformation matrix which represents a Cartesian Motion. 39 | /// 40 | /// # Arguments 41 | /// * `sample_time` - Sample time constant 42 | /// * `y` - Current Cartesian transformation matrix to be filtered 43 | /// * `y_last` - Cartesian transformation matrix from the previous time step 44 | /// * `cutoff_frequency` - Cutoff frequency of the low-pass filter 45 | /// # Panics 46 | /// This function panics if: 47 | /// * elements of y is infinite or NaN. 48 | /// * elements of y_last is infinite or NaN. 49 | /// * cutoff_frequency is zero, negative, infinite or NaN. 50 | /// * sample_time is negative, infinite or NaN. 51 | /// # Return 52 | /// Filtered Cartesian transformation matrix. 53 | pub fn cartesian_low_pass_filter( 54 | sample_time: f64, 55 | y: &[f64; 16], 56 | y_last: &[f64; 16], 57 | cutoff_frequency: f64, 58 | ) -> [f64; 16] { 59 | assert!(sample_time.is_sign_positive() && sample_time.is_finite()); 60 | assert!(cutoff_frequency.is_sign_positive() && cutoff_frequency.is_finite()); 61 | y.iter() 62 | .zip(y_last.iter()) 63 | .for_each(|(i, j)| assert!(i.is_finite() && j.is_finite())); 64 | let mut transform = array_to_isometry(y); 65 | let transform_last = array_to_isometry(y_last); 66 | let gain = sample_time / (sample_time + (1.0 / (2.0 * PI * cutoff_frequency))); 67 | transform.translation.vector = 68 | gain * transform.translation.vector + (1. - gain) * transform_last.translation.vector; 69 | transform.rotation = transform_last.rotation.slerp(&transform.rotation, gain); 70 | 71 | let mut out = [0.; 16]; 72 | for (i, &x) in transform.to_homogeneous().iter().enumerate() { 73 | out[i] = x; 74 | } 75 | out 76 | } 77 | 78 | #[cfg(test)] 79 | mod tests { 80 | use crate::robot::low_pass_filter::{cartesian_low_pass_filter, low_pass_filter}; 81 | 82 | #[test] 83 | fn low_pass_test() { 84 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 100.0) - 1.) < 0.000001); 85 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 500.0) - 1.) < 0.000001); 86 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 1000.0) - 1.) < 0.000001); 87 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 100.0) - 0.3859) < 0.0001); 88 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 500.0) - 0.7585) < 0.0001); 89 | assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 900.0) - 0.8497) < 0.0001); 90 | } 91 | 92 | #[test] 93 | fn low_pass_cartesian_test() { 94 | let sample_time = 0.001; 95 | let cutoff_frequency = 100.; 96 | let y = [ 97 | 0.9999903734042686, 98 | 0.0000000002540163079878255, 99 | -0.00000000012581154368346085, 100 | 0.0, 101 | 0.0000000002540163079878255, 102 | -0.9999903734042686, 103 | 0.00000000004614105974113725, 104 | 0.0, 105 | -0.00000000012581275483128821, 106 | -0.000000000046141503958700795, 107 | -1.0, 108 | 0.0, 109 | 0.30689056659844144, 110 | 0.0000000000692086410879149, 111 | 0.4868820527992277, 112 | 1.0, 113 | ]; 114 | let y_last = [ 115 | 0.9999903734042686, 116 | 0.0000000002540163079878255, 117 | -0.00000000012581154368346085, 118 | 0.0, 119 | 0.0000000002540163079878255, 120 | -0.9999903734042686, 121 | 0.00000000004614105974113725, 122 | 0.0, 123 | -0.00000000012581275483128821, 124 | -0.000000000046141503958700795, 125 | -1.0, 126 | 0.0, 127 | 0.30689056659844144, 128 | 0.0000000000692086410879149, 129 | 0.4868820527992277, 130 | 1.0, 131 | ]; 132 | 133 | println!( 134 | "{:?}", 135 | cartesian_low_pass_filter(sample_time, &y, &y_last, cutoff_frequency) 136 | ); 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /src/robot/motion_generator_traits.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use crate::robot::service_types::MoveMotionGeneratorMode; 4 | 5 | pub trait MotionGeneratorTrait { 6 | fn get_motion_generator_mode() -> MoveMotionGeneratorMode; 7 | } 8 | -------------------------------------------------------------------------------- /src/robot/robot_control.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use crate::exception::FrankaResult; 4 | use crate::robot::control_types::RealtimeConfig; 5 | use crate::robot::robot_state::RobotState; 6 | use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode}; 7 | use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; 8 | 9 | pub trait RobotControl { 10 | fn start_motion( 11 | &mut self, 12 | controller_mode: MoveControllerMode, 13 | motion_generator_mode: MoveMotionGeneratorMode, 14 | maximum_path_deviation: &MoveDeviation, 15 | maximum_goal_deviation: &MoveDeviation, 16 | ) -> FrankaResult; 17 | fn finish_motion( 18 | &mut self, 19 | motion_id: u32, 20 | motion_command: Option<&MotionGeneratorCommand>, 21 | control_command: Option<&ControllerCommand>, 22 | ) -> FrankaResult<()>; 23 | fn cancel_motion(&mut self, motion_id: u32); 24 | fn update( 25 | &mut self, 26 | motion_command: Option<&MotionGeneratorCommand>, 27 | control_command: Option<&ControllerCommand>, 28 | ) -> FrankaResult; 29 | fn realtime_config(&self) -> RealtimeConfig; 30 | fn throw_on_motion_error( 31 | &mut self, 32 | robot_state: &RobotState, 33 | motion_id: u32, 34 | ) -> FrankaResult<()>; 35 | } 36 | -------------------------------------------------------------------------------- /src/robot/service_types.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | use std::fmt::Debug; 5 | 6 | use serde::Deserialize; 7 | use serde::Serialize; 8 | use serde_repr::{Deserialize_repr, Serialize_repr}; 9 | 10 | use crate::network::MessageCommand; 11 | 12 | pub static VERSION: u16 = 5; 13 | pub static COMMAND_PORT: u16 = 1337; 14 | 15 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 16 | #[repr(u32)] 17 | pub enum RobotCommandEnum { 18 | Connect, 19 | Move, 20 | StopMove, 21 | GetCartesianLimit, 22 | SetCollisionBehavior, 23 | SetJointImpedance, 24 | SetCartesianImpedance, 25 | SetGuidingMode, 26 | SetEeToK, 27 | SetNeToEe, 28 | SetLoad, 29 | SetFilters, 30 | AutomaticErrorRecovery, 31 | LoadModelLibrary, 32 | } 33 | 34 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 35 | #[repr(u8)] 36 | pub enum DefaultStatus { 37 | Success, 38 | CommandNotPossibleRejected, 39 | } 40 | 41 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 42 | #[repr(u8)] 43 | pub enum ConnectStatus { 44 | Success, 45 | IncompatibleLibraryVersion, 46 | } 47 | 48 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] 49 | #[repr(u8)] 50 | pub enum MoveStatus { 51 | Success, 52 | MotionStarted, 53 | Preempted, 54 | CommandNotPossibleRejected, 55 | StartAtSingularPoseRejected, 56 | InvalidArgumentRejected, 57 | ReflexAborted, 58 | EmergencyAborted, 59 | InputErrorAborted, 60 | Aborted, 61 | } 62 | 63 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 64 | #[repr(u8)] 65 | pub enum StopMoveStatus { 66 | Success, 67 | CommandNotPossibleRejected, 68 | EmergencyAborted, 69 | ReflexAborted, 70 | Aborted, 71 | } 72 | 73 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 74 | #[repr(u8)] 75 | pub enum AutomaticErrorRecoveryStatus { 76 | Success, 77 | CommandNotPossibleRejected, 78 | ManualErrorRecoveryRequiredRejected, 79 | ReflexAborted, 80 | EmergencyAborted, 81 | Aborted, 82 | } 83 | 84 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 85 | #[repr(u8)] 86 | pub enum LoadModelLibraryStatus { 87 | Success, 88 | Error, 89 | } 90 | 91 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 92 | #[repr(u8)] 93 | pub enum GetterSetterStatus { 94 | Success, 95 | CommandNotPossibleRejected, 96 | InvalidArgumentRejected, 97 | } 98 | 99 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 100 | #[repr(packed)] 101 | pub struct ConnectRequest { 102 | pub version: u16, 103 | pub udp_port: u16, 104 | } 105 | 106 | impl ConnectRequest { 107 | pub fn new(udp_port: u16) -> Self { 108 | ConnectRequest { 109 | version: VERSION, 110 | udp_port, 111 | } 112 | } 113 | } 114 | 115 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 116 | #[repr(packed)] 117 | pub struct ConnectRequestWithHeader { 118 | pub header: RobotCommandHeader, 119 | pub request: ConnectRequest, 120 | } 121 | 122 | impl MessageCommand for ConnectRequestWithHeader { 123 | fn get_command_message_id(&self) -> u32 { 124 | self.header.get_command_message_id() 125 | } 126 | } 127 | 128 | #[derive(Serialize, Deserialize, Debug)] 129 | pub struct ConnectResponse { 130 | pub header: RobotCommandHeader, 131 | pub status: ConnectStatus, 132 | pub version: u16, 133 | } 134 | 135 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 136 | #[repr(u32)] 137 | pub enum MoveControllerMode { 138 | JointImpedance, 139 | CartesianImpedance, 140 | ExternalController, 141 | } 142 | 143 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 144 | #[repr(u32)] 145 | pub enum MoveMotionGeneratorMode { 146 | JointPosition, 147 | JointVelocity, 148 | CartesianPosition, 149 | CartesianVelocity, 150 | } 151 | 152 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 153 | #[repr(packed)] 154 | pub struct MoveDeviation { 155 | pub(crate) translation: f64, 156 | pub(crate) rotation: f64, 157 | pub(crate) elbow: f64, 158 | } 159 | 160 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 161 | #[repr(packed)] 162 | pub struct MoveRequest { 163 | controller_mode: MoveControllerMode, 164 | motion_generator_mode: MoveMotionGeneratorMode, 165 | maximum_path_deviation: MoveDeviation, 166 | maximum_goal_deviation: MoveDeviation, 167 | } 168 | 169 | impl MoveRequest { 170 | pub fn new( 171 | controller_mode: MoveControllerMode, 172 | motion_generator_mode: MoveMotionGeneratorMode, 173 | maximum_path_deviation: MoveDeviation, 174 | maximum_goal_deviation: MoveDeviation, 175 | ) -> Self { 176 | MoveRequest { 177 | controller_mode, 178 | motion_generator_mode, 179 | maximum_path_deviation, 180 | maximum_goal_deviation, 181 | } 182 | } 183 | } 184 | 185 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 186 | #[repr(packed)] 187 | pub struct MoveRequestWithHeader { 188 | pub header: RobotCommandHeader, 189 | pub request: MoveRequest, 190 | } 191 | 192 | impl MessageCommand for MoveRequestWithHeader { 193 | fn get_command_message_id(&self) -> u32 { 194 | self.header.get_command_message_id() 195 | } 196 | } 197 | 198 | #[derive(Serialize, Deserialize, Debug)] 199 | pub struct MoveResponse { 200 | pub header: RobotCommandHeader, 201 | pub status: MoveStatus, 202 | } 203 | 204 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 205 | #[repr(packed)] 206 | pub struct StopMoveRequestWithHeader { 207 | pub header: RobotCommandHeader, 208 | } 209 | 210 | impl MessageCommand for StopMoveRequestWithHeader { 211 | fn get_command_message_id(&self) -> u32 { 212 | self.header.get_command_message_id() 213 | } 214 | } 215 | 216 | #[derive(Serialize, Deserialize, Debug)] 217 | pub struct StopMoveResponse { 218 | pub header: RobotCommandHeader, 219 | pub status: StopMoveStatus, 220 | } 221 | 222 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 223 | #[repr(packed)] 224 | pub struct GetCartesianLimitRequest { 225 | pub id: i32, 226 | } 227 | 228 | impl GetCartesianLimitRequest { 229 | pub fn new(id: i32) -> Self { 230 | GetCartesianLimitRequest { id } 231 | } 232 | } 233 | 234 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 235 | #[repr(packed)] 236 | pub struct GetCartesianLimitRequestWithHeader { 237 | pub header: RobotCommandHeader, 238 | pub request: GetCartesianLimitRequest, 239 | } 240 | 241 | impl MessageCommand for GetCartesianLimitRequestWithHeader { 242 | fn get_command_message_id(&self) -> u32 { 243 | self.header.get_command_message_id() 244 | } 245 | } 246 | 247 | #[derive(Serialize, Deserialize, Debug)] 248 | pub struct GetCartesianLimitResponse { 249 | pub header: RobotCommandHeader, 250 | pub status: GetterSetterStatus, 251 | pub object_world_size: [f64; 3], 252 | pub object_frame: [f64; 16], 253 | pub object_activation: bool, 254 | } 255 | 256 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 257 | #[repr(packed)] 258 | pub struct SetCollisionBehaviorRequest { 259 | pub lower_torque_thresholds_acceleration: [f64; 7], 260 | pub upper_torque_thresholds_acceleration: [f64; 7], 261 | pub lower_torque_thresholds_nominal: [f64; 7], 262 | pub upper_torque_thresholds_nominal: [f64; 7], 263 | pub lower_force_thresholds_acceleration: [f64; 6], 264 | pub upper_force_thresholds_acceleration: [f64; 6], 265 | pub lower_force_thresholds_nominal: [f64; 6], 266 | pub upper_force_thresholds_nominal: [f64; 6], 267 | } 268 | 269 | impl SetCollisionBehaviorRequest { 270 | #[allow(clippy::too_many_arguments)] 271 | pub fn new( 272 | lower_torque_thresholds_acceleration: [f64; 7], 273 | upper_torque_thresholds_acceleration: [f64; 7], 274 | lower_torque_thresholds_nominal: [f64; 7], 275 | upper_torque_thresholds_nominal: [f64; 7], 276 | lower_force_thresholds_acceleration: [f64; 6], 277 | upper_force_thresholds_acceleration: [f64; 6], 278 | lower_force_thresholds_nominal: [f64; 6], 279 | upper_force_thresholds_nominal: [f64; 6], 280 | ) -> Self { 281 | SetCollisionBehaviorRequest { 282 | lower_torque_thresholds_acceleration, 283 | upper_torque_thresholds_acceleration, 284 | lower_torque_thresholds_nominal, 285 | upper_torque_thresholds_nominal, 286 | lower_force_thresholds_acceleration, 287 | upper_force_thresholds_acceleration, 288 | lower_force_thresholds_nominal, 289 | upper_force_thresholds_nominal, 290 | } 291 | } 292 | } 293 | 294 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 295 | #[repr(packed)] 296 | pub struct SetCollisionBehaviorRequestWithHeader { 297 | pub header: RobotCommandHeader, 298 | pub request: SetCollisionBehaviorRequest, 299 | } 300 | 301 | impl MessageCommand for SetCollisionBehaviorRequestWithHeader { 302 | fn get_command_message_id(&self) -> u32 { 303 | self.header.get_command_message_id() 304 | } 305 | } 306 | 307 | pub type SetCollisionBehaviorResponse = SetterResponse; 308 | 309 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 310 | #[repr(packed)] 311 | #[allow(non_snake_case)] 312 | pub struct SetJointImpedanceRequest { 313 | pub K_theta: [f64; 7], 314 | } 315 | 316 | #[allow(non_snake_case)] 317 | impl SetJointImpedanceRequest { 318 | pub fn new(K_theta: [f64; 7]) -> Self { 319 | SetJointImpedanceRequest { K_theta } 320 | } 321 | } 322 | 323 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 324 | #[repr(packed)] 325 | pub struct SetJointImpedanceRequestWithHeader { 326 | pub header: RobotCommandHeader, 327 | pub request: SetJointImpedanceRequest, 328 | } 329 | 330 | impl MessageCommand for SetJointImpedanceRequestWithHeader { 331 | fn get_command_message_id(&self) -> u32 { 332 | self.header.get_command_message_id() 333 | } 334 | } 335 | 336 | pub type SetJointImpedanceResponse = SetterResponse; 337 | 338 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 339 | #[repr(packed)] 340 | #[allow(non_snake_case)] 341 | pub struct SetCartesianImpedanceRequest { 342 | pub K_x: [f64; 6], 343 | } 344 | 345 | #[allow(non_snake_case)] 346 | impl SetCartesianImpedanceRequest { 347 | pub fn new(K_x: [f64; 6]) -> Self { 348 | SetCartesianImpedanceRequest { K_x } 349 | } 350 | } 351 | 352 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 353 | #[repr(packed)] 354 | pub struct SetCartesianImpedanceRequestWithHeader { 355 | pub header: RobotCommandHeader, 356 | pub request: SetCartesianImpedanceRequest, 357 | } 358 | 359 | impl MessageCommand for SetCartesianImpedanceRequestWithHeader { 360 | fn get_command_message_id(&self) -> u32 { 361 | self.header.get_command_message_id() 362 | } 363 | } 364 | 365 | pub type SetCartesianImpedanceResponse = SetterResponse; 366 | 367 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 368 | #[repr(packed)] 369 | #[allow(non_snake_case)] 370 | pub struct SetGuidingModeRequest { 371 | pub guiding_mode: [bool; 6], 372 | pub nullspace: bool, 373 | } 374 | 375 | #[allow(non_snake_case)] 376 | impl SetGuidingModeRequest { 377 | pub fn new(guiding_mode: [bool; 6], nullspace: bool) -> Self { 378 | SetGuidingModeRequest { 379 | guiding_mode, 380 | nullspace, 381 | } 382 | } 383 | } 384 | 385 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 386 | #[repr(packed)] 387 | pub struct SetGuidingModeRequestWithHeader { 388 | pub header: RobotCommandHeader, 389 | pub request: SetGuidingModeRequest, 390 | } 391 | 392 | impl MessageCommand for SetGuidingModeRequestWithHeader { 393 | fn get_command_message_id(&self) -> u32 { 394 | self.header.get_command_message_id() 395 | } 396 | } 397 | 398 | pub type SetGuidingModeResponse = SetterResponse; 399 | 400 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 401 | #[repr(packed)] 402 | #[allow(non_snake_case)] 403 | pub struct SetEeToKRequest { 404 | pub EE_T_K: [f64; 16], 405 | } 406 | 407 | #[allow(non_snake_case)] 408 | impl SetEeToKRequest { 409 | pub fn new(EE_T_K: [f64; 16]) -> Self { 410 | SetEeToKRequest { EE_T_K } 411 | } 412 | } 413 | 414 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 415 | #[repr(packed)] 416 | pub struct SetEeToKRequestWithHeader { 417 | pub header: RobotCommandHeader, 418 | pub request: SetEeToKRequest, 419 | } 420 | 421 | impl MessageCommand for SetEeToKRequestWithHeader { 422 | fn get_command_message_id(&self) -> u32 { 423 | self.header.get_command_message_id() 424 | } 425 | } 426 | 427 | pub type SetEeToKResponse = SetterResponse; 428 | 429 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 430 | #[repr(packed)] 431 | #[allow(non_snake_case)] 432 | pub struct SetNeToEeRequest { 433 | pub NE_T_EE: [f64; 16], 434 | } 435 | 436 | #[allow(non_snake_case)] 437 | impl SetNeToEeRequest { 438 | pub fn new(NE_T_EE: [f64; 16]) -> Self { 439 | SetNeToEeRequest { NE_T_EE } 440 | } 441 | } 442 | 443 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 444 | #[repr(packed)] 445 | pub struct SetNeToEeRequestWithHeader { 446 | pub header: RobotCommandHeader, 447 | pub request: SetNeToEeRequest, 448 | } 449 | 450 | impl MessageCommand for SetNeToEeRequestWithHeader { 451 | fn get_command_message_id(&self) -> u32 { 452 | self.header.get_command_message_id() 453 | } 454 | } 455 | 456 | pub type SetNeToEeResponse = SetterResponse; 457 | 458 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 459 | #[repr(packed)] 460 | #[allow(non_snake_case)] 461 | pub struct SetLoadRequest { 462 | pub m_load: f64, 463 | pub F_x_Cload: [f64; 3], 464 | pub I_Load: [f64; 9], 465 | } 466 | 467 | #[allow(non_snake_case)] 468 | impl SetLoadRequest { 469 | pub fn new(m_load: f64, F_x_Cload: [f64; 3], I_Load: [f64; 9]) -> Self { 470 | SetLoadRequest { 471 | m_load, 472 | F_x_Cload, 473 | I_Load, 474 | } 475 | } 476 | } 477 | 478 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 479 | #[repr(packed)] 480 | pub struct SetLoadRequestWithHeader { 481 | pub header: RobotCommandHeader, 482 | pub request: SetLoadRequest, 483 | } 484 | 485 | impl MessageCommand for SetLoadRequestWithHeader { 486 | fn get_command_message_id(&self) -> u32 { 487 | self.header.get_command_message_id() 488 | } 489 | } 490 | 491 | pub type SetLoadResponse = SetterResponse; 492 | 493 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 494 | #[repr(packed)] 495 | #[allow(non_snake_case)] 496 | pub struct SetFiltersRequest { 497 | pub joint_position_filter_frequency: f64, 498 | pub joint_velocity_filter_frequency: f64, 499 | pub cartesian_position_filter_frequency: f64, 500 | pub cartesian_velocity_filter_frequency: f64, 501 | pub controller_filter_frequency: f64, 502 | } 503 | 504 | #[allow(non_snake_case)] 505 | impl SetFiltersRequest { 506 | pub fn new( 507 | joint_position_filter_frequency: f64, 508 | joint_velocity_filter_frequency: f64, 509 | cartesian_position_filter_frequency: f64, 510 | cartesian_velocity_filter_frequency: f64, 511 | controller_filter_frequency: f64, 512 | ) -> Self { 513 | SetFiltersRequest { 514 | joint_position_filter_frequency, 515 | joint_velocity_filter_frequency, 516 | cartesian_position_filter_frequency, 517 | cartesian_velocity_filter_frequency, 518 | controller_filter_frequency, 519 | } 520 | } 521 | } 522 | 523 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 524 | #[repr(packed)] 525 | pub struct SetFiltersRequestWithHeader { 526 | pub header: RobotCommandHeader, 527 | pub request: SetFiltersRequest, 528 | } 529 | 530 | impl MessageCommand for SetFiltersRequestWithHeader { 531 | fn get_command_message_id(&self) -> u32 { 532 | self.header.get_command_message_id() 533 | } 534 | } 535 | 536 | pub type SetFiltersResponse = SetterResponse; 537 | 538 | #[derive(Serialize, Deserialize, Debug)] 539 | pub struct SetterResponse { 540 | pub header: RobotCommandHeader, 541 | pub status: GetterSetterStatus, 542 | } 543 | 544 | pub type AutomaticErrorRecoveryRequestWithHeader = RobotCommandHeader; 545 | 546 | #[derive(Serialize, Deserialize, Debug)] 547 | pub struct AutomaticErrorRecoveryResponse { 548 | pub header: RobotCommandHeader, 549 | pub status: AutomaticErrorRecoveryStatus, 550 | } 551 | 552 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 553 | #[repr(u8)] 554 | pub enum LoadModelLibraryArchitecture { 555 | X64, 556 | X86, 557 | Arm, 558 | Arm64, 559 | } 560 | 561 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] 562 | #[repr(u8)] 563 | pub enum LoadModelLibrarySystem { 564 | Linux, 565 | Windows, 566 | } 567 | 568 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 569 | #[repr(packed)] 570 | pub struct LoadModelLibraryRequest { 571 | pub architecture: LoadModelLibraryArchitecture, 572 | pub system: LoadModelLibrarySystem, 573 | } 574 | 575 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 576 | #[repr(packed)] 577 | pub struct LoadModelLibraryRequestWithHeader { 578 | pub header: RobotCommandHeader, 579 | pub request: LoadModelLibraryRequest, 580 | } 581 | 582 | impl MessageCommand for LoadModelLibraryRequestWithHeader { 583 | fn get_command_message_id(&self) -> u32 { 584 | self.header.get_command_message_id() 585 | } 586 | } 587 | 588 | #[derive(Serialize, Deserialize, Debug)] 589 | pub struct LoadModelLibraryResponse { 590 | pub header: RobotCommandHeader, 591 | pub status: LoadModelLibraryStatus, 592 | } 593 | 594 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 595 | #[repr(packed)] 596 | pub struct RobotCommandHeader { 597 | pub command: RobotCommandEnum, 598 | pub command_id: u32, 599 | pub size: u32, 600 | } 601 | 602 | impl RobotCommandHeader { 603 | pub fn new(command: RobotCommandEnum, command_id: u32, size: u32) -> RobotCommandHeader { 604 | RobotCommandHeader { 605 | command, 606 | command_id, 607 | size, 608 | } 609 | } 610 | } 611 | 612 | impl MessageCommand for RobotCommandHeader { 613 | fn get_command_message_id(&self) -> u32 { 614 | self.command_id 615 | } 616 | } 617 | -------------------------------------------------------------------------------- /src/robot/types.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | use std::fmt::Debug; 4 | 5 | use crate::robot::types::RobotMode::Other; 6 | use serde::Deserialize; 7 | use serde::Serialize; 8 | use serde_repr::{Deserialize_repr, Serialize_repr}; 9 | 10 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] 11 | #[repr(u8)] 12 | pub enum MotionGeneratorMode { 13 | Idle, 14 | JointPosition, 15 | JointVelocity, 16 | CartesianPosition, 17 | CartesianVelocity, 18 | } 19 | 20 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] 21 | #[repr(u8)] 22 | pub enum ControllerMode { 23 | JointImpedance, 24 | CartesianImpedance, 25 | ExternalController, 26 | Other, 27 | } 28 | /// Describes the robot's current mode. 29 | #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] 30 | #[repr(u8)] 31 | pub enum RobotMode { 32 | Other, 33 | Idle, 34 | Move, 35 | Guiding, 36 | Reflex, 37 | UserStopped, 38 | AutomaticErrorRecovery, 39 | } 40 | impl Default for RobotMode { 41 | fn default() -> Self { 42 | Other 43 | } 44 | } 45 | 46 | #[derive(Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] 47 | #[allow(non_snake_case)] 48 | #[repr(packed)] 49 | pub struct RobotStateIntern { 50 | pub message_id: u64, 51 | pub O_T_EE: [f64; 16], 52 | pub O_T_EE_d: [f64; 16], 53 | pub F_T_EE: [f64; 16], 54 | pub EE_T_K: [f64; 16], 55 | pub F_T_NE: [f64; 16], 56 | pub NE_T_EE: [f64; 16], 57 | pub m_ee: f64, 58 | pub I_ee: [f64; 9], 59 | pub F_x_Cee: [f64; 3], 60 | pub m_load: f64, 61 | pub I_load: [f64; 9], 62 | pub F_x_Cload: [f64; 3], 63 | pub elbow: [f64; 2], 64 | pub elbow_d: [f64; 2], 65 | pub tau_J: [f64; 7], 66 | pub tau_J_d: [f64; 7], 67 | pub dtau_J: [f64; 7], 68 | pub q: [f64; 7], 69 | pub q_d: [f64; 7], 70 | pub dq: [f64; 7], 71 | pub dq_d: [f64; 7], 72 | pub ddq_d: [f64; 7], 73 | pub joint_contact: [f64; 7], 74 | pub cartesian_contact: [f64; 6], 75 | pub joint_collision: [f64; 7], 76 | pub cartesian_collision: [f64; 6], 77 | pub tau_ext_hat_filtered: [f64; 7], 78 | pub O_F_ext_hat_K: [f64; 6], 79 | pub K_F_ext_hat_K: [f64; 6], 80 | pub O_dP_EE_d: [f64; 6], 81 | pub O_ddP_O: [f64; 3], 82 | pub elbow_c: [f64; 2], 83 | pub delbow_c: [f64; 2], 84 | pub ddelbow_c: [f64; 2], 85 | pub O_T_EE_c: [f64; 16], 86 | pub O_dP_EE_c: [f64; 6], 87 | pub O_ddP_EE_c: [f64; 6], 88 | pub theta: [f64; 7], 89 | pub dtheta: [f64; 7], 90 | pub motion_generator_mode: MotionGeneratorMode, 91 | pub controller_mode: ControllerMode, 92 | // pub errors: [bool; 37], reenable when const generics arrive 93 | // pub reflex_reason: [bool; 37], reenable when const generics arrive 94 | pub errors: RoboErrorHelperStruct, 95 | pub robot_mode: RobotMode, 96 | pub control_command_success_rate: f64, 97 | } 98 | 99 | impl RobotStateIntern { 100 | pub fn dummy() -> Self { 101 | RobotStateIntern { 102 | message_id: 0, 103 | O_T_EE: [0.; 16], 104 | O_T_EE_d: [0.; 16], 105 | F_T_EE: [0.; 16], 106 | EE_T_K: [0.; 16], 107 | F_T_NE: [0.; 16], 108 | NE_T_EE: [0.; 16], 109 | m_ee: 0., 110 | I_ee: [0.; 9], 111 | F_x_Cee: [0.; 3], 112 | m_load: 0., 113 | I_load: [0.; 9], 114 | F_x_Cload: [0.; 3], 115 | elbow: [0.; 2], 116 | elbow_d: [0.; 2], 117 | tau_J: [0.; 7], 118 | tau_J_d: [0.; 7], 119 | dtau_J: [0.; 7], 120 | q: [0.; 7], 121 | q_d: [0.; 7], 122 | dq: [0.; 7], 123 | dq_d: [0.; 7], 124 | ddq_d: [0.; 7], 125 | joint_contact: [0.; 7], 126 | cartesian_contact: [0.; 6], 127 | joint_collision: [0.; 7], 128 | cartesian_collision: [0.; 6], 129 | tau_ext_hat_filtered: [0.; 7], 130 | O_F_ext_hat_K: [0.; 6], 131 | K_F_ext_hat_K: [0.; 6], 132 | O_dP_EE_d: [0.; 6], 133 | O_ddP_O: [0.; 3], 134 | elbow_c: [0.; 2], 135 | delbow_c: [0.; 2], 136 | ddelbow_c: [0.; 2], 137 | O_T_EE_c: [0.; 16], 138 | O_dP_EE_c: [0.; 6], 139 | O_ddP_EE_c: [0.; 6], 140 | theta: [0.; 7], 141 | dtheta: [0.; 7], 142 | motion_generator_mode: MotionGeneratorMode::Idle, 143 | controller_mode: ControllerMode::JointImpedance, 144 | errors: RoboErrorHelperStruct { 145 | errors1: [false; 32], 146 | errors2: [false; 9], 147 | reflex_reason1: [false; 32], 148 | reflex_reason2: [false; 9], 149 | }, 150 | robot_mode: RobotMode::Other, 151 | control_command_success_rate: 0.0, 152 | } 153 | } 154 | } 155 | 156 | /// this struct is used as serde cant serialize arrays bigger than 32. 157 | /// we have to wait for const generics to arrive in serde. 158 | /// https://crates.io/crates/serde-big-array would be an alternative 159 | /// but I do not want to add a dependency with less than 1 million downloads. 160 | #[derive(Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] 161 | #[repr(packed)] 162 | pub struct RoboErrorHelperStruct { 163 | pub errors1: [bool; 32], 164 | pub errors2: [bool; 9], 165 | pub reflex_reason1: [bool; 32], 166 | pub reflex_reason2: [bool; 9], 167 | } 168 | 169 | impl RoboErrorHelperStruct { 170 | pub fn combine_errors(&self) -> [bool; 41] { 171 | let mut out = [false; 41]; 172 | for (i, &val) in self.errors1.iter().enumerate() { 173 | out[i] = val; 174 | } 175 | for (i, &val) in self.errors2.iter().enumerate() { 176 | out[i + 32] = val; 177 | } 178 | out 179 | } 180 | pub fn combine_reflex_reasons(&self) -> [bool; 41] { 181 | let mut out = [false; 41]; 182 | for (i, &val) in self.reflex_reason1.iter().enumerate() { 183 | out[i] = val; 184 | } 185 | for (i, &val) in self.reflex_reason2.iter().enumerate() { 186 | out[i + 32] = val; 187 | } 188 | out 189 | } 190 | } 191 | 192 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 193 | #[allow(non_snake_case)] 194 | #[repr(packed)] 195 | pub struct MotionGeneratorCommandPacked { 196 | pub q_c: [f64; 7], 197 | pub dq_c: [f64; 7], 198 | pub O_T_EE_c: [f64; 16], 199 | pub O_dP_EE_c: [f64; 6], 200 | pub elbow_c: [f64; 2], 201 | pub valid_elbow: bool, 202 | pub motion_generation_finished: bool, 203 | } 204 | 205 | impl MotionGeneratorCommandPacked { 206 | pub fn new(command: MotionGeneratorCommand) -> Self { 207 | MotionGeneratorCommandPacked { 208 | q_c: command.q_c, 209 | dq_c: command.dq_c, 210 | O_T_EE_c: command.O_T_EE_c, 211 | O_dP_EE_c: command.O_dP_EE_c, 212 | elbow_c: command.elbow_c, 213 | valid_elbow: command.valid_elbow, 214 | motion_generation_finished: command.motion_generation_finished, 215 | } 216 | } 217 | } 218 | 219 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 220 | #[allow(non_snake_case)] 221 | pub struct MotionGeneratorCommand { 222 | pub q_c: [f64; 7], 223 | pub dq_c: [f64; 7], 224 | pub O_T_EE_c: [f64; 16], 225 | pub O_dP_EE_c: [f64; 6], 226 | pub elbow_c: [f64; 2], 227 | pub valid_elbow: bool, 228 | pub motion_generation_finished: bool, 229 | } 230 | 231 | #[allow(non_snake_case)] 232 | impl MotionGeneratorCommand { 233 | pub fn new( 234 | q_c: [f64; 7], 235 | dq_c: [f64; 7], 236 | O_T_EE_c: [f64; 16], 237 | O_dP_EE_c: [f64; 6], 238 | elbow_c: [f64; 2], 239 | ) -> Self { 240 | MotionGeneratorCommand { 241 | q_c, 242 | dq_c, 243 | O_T_EE_c, 244 | O_dP_EE_c, 245 | elbow_c, 246 | valid_elbow: false, 247 | motion_generation_finished: false, 248 | } 249 | } 250 | pub fn pack(self) -> MotionGeneratorCommandPacked { 251 | MotionGeneratorCommandPacked::new(self) 252 | } 253 | } 254 | 255 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 256 | #[allow(non_snake_case)] 257 | #[repr(packed)] 258 | pub struct ControllerCommandPacked { 259 | pub tau_J_d: [f64; 7], 260 | } 261 | 262 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 263 | #[allow(non_snake_case)] 264 | pub struct ControllerCommand { 265 | pub tau_J_d: [f64; 7], 266 | } 267 | 268 | impl ControllerCommand { 269 | pub fn pack(self) -> ControllerCommandPacked { 270 | ControllerCommandPacked { 271 | tau_J_d: self.tau_J_d, 272 | } 273 | } 274 | } 275 | 276 | #[derive(Serialize, Deserialize, Debug, Copy, Clone)] 277 | #[repr(packed)] 278 | pub struct RobotCommand { 279 | pub message_id: u64, 280 | pub motion: MotionGeneratorCommandPacked, 281 | pub control: ControllerCommandPacked, 282 | } 283 | -------------------------------------------------------------------------------- /src/robot/virtual_wall_cuboid.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! Contains model VirtualWallCuboid type. 5 | use crate::robot::service_types::GetCartesianLimitResponse; 6 | 7 | ///Parameters of a cuboid used as virtual wall. 8 | pub struct VirtualWallCuboid { 9 | ///ID of the virtual wall. 10 | pub id: i32, 11 | ///Corner point of the cuboid in world frame in \[m\]. 12 | pub object_world_size: [f64; 3], 13 | ///4x4 transformation matrix, column-major. 14 | pub p_frame: [f64; 16], 15 | ///True if this Cartesian limit is active, false otherwise. 16 | pub active: bool, 17 | } 18 | 19 | impl VirtualWallCuboid { 20 | pub fn new(id: i32, response: GetCartesianLimitResponse) -> Self { 21 | VirtualWallCuboid { 22 | id, 23 | object_world_size: response.object_world_size, 24 | p_frame: response.object_frame, 25 | active: response.object_activation, 26 | } 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/utils.rs: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | 4 | //! contains useful type definitions and conversion functions. 5 | use crate::robot::control_types::{Finishable, JointPositions}; 6 | use crate::robot::robot_state::RobotState; 7 | use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; 8 | use std::time::Duration; 9 | 10 | /// converts a 4x4 column-major homogenous matrix to an Isometry 11 | pub fn array_to_isometry(array: &[f64; 16]) -> Isometry3 { 12 | let rot = Rotation3::from_matrix( 13 | &Matrix4::from_column_slice(array) 14 | .remove_column(3) 15 | .remove_row(3), 16 | ); 17 | Isometry3::from_parts( 18 | Vector3::new(array[12], array[13], array[14]).into(), 19 | rot.into(), 20 | ) 21 | } 22 | /// A Vector with 7 entries 23 | pub type Vector7 = VectorN; 24 | /// A Matrix with 6 rows and 7 columns 25 | pub type Matrix6x7 = MatrixMN; 26 | /// A Matrix with 7 rows and 7 columns 27 | pub type Matrix7 = MatrixN; 28 | /// An example showing how to generate a joint pose motion to a goal position. Adapted from: 29 | /// Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots 30 | /// (Kogan Page Science Paper edition). 31 | #[derive(Debug)] 32 | pub struct MotionGenerator { 33 | q_goal: Vector7, 34 | q_start: Vector7, 35 | delta_q: Vector7, 36 | dq_max_sync: Vector7, 37 | t_1_sync: Vector7, 38 | t_2_sync: Vector7, 39 | t_f_sync: Vector7, 40 | q_1: Vector7, 41 | 42 | time: f64, 43 | dq_max: Vector7, 44 | ddq_max_start: Vector7, 45 | ddq_max_goal: Vector7, 46 | } 47 | 48 | impl MotionGenerator { 49 | const DELTA_Q_MOTION_FINISHED: f64 = 1e-6; 50 | /// Creates a new MotionGenerator instance for a target q. 51 | /// 52 | /// # Arguments 53 | /// * `speed_factor` - General speed factor in range [0, 1]. 54 | /// * `q_goal` - Target joint positions. 55 | pub fn new(speed_factor: f64, q_goal: &[f64; 7]) -> Self { 56 | let time = 0.; 57 | let q_goal = Vector7::from_row_slice(q_goal); 58 | let dq_max = Vector7::from_row_slice(&[2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5]) * speed_factor; 59 | let ddq_max_start = Vector7::from_row_slice(&[5.; 7]) * speed_factor; 60 | let ddq_max_goal = Vector7::from_row_slice(&[5.; 7]) * speed_factor; 61 | 62 | let dq_max_sync = Vector7::zeros(); 63 | let q_start = Vector7::zeros(); 64 | let delta_q = Vector7::zeros(); 65 | let t_1_sync = Vector7::zeros(); 66 | let t_2_sync = Vector7::zeros(); 67 | let t_f_sync = Vector7::zeros(); 68 | let q_1 = Vector7::zeros(); 69 | MotionGenerator { 70 | q_goal, 71 | q_start, 72 | delta_q, 73 | dq_max_sync, 74 | t_1_sync, 75 | t_2_sync, 76 | t_f_sync, 77 | q_1, 78 | time, 79 | dq_max, 80 | ddq_max_start, 81 | ddq_max_goal, 82 | } 83 | } 84 | fn calculate_desired_values(&self, t: f64, delta_q_d: &mut Vector7) -> bool { 85 | let sign_delta_q = Vector7::from_iterator(self.delta_q.iter().map(|&x| x.signum())); 86 | let t_d = self.t_2_sync - self.t_1_sync; 87 | let delta_t_2_sync = self.t_f_sync - self.t_2_sync; 88 | let mut joint_motion_finished = [false; 7]; 89 | for i in 0..7 { 90 | if self.delta_q[i].abs() < MotionGenerator::DELTA_Q_MOTION_FINISHED { 91 | delta_q_d[i] = 0.; 92 | joint_motion_finished[i] = true; 93 | } else if t < self.t_1_sync[i] { 94 | delta_q_d[i] = -1. / self.t_1_sync[i].powi(3) 95 | * self.dq_max_sync[i] 96 | * sign_delta_q[i] 97 | * (0.5 * t - self.t_1_sync[i]) 98 | * t.powi(3); 99 | } else if t >= self.t_1_sync[i] && t < self.t_2_sync[i] { 100 | delta_q_d[i] = 101 | self.q_1[i] + (t - self.t_1_sync[i]) * self.dq_max_sync[i] * sign_delta_q[i]; 102 | } else if t >= self.t_2_sync[i] && t < self.t_f_sync[i] { 103 | delta_q_d[i] = self.delta_q[i] 104 | + 0.5 105 | * (1. / delta_t_2_sync[i].powi(3) 106 | * (t - self.t_1_sync[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) 107 | * (t - self.t_1_sync[i] - t_d[i]).powi(3) 108 | + (2. * t - 2. * self.t_1_sync[i] - delta_t_2_sync[i] - 2. * t_d[i])) 109 | * self.dq_max_sync[i] 110 | * sign_delta_q[i]; 111 | } else { 112 | delta_q_d[i] = self.delta_q[i]; 113 | joint_motion_finished[i] = true; 114 | } 115 | } 116 | joint_motion_finished.iter().all(|i| *i) 117 | } 118 | fn calculate_synchronized_values(&mut self) { 119 | let mut dq_max_reach = self.dq_max; 120 | let mut t_f: Vector7 = Vector7::zeros(); 121 | let mut delta_t_2 = Vector7::zeros(); 122 | let mut t_1 = Vector7::zeros(); 123 | 124 | let mut delta_t_2_sync = Vector7::zeros(); 125 | let sign_delta_q = Vector7::from_iterator(self.delta_q.iter().map(|&x| x.signum())); 126 | for i in 0..7 { 127 | if self.delta_q[i].abs() > MotionGenerator::DELTA_Q_MOTION_FINISHED { 128 | if self.delta_q[i].abs() 129 | < (3.0 / 4.0 * (self.dq_max[i].powi(2) / self.ddq_max_start[i]) 130 | + 3. / 4. * (self.dq_max[i].powi(2) / self.ddq_max_goal[i])) 131 | { 132 | dq_max_reach[i] = f64::sqrt( 133 | 4. / 3. 134 | * self.delta_q[i] 135 | * sign_delta_q[i] 136 | * (self.ddq_max_start[i] * self.ddq_max_goal[i]) 137 | / (self.ddq_max_start[i] + self.ddq_max_goal[i]), 138 | ) 139 | } 140 | t_1[i] = 1.5 * dq_max_reach[i] / self.ddq_max_start[i]; 141 | delta_t_2[i] = 1.5 * dq_max_reach[i] / self.ddq_max_goal[i]; 142 | t_f[i] = 143 | t_1[i] / 2. + delta_t_2[i] / 2. + f64::abs(self.delta_q[i]) / dq_max_reach[i]; 144 | } 145 | } 146 | let max_t_f = t_f.max(); 147 | for i in 0..7 { 148 | if self.delta_q[i].abs() > MotionGenerator::DELTA_Q_MOTION_FINISHED { 149 | let a = 1.5 / 2. * (self.ddq_max_goal[i] + self.ddq_max_start[i]); 150 | let b = -1. * max_t_f * self.ddq_max_goal[i] * self.ddq_max_start[i]; 151 | let c = f64::abs(self.delta_q[i]) * self.ddq_max_goal[i] * self.ddq_max_start[i]; 152 | let delta = f64::max(b * b - 4. * a * c, 0.); 153 | self.dq_max_sync[i] = (-1. * b - f64::sqrt(delta)) / (2. * a); 154 | self.t_1_sync[i] = 1.5 * self.dq_max_sync[i] / self.ddq_max_start[i]; 155 | delta_t_2_sync[i] = 1.5 * self.dq_max_sync[i] / self.ddq_max_goal[i]; 156 | self.t_f_sync[i] = self.t_1_sync[i] / 2. 157 | + delta_t_2_sync[i] / 2. 158 | + f64::abs(self.delta_q[i] / self.dq_max_sync[i]); 159 | self.t_2_sync[i] = self.t_f_sync[i] - delta_t_2_sync[i]; 160 | self.q_1[i] = self.dq_max_sync[i] * sign_delta_q[i] * (0.5 * self.t_1_sync[i]) 161 | } 162 | } 163 | } 164 | /// Sends joint position calculations 165 | /// 166 | /// # Arguments 167 | /// `robot_state` - Current state of the robot. 168 | /// `period` - Duration of execution. 169 | /// 170 | /// # Return 171 | /// Joint positions for use inside a control loop. 172 | pub fn generate_motion( 173 | &mut self, 174 | robot_state: &RobotState, 175 | period: &Duration, 176 | ) -> JointPositions { 177 | self.time += period.as_secs_f64(); 178 | 179 | if self.time == 0. { 180 | self.q_start = Vector7::from_column_slice(&robot_state.q_d); 181 | self.delta_q = self.q_goal - self.q_start; 182 | self.calculate_synchronized_values(); 183 | } 184 | let mut delta_q_d = Vector7::zeros(); 185 | let motion_finished = self.calculate_desired_values(self.time, &mut delta_q_d); 186 | 187 | let mut output = JointPositions::new((self.q_start + delta_q_d).into()); 188 | output.set_motion_finished(motion_finished); 189 | output 190 | } 191 | } 192 | 193 | #[cfg(test)] 194 | mod test { 195 | use crate::{array_to_isometry, Finishable, MotionGenerator, RobotState}; 196 | use nalgebra::Rotation3; 197 | use std::time::Duration; 198 | 199 | fn slice_compare(a: &[f64], b: &[f64], thresh: f64) { 200 | for i in 0..a.len() { 201 | float_compare(a[i], b[i], thresh); 202 | } 203 | } 204 | 205 | fn float_compare(a: f64, b: f64, thresh: f64) { 206 | assert!((a - b).abs() < thresh); 207 | } 208 | 209 | #[test] 210 | fn motion_generator() { 211 | let q_des = [ 212 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 213 | [ 214 | 0.005530250638946291, 215 | 0.005530250638946291, 216 | 0.005530250638946291, 217 | 0.005530250638946291, 218 | 0.005530250638946291, 219 | 0.005530250638946291, 220 | 0.005530250638946291, 221 | ], 222 | [ 223 | 0.039797560667125885, 224 | 0.039797560667125885, 225 | 0.039797560667125885, 226 | 0.039797560667125885, 227 | 0.039797560667125885, 228 | 0.039797560667125885, 229 | 0.039797560667125885, 230 | ], 231 | [ 232 | 0.11931676725154987, 233 | 0.11931676725154987, 234 | 0.11931676725154987, 235 | 0.11931676725154987, 236 | 0.11931676725154987, 237 | 0.11931676725154987, 238 | 0.11931676725154987, 239 | ], 240 | [ 241 | 0.24726937422589595, 242 | 0.24726937422589595, 243 | 0.24726937422589595, 244 | 0.24726937422589595, 245 | 0.24726937422589595, 246 | 0.24726937422589595, 247 | 0.24726937422589595, 248 | ], 249 | [ 250 | 0.4135035520905085, 251 | 0.4135035520905085, 252 | 0.4135035520905085, 253 | 0.4135035520905085, 254 | 0.4135035520905085, 255 | 0.4135035520905085, 256 | 0.4135035520905085, 257 | ], 258 | [ 259 | 0.5946171259214752, 260 | 0.5946171259214752, 261 | 0.5946171259214752, 262 | 0.5946171259214752, 263 | 0.5946171259214752, 264 | 0.5946171259214752, 265 | 0.5946171259214752, 266 | ], 267 | [ 268 | 0.7595171113649393, 269 | 0.7595171113649393, 270 | 0.7595171113649393, 271 | 0.7595171113649393, 272 | 0.7595171113649393, 273 | 0.7595171113649393, 274 | 0.7595171113649393, 275 | ], 276 | [ 277 | 0.8853832940789487, 278 | 0.8853832940789487, 279 | 0.8853832940789487, 280 | 0.8853832940789487, 281 | 0.8853832940789487, 282 | 0.8853832940789487, 283 | 0.8853832940789487, 284 | ], 285 | [ 286 | 0.9626711625624701, 287 | 0.9626711625624701, 288 | 0.9626711625624701, 289 | 0.9626711625624701, 290 | 0.9626711625624701, 291 | 0.9626711625624701, 292 | 0.9626711625624701, 293 | ], 294 | [ 295 | 0.9951695386478036, 296 | 0.9951695386478036, 297 | 0.9951695386478036, 298 | 0.9951695386478036, 299 | 0.9951695386478036, 300 | 0.9951695386478036, 301 | 0.9951695386478036, 302 | ], 303 | [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 304 | ]; 305 | let mut state = RobotState::default(); 306 | let mut motion_generator = MotionGenerator::new(1.0, &[1.; 7]); 307 | let mut joint_pos = motion_generator.generate_motion(&state, &Duration::from_secs_f64(0.0)); 308 | slice_compare(&joint_pos.q, &q_des[0], 1e-10); 309 | let mut counter = 1; 310 | while !joint_pos.is_finished() { 311 | state.q_d = joint_pos.q; 312 | joint_pos = motion_generator.generate_motion(&state, &Duration::from_secs_f64(0.1)); 313 | slice_compare(&joint_pos.q, &q_des[counter], 1e-10); 314 | counter += 1; 315 | } 316 | assert_eq!(counter, q_des.len()) 317 | } 318 | 319 | #[test] 320 | fn array_to_isometry_nan_test_1() { 321 | let o_t_ee_c = [ 322 | 1.0, 323 | -0.000000011046552201160267, 324 | 0.000000008312911920110592, 325 | 0.0, 326 | -0.000000011046552077288223, 327 | -0.9999999999999999, 328 | -0.000000014901161362226844, 329 | 0.0, 330 | 0.000000008312912084717049, 331 | 0.000000014901161270397825, 332 | -0.9999999999999999, 333 | 0.0, 334 | 0.30689056578595225, 335 | -0.000000003883240449999549, 336 | 0.486882056335292, 337 | 1.0, 338 | ]; 339 | let last_o_t_ee_c = [ 340 | 0.9999903734042683, 341 | -0.000000011046444370332864, 342 | 0.00000000831299559264306, 343 | 0.0, 344 | -0.000000011046444370332864, 345 | -0.9999903734042683, 346 | -0.0000000021131709957245164, 347 | 0.0, 348 | 0.000000008313075597526054, 349 | 0.0000000021131912467324254, 350 | -0.9999999999999999, 351 | 0.0, 352 | 0.30689056578595225, 353 | -0.000000003883240449999549, 354 | 0.486882056335292, 355 | 1.0, 356 | ]; 357 | let commanded_pose = array_to_isometry(&o_t_ee_c); 358 | let last_commanded_pose = array_to_isometry(&last_o_t_ee_c); 359 | 360 | let mut rot_diff: Rotation3 = commanded_pose.rotation.to_rotation_matrix() 361 | * last_commanded_pose 362 | .rotation 363 | .to_rotation_matrix() 364 | .transpose(); 365 | rot_diff.renormalize(); 366 | for i in rot_diff.matrix().iter() { 367 | assert!(i.is_finite()); 368 | } 369 | } 370 | 371 | #[test] 372 | fn array_to_isometry_nan_test_2() { 373 | let y = [ 374 | 0.9999903734042686, 375 | 0.0000000002540163079878255, 376 | -0.00000000012581154368346085, 377 | 0.0, 378 | 0.0000000002540163079878255, 379 | -0.9999903734042686, 380 | 0.00000000004614105974113725, 381 | 0.0, 382 | -0.00000000012581275483128821, 383 | -0.000000000046141503958700795, 384 | -1.0, 385 | 0.0, 386 | 0.30689056659844144, 387 | 0.0000000000692086410879149, 388 | 0.4868820527992277, 389 | 1.0, 390 | ]; 391 | assert!(array_to_isometry(&y).rotation.angle().is_finite()) 392 | } 393 | } 394 | --------------------------------------------------------------------------------