diff --git a/.github/actions-rs/grcov.yml b/.github/actions-rs/grcov.yml new file mode 100644 index 0000000..db358d0 --- /dev/null +++ b/.github/actions-rs/grcov.yml @@ -0,0 +1,8 @@ +# grcov . -s . --binary-path ./target/debug/ -t html --branch --ignore-not-existing -o ./target/debug/coverage/ +source-dir: . +binary-path: ./target/debug/ +output-type: lcov +branch: true +ignore-not-existing: true +llvm: true +output-path: ./lcov.info diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index c9c193c..5fcbba3 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -22,5 +22,35 @@ jobs: run: cargo fmt -- --check - name: Clippy run: cargo clippy --all-features -- -D warnings - - name: Run tests - run: cargo test --all-features --verbose -- --test-threads=1 + - uses: actions-rs/toolchain@v1 + with: + toolchain: nightly + override: true + - uses: actions-rs/cargo@v1 + with: + command: clean + - uses: actions-rs/cargo@v1 + with: + command: test + args: --all-features --no-fail-fast --verbose -- --test-threads=1 + env: + CARGO_INCREMENTAL: '0' + RUSTFLAGS: '-Zprofile -Ccodegen-units=1 -Cinline-threshold=0 -Clink-dead-code -Coverflow-checks=off -Cpanic=abort -Zpanic_abort_tests -Cinstrument-coverage -Copt-level=0' + RUSTDOCFLAGS: '-Zprofile -Ccodegen-units=1 -Cinline-threshold=0 -Clink-dead-code -Coverflow-checks=off -Cpanic=abort -Zpanic_abort_tests -Cinstrument-coverage -Copt-level=0' + - uses: actions-rs/grcov@0.2-proto + with: + args: > + -t lcov + --llvm + --ignore-not-existing + --ignore "/*" + -o ./target/lcov.info + ./target/debug/ + - name: Codecov + uses: codecov/codecov-action@v3 + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} + with: + verbose: true + fail_ci_if_error: true + files: lcov.info diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..8e51d20 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,53 @@ +# Changelog + +## [0.10.0] - Unreleased + +This version supports Panda and Franka Research 3 robots. + +Requires Franka Research 3 system version >= 5.2.0 + +Requires Panda system version >= 4.2.1 + +### Added + +- Support for FR3 was added +- Most methods are now generic over the robot type, so they can work with FR3 and Panda robots +- Downloading of the robot model for other architectures +- New `RateLimiter` trait that is implemented by the `Panda` and `Fr3` structs +- The rate limiter for FR3 is using position based velocity limits +- Code coverage reports + +### Changed + +- The Robot struct became a trait that is implemented by the `Panda` and `Fr3` structs +- The Model struct became a trait that is implemented by the `RobotModel` struct +- Examples now default to FR3 unless `--panda` is passed as argument +- Rate limiter is disabled by default for FR3 +- FR3 does not have get_virtual_wall and set_filters methods +- Updated `nalgebra` to version 0.32 +- Updated `clap` to version 4 + +### Fixed + +- Badges in README + +## [0.9.0] - 2022-04-01 + +Requires Panda system version >= 4.2.1 + +### Added + +- Add `O_ddP_O` base acceleration to robot state, hardcoded to `{0, 0, -9.81}` +- New `BaseAccelerationInitializationTimeout`, `BaseAccelerationInvalidReading` + `CartesianSplineViolation` and + `JointViaPlanLimitViolation` reflexes + +### Changed + +- `Model::gravity_from_state` uses `O_ddP_O` from the robot state + +## [0.8.2-alpha-2] - 2021-06-18 + +Requires Panda system version >= 4.0.0 + +Initial release diff --git a/Cargo.toml b/Cargo.toml index 98048fd..183f9ef 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "libfranka-rs" -version = "0.9.0" +version = "0.10.0" authors = ["Marco Boneberger "] edition = "2018" license = "EUPL-1.2" @@ -79,11 +79,11 @@ nix = "0.20.0" mio = { version = "0.7", features = ["os-poll", "tcp", "udp"] } num-derive = "0.3" num-traits = "0.2" -nalgebra = "0.23" +nalgebra = "0.32" thiserror = "1.0" libloading = "0.7.0" [dev-dependencies] -clap = { version = "3.1.7", features = ["derive"] } +clap = { version = "4", features = ["derive"] } mockall = "0.9.1" float_extras = "0.1.6" diff --git a/README.md b/README.md index 0dcd04f..4a7b959 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,13 @@ -[![crates.io](https://img.shields.io/crates/v/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) -![GitHub Workflow Status](https://img.shields.io/github/workflow/status/marcbone/libfranka-rs/Rust) -[![crates.io](https://img.shields.io/crates/l/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) -[![crates.io](https://img.shields.io/crates/d/libfranka-rs.svg)](https://crates.io/crates/libfranka-rs) -[![docs.rs](https://docs.rs/libfranka-rs/badge.svg)](https://docs.rs/libfranka-rs) +[![Crates.io](https://img.shields.io/crates/v/libfranka-rs?style=flat-square)](https://crates.io/crates/libfranka-rs) +[![GitHub Workflow Status](https://img.shields.io/github/actions/workflow/status/marcbone/libfranka-rs/rust.yml?style=flat-square)](https://github.com/marcbone/libfranka-rs/actions) +[![Crates.io](https://img.shields.io/crates/l/libfranka-rs?style=flat-square)](https://github.com/marcbone/libfranka-rs/blob/master/LICENSE) +[![Crates.io](https://img.shields.io/crates/d/libfranka-rs?style=flat-square)](https://crates.io/crates/libfranka-rs) +[![docs.rs](https://img.shields.io/docsrs/libfranka-rs?style=flat-square)](https://docs.rs/libfranka-rs) +[![Codecov](https://img.shields.io/codecov/c/github/marcbone/libfranka-rs?style=flat-square)](https://codecov.io/gh/marcbone/libfranka-rs) + # libfranka-rs libfranka-rs is an **unofficial** port of [libfranka](https://github.com/frankaemika/libfranka) written in Rust. -This library can interact with research versions of Franka Emika Robots. +This library can interact with research versions of Franka Emika Robots (Panda and FR3). The library aims to provide researchers the possibility to experiment with Rust within a real-time robotics application. @@ -15,6 +17,7 @@ THE ROBOT!** ## Features * Real-time control of the robot * A libfranka-like API + * Generic over Panda and FR3 * Usage with Preempt_RT or stock Linux kernel * Usage of the gripper * Usage of the robot model @@ -24,28 +27,54 @@ THE ROBOT!** * Direct Conversions from [nalgebra](https://nalgebra.org/) (Eigen3 equivalent) types into libfranka control types (JointPositions, CartesianPose, ...) * Proper error handling with Result types -TODO: - * Usage of the Model for anything else but Linux x86_64 - Not supported: * Windows (macOS could maybe work, but I have not tested it) * Vacuum Grippers (we do not have those, so I cannot test them) ## Example A small example for controlling joint positions. You can find more in the examples folder. - ```rust -use franka::{FrankaResult, JointPositions, MotionFinished, Robot, RobotState}; +```rust + use std::time::Duration; + use std::f64::consts::PI; + use franka::{JointPositions, MotionFinished, Robot, RobotState, Fr3, FrankaResult}; + fn main() -> FrankaResult<()> { + let mut robot = Fr3::new("robotik-bs.de", None, None)?; + robot.set_default_behavior()?; + 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], + [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], + [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], + [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], [20.0, 20.0, 20.0, 25.0, 25.0, 25.0])?; + let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; + robot.joint_motion(0.5, &q_goal)?; + let mut initial_position = JointPositions::new([0.0; 7]); + let mut time = 0.; + let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { + time += time_step.as_secs_f64(); + if time == 0. { + initial_position.q = state.q_d; + } + let mut out = JointPositions::new(initial_position.q); + let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time)); + out.q[3] += delta_angle; + out.q[4] += delta_angle; + out.q[6] += delta_angle; + if time >= 5.0 { + return out.motion_finished(); + } + out + }; + robot.control_joint_positions(callback, None, None, None) + } +``` + +It is also possible to write the example above in a generic way, so it can be used for FR3 or Panda: +```rust +use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, Robot, RobotState}; use std::f64::consts::PI; use std::time::Duration; -fn main() -> FrankaResult<()> { - let mut robot = Robot::new("robotik-bs.de", None, None)?; - robot.set_default_behavior()?; - println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); - println!("Press Enter to continue..."); - std::io::stdin().read_line(&mut String::new()).unwrap(); - // Set additional parameters always before the control loop, NEVER in the control loop! - // Set collision behavior. +fn generate_motion(robot: &mut R) -> FrankaResult<()> { + robot.set_default_behavior()?; 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], @@ -56,11 +85,9 @@ fn main() -> FrankaResult<()> { [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], [20.0, 20.0, 20.0, 25.0, 25.0, 25.0], )?; - let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; robot.joint_motion(0.5, &q_goal)?; - println!("Finished moving to initial joint configuration."); - let mut initial_position = JointPositions::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); + let mut initial_position = JointPositions::new([0.0; 7]); let mut time = 0.; let callback = |state: &RobotState, time_step: &Duration| -> JointPositions { time += time_step.as_secs_f64(); @@ -73,17 +100,25 @@ fn main() -> FrankaResult<()> { out.q[4] += delta_angle; out.q[6] += delta_angle; if time >= 5.0 { - println!("Finished motion, shutting down example"); return out.motion_finished(); } out }; robot.control_joint_positions(callback, None, None, None) } - ``` +fn main() -> FrankaResult<()> { + let mut fr3 = Fr3::new("robotik-bs.de", None, None)?; + let mut panda = Panda::new("robotik-bs.de", None, None)?; + generate_motion(&mut fr3)?; + generate_motion(&mut panda) +} +``` + + ## How to get started -As it is a straight port, you may find the + +Since libfranka-rs is a port of libfranka, you may find the [Franka Control Interface Documentation](https://frankaemika.github.io/docs/index.html) helpful. ### With zero Rust knowledge @@ -91,7 +126,7 @@ If this is your first time using Rust, I recommend reading the [Rust Book](https If you have Rust installed and just want to play with the examples, you can also run: ```bash -cargo install libfranka-rs --examples --version 0.9.0 +cargo install libfranka-rs --examples --version 0.10.0 generate_joint_position_motion ``` @@ -104,18 +139,15 @@ hoping that it makes your introduction into the Rust world as smooth as possible ### With zero libfranka knowledge The [Franka Control Interface Documentation](https://frankaemika.github.io/docs/index.html) also includes a setup guide. You can skip the installation of libfranka as you will be using libfranka-rs. -Take a look at the [Documentation](https://docs.rs/libfranka-rs) and the examples folder. You should run the -communication_test example to verify that your setup is correct. +Take a look at the [Documentation](https://docs.rs/libfranka-rs) and the [examples](examples) folder. You should run the +[communication_test](examples/communication_test.rs) example to verify that your setup is correct. ### How to use libfranka-rs If you want to use libfranka-rs in your project, you have to add ```text -libfranka-rs = "0.9.0" +libfranka-rs = "0.10.0" ``` to your Cargo.toml file. -libfranka-rs version numbers are structured as MAJOR.MINOR.PATCH. The Major and Minor versions match the original libfranka -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 -libfranka-rs. You can find more information about system updates [here](https://frankaemika.github.io). ## Licence This library is copyrighted © 2021 Marco Boneberger diff --git a/examples/cartesian_impedance_control.rs b/examples/cartesian_impedance_control.rs index 0aac5c6..3857ffd 100644 --- a/examples/cartesian_impedance_control.rs +++ b/examples/cartesian_impedance_control.rs @@ -1,16 +1,16 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::Frame; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; -use franka::Torques; -use franka::{array_to_isometry, Matrix6x7, Vector7}; -use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3, U1, U3}; use std::time::Duration; +use clap::Parser; +use nalgebra::{Matrix3, Matrix6, Matrix6x1, UnitQuaternion, Vector3}; + +use franka::{ + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Model, Panda, Robot, RobotState, + Torques, Vector7, +}; + /// An example showing a simple cartesian impedance controller without inertia shaping /// that renders a spring damper system where the equilibrium is the initial configuration. /// After starting the controller try to push the robot around and try different stiffness levels. @@ -21,31 +21,46 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { + let model = robot.load_model(false)?; let translational_stiffness = 150.; let rotational_stiffness = 10.; let mut stiffness: Matrix6 = Matrix6::zeros(); let mut damping: Matrix6 = Matrix6::zeros(); { - let mut top_left_corner = stiffness.fixed_slice_mut::(0, 0); + let mut top_left_corner = stiffness.fixed_view_mut::<3, 3>(0, 0); top_left_corner.copy_from(&(Matrix3::identity() * translational_stiffness)); - let mut top_left_corner = damping.fixed_slice_mut::(0, 0); + let mut top_left_corner = damping.fixed_view_mut::<3, 3>(0, 0); top_left_corner.copy_from(&(2. * f64::sqrt(translational_stiffness) * Matrix3::identity())); } { - let mut bottom_right_corner = stiffness.fixed_slice_mut::(3, 3); + let mut bottom_right_corner = stiffness.fixed_view_mut::<3, 3>(3, 3); bottom_right_corner.copy_from(&(Matrix3::identity() * rotational_stiffness)); - let mut bottom_right_corner = damping.fixed_slice_mut::(3, 3); + let mut bottom_right_corner = damping.fixed_view_mut::<3, 3>(3, 3); bottom_right_corner .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); } - let mut robot = Robot::new(args.franka_ip.as_str(), None, None)?; - let model = robot.load_model(true)?; - // Set additional parameters always before the control loop, NEVER in the control loop! // Set collision behavior. robot.set_collision_behavior( @@ -67,8 +82,8 @@ fn main() -> FrankaResult<()> { std::io::stdin().read_line(&mut String::new()).unwrap(); let result = robot.control_torques( |state: &RobotState, _step: &Duration| -> Torques { - let coriolis: Vector7 = model.coriolis_from_state(&state).into(); - let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, &state); + let coriolis: Vector7 = model.coriolis_from_state(state).into(); + let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, state); let jacobian = Matrix6x7::from_column_slice(&jacobian_array); let _q = Vector7::from_column_slice(&state.q); let dq = Vector7::from_column_slice(&state.dq); @@ -78,7 +93,7 @@ fn main() -> FrankaResult<()> { let mut error: Matrix6x1 = Matrix6x1::::zeros(); { - let mut error_head = error.fixed_slice_mut::(0, 0); + let mut error_head = error.fixed_view_mut::<3, 1>(0, 0); error_head.set_column(0, &(position - position_d)); } @@ -88,7 +103,7 @@ fn main() -> FrankaResult<()> { let orientation = UnitQuaternion::new_normalize(orientation); let error_quaternion: UnitQuaternion = orientation.inverse() * orientation_d; { - let mut error_tail = error.fixed_slice_mut::(3, 0); + let mut error_tail = error.fixed_view_mut::<3, 1>(3, 0); error_tail.copy_from( &-(transform.rotation.to_rotation_matrix() * Vector3::new(error_quaternion.i, error_quaternion.j, error_quaternion.k)), @@ -98,7 +113,7 @@ fn main() -> FrankaResult<()> { jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq)); let tau_d: Vector7 = tau_task + coriolis; - tau_d.into() + Torques::new(tau_d.into()) }, None, None, diff --git a/examples/communication_test.rs b/examples/communication_test.rs index e3e953a..d8f44c0 100644 --- a/examples/communication_test.rs +++ b/examples/communication_test.rs @@ -1,13 +1,12 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; -use franka::{MotionFinished, Torques}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState, Torques}; + /// An example indicating the network performance. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -16,11 +15,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let args = CommandLineArguments::parse(); - let mut robot = Robot::new(args.franka_ip.as_str(), None, None)?; + let address = CommandLineArguments::parse(); + match address.panda { + true => { + let robot = Panda::new(address.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(address.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); diff --git a/examples/download_model.rs b/examples/download_model.rs index 745b1d1..05a1b3b 100644 --- a/examples/download_model.rs +++ b/examples/download_model.rs @@ -1,13 +1,14 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::exception::FrankaException::ModelException; -use franka::Robot; -use franka::{FrankaResult, RealtimeConfig}; use std::fs; use std::path::PathBuf; +use clap::Parser; + +use franka::exception::FrankaException::ModelException; +use franka::{Fr3, FrankaResult, Model, Panda, RealtimeConfig, Robot}; + /// Downloads the model for offline usage #[derive(Parser, Debug)] #[clap(author, version, name = "download_model")] @@ -16,18 +17,35 @@ struct CommandLineArguments { pub franka_ip: String, /// Directory where the model should be downloaded - #[clap(parse(from_os_str))] download_path: PathBuf, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { let args: CommandLineArguments = CommandLineArguments::parse(); - let mut path = args.download_path; - path.push("model.so"); - let mut robot = Robot::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; - robot.load_model(true)?; - fs::copy("/tmp/model.so", &path).map_err(|_| ModelException { - message: "Could copy model to download location".to_string(), + let path = args.download_path; + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + download_model(robot, path) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + download_model(robot, path) + } + } +} + +fn download_model(mut robot: R, mut path: PathBuf) -> FrankaResult<()> { + let model = robot.load_model(true)?; + let model_path = model.get_model_path().expect("Model file has been deleted"); + if path.is_dir() { + path.push(model_path.file_name().unwrap()); + } + fs::copy(model_path, &path).map_err(|_| ModelException { + message: "Could not copy model to download location".to_string(), })?; println!("Model successfully downloaded to {:?}", &path); Ok(()) diff --git a/examples/echo_robot_state.rs b/examples/echo_robot_state.rs index d1f552c..6a0eba8 100644 --- a/examples/echo_robot_state.rs +++ b/examples/echo_robot_state.rs @@ -2,9 +2,8 @@ // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; + +use franka::{Fr3, FrankaResult, Panda, Robot, RobotState}; /// An example showing how to continuously read the robot state. #[derive(Parser, Debug)] @@ -12,11 +11,26 @@ use franka::RobotState; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + echo_robot_state(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + echo_robot_state(robot) + } + } +} + +fn echo_robot_state(mut robot: R) -> FrankaResult<()> { let mut count = 0; robot.read(|robot_state: &RobotState| { // Printing to standard output adds a delay. This is acceptable for a read loop such as this, but @@ -25,6 +39,5 @@ fn main() -> FrankaResult<()> { count += 1; count <= 100 })?; - println!("Done"); Ok(()) } diff --git a/examples/generate_cartesian_pose_motion.rs b/examples/generate_cartesian_pose_motion.rs index 12d0e4e..26f25a8 100644 --- a/examples/generate_cartesian_pose_motion.rs +++ b/examples/generate_cartesian_pose_motion.rs @@ -1,14 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; -use franka::{CartesianPose, MotionFinished}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to generate a Cartesian motion. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -17,11 +16,27 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/generate_cartesian_velocity_motion.rs b/examples/generate_cartesian_velocity_motion.rs index 56a15e3..f45d1ea 100644 --- a/examples/generate_cartesian_velocity_motion.rs +++ b/examples/generate_cartesian_velocity_motion.rs @@ -1,11 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::{CartesianVelocities, FrankaResult, MotionFinished, Robot, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianVelocities, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to generate a Cartesian velocity motion. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -14,11 +16,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/generate_consecutive_motions.rs b/examples/generate_consecutive_motions.rs index ef1bb24..d422086 100644 --- a/examples/generate_consecutive_motions.rs +++ b/examples/generate_consecutive_motions.rs @@ -1,12 +1,14 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::exception::FrankaException; -use franka::{FrankaResult, JointVelocities, MotionFinished, Robot, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::exception::FrankaException; +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to execute consecutive motions with error recovery. /// /// WARNING: Before executing this example, make sure there is enough space in front and to the side @@ -16,11 +18,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/generate_elbow_motion.rs b/examples/generate_elbow_motion.rs index adb5232..f3fb583 100644 --- a/examples/generate_elbow_motion.rs +++ b/examples/generate_elbow_motion.rs @@ -1,11 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::{CartesianPose, FrankaResult, MotionFinished, Robot, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{CartesianPose, Fr3, FrankaResult, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to move the robot's elbow. /// /// WARNING: Before executing this example, make sure that the elbow has enough space to move. @@ -14,11 +16,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/generate_joint_position_motion.rs b/examples/generate_joint_position_motion.rs index f786529..09e708c 100644 --- a/examples/generate_joint_position_motion.rs +++ b/examples/generate_joint_position_motion.rs @@ -1,11 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::{FrankaResult, JointPositions, MotionFinished, Robot, RobotState}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to generate a joint position motion. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -14,11 +16,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/generate_joint_velocity_motion.rs b/examples/generate_joint_velocity_motion.rs index 013c638..b2adaae 100644 --- a/examples/generate_joint_velocity_motion.rs +++ b/examples/generate_joint_velocity_motion.rs @@ -1,14 +1,13 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; -use franka::FrankaResult; -use franka::Robot; -use franka::RobotState; -use franka::{JointVelocities, MotionFinished}; use std::f64::consts::PI; use std::time::Duration; +use clap::Parser; + +use franka::{Fr3, FrankaResult, JointVelocities, MotionFinished, Panda, Robot, RobotState}; + /// An example showing how to generate a joint velocity motion. /// /// WARNING: Before executing this example, make sure there is enough space in front of the robot. @@ -17,11 +16,26 @@ use std::time::Duration; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { - let address = CommandLineArguments::parse(); - let mut robot = Robot::new(address.franka_ip.as_str(), None, None)?; + let args = CommandLineArguments::parse(); + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { robot.set_default_behavior()?; println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!"); println!("Press Enter to continue..."); diff --git a/examples/grasp_object.rs b/examples/grasp_object.rs index 382b8ce..55de01f 100644 --- a/examples/grasp_object.rs +++ b/examples/grasp_object.rs @@ -1,8 +1,10 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later +use std::time::Duration; + use clap::Parser; + use franka::{FrankaResult, Gripper}; -use std::time::Duration; /// An example showing how to control FRANKA's gripper. #[derive(Parser, Debug)] @@ -28,7 +30,10 @@ fn main() -> FrankaResult<()> { eprintln!("Object is too large for the current fingers on the gripper."); std::process::exit(-1); } - gripper.grasp(args.object_width, 0.1, 60., None, None)?; + if !gripper.grasp(args.object_width, 0.1, 60., None, None)? { + eprintln!("Failed to grasp object."); + std::process::exit(-1); + } std::thread::sleep(Duration::from_secs(3)); let state = gripper.read_once()?; if !state.is_grasped { diff --git a/examples/mirror_robot.rs b/examples/mirror_robot.rs index 4caf3bb..241c1eb 100644 --- a/examples/mirror_robot.rs +++ b/examples/mirror_robot.rs @@ -1,18 +1,18 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use clap::Parser; + use core::f64::consts::PI; -use franka::exception::FrankaResult; -use franka::model::Frame; -use franka::robot::control_types::Torques; -use franka::robot::Robot; -use franka::utils::{array_to_isometry, Matrix6x7, Vector7}; -use franka::Matrix7; -use franka::RobotState; -use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3, U1, U3}; use std::sync::mpsc::channel; use std::time::Duration; +use clap::Parser; +use nalgebra::{Matrix3, Matrix6, Matrix6x1, Quaternion, UnitQuaternion, Vector3}; + +use franka::{ + array_to_isometry, Fr3, Frame, FrankaResult, Matrix6x7, Matrix7, Model, Panda, Robot, + RobotState, Torques, Vector7, +}; + /// An example where one robot is guided by the user and the other robot acts as a mirror. In this /// case mirror does not mean equal joint positions. Instead, it acts like a real physical mirror that /// stands in front of the robot (mirrored cartesian poses). Hand guide the end-effector of the user robot @@ -27,31 +27,69 @@ struct CommandLineArguments { /// IP-Address or hostname of the robot which mirrors the movement #[clap(long)] pub franka_ip_mirror: String, + + /// Use this option if the hand-guided robot is a Panda + #[clap(long, action)] + pub panda_user: bool, + + /// Use this option if the mirrored robot is a Panda + #[clap(long, action)] + pub panda_mirror: bool, } const NULLSPACE_TORQUE_SCALING: f64 = 5.; fn main() -> FrankaResult<()> { let args = CommandLineArguments::parse(); + match args.panda_user { + true => { + let robot_user = Panda::new(args.franka_ip_user.as_str(), None, None)?; + initialize_and_start_robots(&args, robot_user) + } + false => { + let robot_user = Fr3::new(args.franka_ip_user.as_str(), None, None)?; + initialize_and_start_robots(&args, robot_user) + } + } +} + +fn initialize_and_start_robots( + args: &CommandLineArguments, + robot_user: User, +) -> FrankaResult<()> { + match args.panda_mirror { + true => { + let robot_mirror = Panda::new(args.franka_ip_mirror.as_str(), None, None)?; + control_robots(robot_user, robot_mirror) + } + false => { + let robot_mirror = Fr3::new(args.franka_ip_mirror.as_str(), None, None)?; + control_robots(robot_user, robot_mirror) + } + } +} + +fn control_robots( + mut robot_user: User, + mut robot_mirror: Mirror, +) -> FrankaResult<()> { let translational_stiffness = 400.; let rotational_stiffness = 50.; let mut stiffness: Matrix6 = Matrix6::zeros(); let mut damping: Matrix6 = Matrix6::zeros(); { - let mut top_left_corner = stiffness.fixed_slice_mut::(0, 0); + let mut top_left_corner = stiffness.fixed_view_mut::<3, 3>(0, 0); top_left_corner.copy_from(&(Matrix3::identity() * translational_stiffness)); - let mut top_left_corner = damping.fixed_slice_mut::(0, 0); + let mut top_left_corner = damping.fixed_view_mut::<3, 3>(0, 0); top_left_corner.copy_from(&(2. * f64::sqrt(translational_stiffness) * Matrix3::identity())); } { - let mut bottom_right_corner = stiffness.fixed_slice_mut::(3, 3); + let mut bottom_right_corner = stiffness.fixed_view_mut::<3, 3>(3, 3); bottom_right_corner.copy_from(&(Matrix3::identity() * rotational_stiffness)); - let mut bottom_right_corner = damping.fixed_slice_mut::(3, 3); + let mut bottom_right_corner = damping.fixed_view_mut::<3, 3>(3, 3); bottom_right_corner .copy_from(&(2. * f64::sqrt(rotational_stiffness) * Matrix3::identity())); } - let mut robot_user = Robot::new(args.franka_ip_user.as_str(), None, None)?; - let mut robot_mirror = Robot::new(args.franka_ip_mirror.as_str(), None, None)?; let model = robot_mirror.load_model(true)?; robot_mirror.set_collision_behavior( [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6], [100.; 6], @@ -101,8 +139,8 @@ fn main() -> FrankaResult<()> { robot_mirror.control_torques( |state: &RobotState, _step: &Duration| -> Torques { let home: Vector7 = q_goal.into(); - let coriolis: Vector7 = model.coriolis_from_state(&state).into(); - let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, &state); + let coriolis: Vector7 = model.coriolis_from_state(state).into(); + let jacobian_array = model.zero_jacobian_from_state(&Frame::EndEffector, state); let jacobian = Matrix6x7::from_column_slice(&jacobian_array); let q = Vector7::from_column_slice(&state.q); let dq = Vector7::from_column_slice(&state.dq); @@ -126,7 +164,7 @@ fn main() -> FrankaResult<()> { orientation_d = UnitQuaternion::from_quaternion(quaternion); let mut error: Matrix6x1 = Matrix6x1::::zeros(); { - let mut error_head = error.fixed_slice_mut::(0, 0); + let mut error_head = error.fixed_view_mut::<3, 1>(0, 0); error_head.set_column(0, &(position - position_d)); } @@ -136,7 +174,7 @@ fn main() -> FrankaResult<()> { let orientation = UnitQuaternion::new_normalize(orientation); let error_quaternion: UnitQuaternion = orientation.inverse() * orientation_d; { - let mut error_tail = error.fixed_slice_mut::(3, 0); + let mut error_tail = error.fixed_view_mut::<3, 1>(3, 0); error_tail.copy_from( &-(transform.rotation.to_rotation_matrix() * Vector3::new(error_quaternion.i, error_quaternion.j, error_quaternion.k)), diff --git a/examples/print_joint_poses.rs b/examples/print_joint_poses.rs index c122cce..f707344 100644 --- a/examples/print_joint_poses.rs +++ b/examples/print_joint_poses.rs @@ -1,9 +1,10 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later use clap::Parser; -use franka::{Frame, FrankaResult, RealtimeConfig, Robot}; use nalgebra::Matrix4; +use franka::{Fr3, Frame, FrankaResult, Model, Panda, Robot}; + /// An example showing how to use the model library that prints the transformation /// matrix of each joint with respect to the base frame. #[derive(Parser, Debug)] @@ -11,11 +12,26 @@ use nalgebra::Matrix4; struct CommandLineArguments { /// IP-Address or hostname of the robot pub franka_ip: String, + /// Use this option to run the example on a Panda + #[clap(short, long, action)] + pub panda: bool, } fn main() -> FrankaResult<()> { let args = CommandLineArguments::parse(); - let mut robot = Robot::new(args.franka_ip.as_str(), RealtimeConfig::Ignore, None)?; + match args.panda { + true => { + let robot = Panda::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + false => { + let robot = Fr3::new(args.franka_ip.as_str(), None, None)?; + generate_motion(robot) + } + } +} + +fn generate_motion(mut robot: R) -> FrankaResult<()> { let model = robot.load_model(false)?; let state = robot.read_once()?; let frames = vec![ diff --git a/src/device_data.rs b/src/device_data.rs new file mode 100644 index 0000000..d93f827 --- /dev/null +++ b/src/device_data.rs @@ -0,0 +1,12 @@ +use crate::gripper::types::CommandHeader; + +pub(crate) trait DeviceData { + type CommandHeader: CommandHeader; + type CommandEnum; + fn create_header( + command_id: &mut u32, + command: Self::CommandEnum, + size: usize, + ) -> Self::CommandHeader; + fn get_library_version() -> u16; +} diff --git a/src/exception.rs b/src/exception.rs index 9e2a4fd..b15402c 100644 --- a/src/exception.rs +++ b/src/exception.rs @@ -3,6 +3,7 @@ //! Contains exception and Result definitions use crate::robot::logger::Record; +use crate::RobotState; use thiserror::Error; /// Represents all kind of errors which correspond to the franka::Exception in the C++ version of @@ -15,7 +16,7 @@ pub enum FrankaException { #[error("{error}")] ControlException { /// Vector of states and commands logged just before the exception occurred. - log: Option>, + log: Option>>, /// Explanatory string. error: String, }, @@ -66,15 +67,3 @@ pub(crate) fn create_command_exception(message: &'static str) -> FrankaException /// Result type which can have FrankaException as Error pub type FrankaResult = Result; -// wait for https://github.com/rust-lang/rust/issues/43301 to be closed -// impl Termination for FrankaResult<()> { -// fn report(self) -> i32 { -// return match self { -// Ok(_) => {ExitCode::SUCCESS.report();} -// Err(e) => { -// eprintln!("{}",e); -// ExitCode::FAILURE.report(); -// } -// } -// } -// } diff --git a/src/gripper.rs b/src/gripper.rs index ae99df1..e69830a 100644 --- a/src/gripper.rs +++ b/src/gripper.rs @@ -5,23 +5,24 @@ use std::mem::size_of; +pub use gripper_state::GripperState; + +use crate::device_data::DeviceData; use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::gripper::gripper_state::GripperState; use crate::gripper::types::{ - Command, ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, - GraspRequestWithHeader, GraspResponse, GripperStateIntern, HomingRequestWithHeader, - HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse, Status, - StopRequestWithHeader, StopResponse, COMMAND_PORT, VERSION, + ConnectRequest, ConnectRequestWithHeader, ConnectResponse, GraspRequest, + GraspRequestWithHeader, GripperCommandEnum, GripperCommandHeader, GripperStateIntern, + MoveRequest, MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, }; -use crate::network::{Network, NetworkType}; +use crate::network::Network; -pub mod gripper_state; +mod gripper_state; pub(crate) mod types; /// Maintains a network connection to the gripper, provides the current gripper state, /// and allows the execution of commands. pub struct Gripper { - network: Network, + network: Network, ri_version: Option, } @@ -34,21 +35,22 @@ impl Gripper { /// * [`IncompatibleLibraryVersionError`](`crate::exception::FrankaException::IncompatibleLibraryVersionError`) if this version of libfranka-rs is not supported pub fn new(franka_address: &str) -> FrankaResult { let mut gripper = Gripper { - network: Network::new(NetworkType::Gripper, franka_address, COMMAND_PORT).map_err( - |e| FrankaException::NetworkException { + network: Network::new(franka_address, COMMAND_PORT).map_err(|e| { + FrankaException::NetworkException { message: e.to_string(), - }, - )?, + } + })?, ri_version: None, }; - gripper.connect_gripper(&VERSION)?; + gripper.connect_gripper(&GRIPPER_VERSION)?; Ok(gripper) } - fn connect_gripper(&mut self, ri_version: &u16) -> Result<(), FrankaException> { + fn connect_gripper(&mut self, ri_version: &u16) -> FrankaResult<()> { let connect_command = ConnectRequestWithHeader { - header: self - .network - .create_header_for_gripper(Command::Connect, size_of::()), + header: self.network.create_header( + GripperCommandEnum::Connect, + size_of::(), + ), request: ConnectRequest::new(self.network.get_udp_port()), }; let command_id: u32 = self.network.tcp_send_request(connect_command); @@ -78,12 +80,12 @@ impl Gripper { let command = MoveRequestWithHeader { header: self .network - .create_header_for_gripper(Command::Move, size_of::()), + .create_header(GripperCommandEnum::Move, size_of::()), request: MoveRequest::new(width, speed), }; let command_id: u32 = self.network.tcp_send_request(command); - let response: MoveResponse = self.network.tcp_blocking_receive_response(command_id); - handle_response_status(&response.status) + let status: Status = self.network.tcp_blocking_receive_status(command_id); + handle_response_status(&status) } /// Performs homing of the gripper. /// @@ -96,12 +98,13 @@ impl Gripper { /// # Return /// True if command was successful, false otherwise. pub fn homing(&mut self) -> FrankaResult { - let command: HomingRequestWithHeader = self - .network - .create_header_for_gripper(Command::Homing, size_of::()); + let command: GripperCommandHeader = self.network.create_header( + GripperCommandEnum::Homing, + size_of::(), + ); let command_id: u32 = self.network.tcp_send_request(command); - let response: HomingResponse = self.network.tcp_blocking_receive_response(command_id); - handle_response_status(&response.status) + let status: Status = self.network.tcp_blocking_receive_status(command_id); + handle_response_status(&status) } /// Stops a currently running gripper move or grasp. @@ -111,12 +114,12 @@ impl Gripper { /// # Return /// True if command was successful, false otherwise. pub fn stop(&mut self) -> FrankaResult { - let command: StopRequestWithHeader = self + let command: GripperCommandHeader = self .network - .create_header_for_gripper(Command::Stop, size_of::()); + .create_header(GripperCommandEnum::Stop, size_of::()); let command_id: u32 = self.network.tcp_send_request(command); - let response: StopResponse = self.network.tcp_blocking_receive_response(command_id); - handle_response_status(&response.status) + let status: Status = self.network.tcp_blocking_receive_status(command_id); + handle_response_status(&status) } /// Grasps an object. @@ -147,14 +150,15 @@ impl Gripper { let epsilon_inner = epsilon_inner.unwrap_or(0.005); let epsilon_outer = epsilon_outer.unwrap_or(0.005); let command = GraspRequestWithHeader { - header: self - .network - .create_header_for_gripper(Command::Grasp, size_of::()), + header: self.network.create_header( + GripperCommandEnum::Grasp, + size_of::(), + ), request: GraspRequest::new(width, speed, force, epsilon_inner, epsilon_outer), }; let command_id: u32 = self.network.tcp_send_request(command); - let response: GraspResponse = self.network.tcp_blocking_receive_response(command_id); - handle_response_status(&response.status) + let status: Status = self.network.tcp_blocking_receive_status(command_id); + handle_response_status(&status) } /// Waits for a gripper state update and returns it. /// # Errors @@ -193,30 +197,37 @@ fn handle_response_status(status: &Status) -> FrankaResult { #[cfg(test)] mod tests { - use crate::gripper::types::{ - Command, CommandHeader, ConnectRequestWithHeader, ConnectResponse, GraspRequest, - GraspRequestWithHeader, GraspResponse, GripperStateIntern, HomingRequestWithHeader, - HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse, Status, - StopRequestWithHeader, StopResponse, COMMAND_PORT, VERSION, - }; - use crate::gripper::Gripper; - use crate::FrankaResult; - use bincode::{deserialize, serialize, serialized_size}; - use mio::net::UdpSocket; - use mockall::{automock, predicate::*}; use std::io::{Read, Write}; + use std::iter::FromIterator; + use std::mem::size_of; use std::net::TcpListener; use std::net::ToSocketAddrs; use std::rc::Rc; use std::sync::{Arc, Mutex}; use std::time::{Duration, Instant}; + use bincode::{deserialize, serialize, serialized_size}; + use mio::net::UdpSocket; + use mockall::{automock, predicate::*}; + use serde::{Deserialize, Serialize}; + use crate::exception::FrankaException; use crate::gripper::types::Status::{Fail, Success}; + use crate::gripper::types::{ + ConnectRequestWithHeader, ConnectResponse, GraspRequest, GraspRequestWithHeader, + GripperCommandEnum, GripperCommandHeader, GripperStateIntern, MoveRequest, + MoveRequestWithHeader, Status, COMMAND_PORT, GRIPPER_VERSION, + }; + use crate::gripper::Gripper; use crate::network::MessageCommand; - use std::iter::FromIterator; - use std::mem::size_of; + use crate::FrankaResult; + #[derive(Serialize, Deserialize, Clone, Copy)] + #[repr(packed)] + struct GripperResponse { + pub header: GripperCommandHeader, + pub status: Status, + } struct Socket), G: Fn(&mut Vec)> { pub send_bytes: F, pub receive_bytes: G, @@ -224,6 +235,7 @@ mod tests { struct GripperMockServer { server_version: u16, + srv_sock: TcpListener, } pub struct ServerReaction {} @@ -240,19 +252,26 @@ mod tests { } impl GripperMockServer { + const HOSTNAME: &'static str = "127.0.0.1"; pub fn new(server_version: u16) -> Self { - GripperMockServer { server_version } + let address = format!("{}:{}", Self::HOSTNAME, COMMAND_PORT); + let srv_sock = TcpListener::bind(address).unwrap(); + GripperMockServer { + server_version, + srv_sock, + } } pub fn server_thread(&mut self, reaction: &mut MockServerReaction) { - let hostname: &str = "127.0.0.1"; - let address = format!("{}:{}", hostname, COMMAND_PORT) - .to_socket_addrs() - .unwrap() - .next() - .unwrap(); - let srv_sock = TcpListener::bind(address).unwrap(); - let (tcp_socket, _remote_address) = srv_sock.accept().unwrap(); + let udp_socket = UdpSocket::bind( + format!("{}:1833", Self::HOSTNAME) + .to_socket_addrs() + .unwrap() + .next() + .unwrap(), + ) + .unwrap(); + let (tcp_socket, _remote_address) = self.srv_sock.accept().unwrap(); tcp_socket.set_nonblocking(false).unwrap(); tcp_socket.set_nodelay(true).unwrap(); let tcp_socket = Rc::new(Mutex::new(tcp_socket)); @@ -264,7 +283,7 @@ mod tests { }, receive_bytes: |bytes| { let mut soc = tcp_socket.lock().unwrap(); - let mut buffer = vec![0 as u8; 100]; + let mut buffer = vec![0_u8; 100]; let num_bytes = soc.read(&mut buffer).unwrap(); buffer.resize(num_bytes, 0); assert_eq!(buffer.len(), num_bytes); @@ -275,17 +294,9 @@ mod tests { let udp_port = request.request.udp_port; self.send_gripper_connect_response(request, &mut tcp_socket_wrapper); - let udp_socket = UdpSocket::bind( - format!("{}:1833", hostname) - .to_socket_addrs() - .unwrap() - .next() - .unwrap(), - ) - .unwrap(); udp_socket .connect( - format!("{}:{}", hostname, udp_port) + format!("{}:{}", Self::HOSTNAME, udp_port) .to_socket_addrs() .unwrap() .next() @@ -308,7 +319,7 @@ mod tests { let udp_thread = std::thread::spawn(move || { let mut counter = 1; let start = Instant::now(); - while (Instant::now() - start).as_secs_f64() < 0.01 { + while (Instant::now() - start).as_secs_f64() < 0.1 { let bytes = serialize(&GripperStateIntern { message_id: counter, width: 0.0, @@ -339,7 +350,7 @@ mod tests { F: Fn(&Vec), G: Fn(&mut Vec), { - let mut bytes = vec![0 as u8; 100]; + let mut bytes = vec![0_u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); let response = reaction.process_received_bytes(&mut bytes); (tcp_socket.send_bytes)(&response); @@ -348,10 +359,10 @@ mod tests { &self, tcp_socket: &mut Socket, ) -> ConnectRequestWithHeader { - let mut bytes = vec![0 as u8; 100]; + let mut bytes = vec![0_u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); let request: ConnectRequestWithHeader = deserialize(bytes.as_slice()).unwrap(); - return request; + request } fn send_gripper_connect_response), G: Fn(&mut Vec)>( &self, @@ -359,8 +370,8 @@ mod tests { tcp_socket: &mut Socket, ) { let mut response = ConnectResponse { - header: CommandHeader { - command: Command::Connect, + header: GripperCommandHeader { + command: GripperCommandEnum::Connect, command_id: request.get_command_message_id(), size: 0, }, @@ -384,8 +395,8 @@ mod tests { let mut generate_move_request = move |width: f64, speed: f64| -> MoveRequestWithHeader { counter += 1; MoveRequestWithHeader { - header: CommandHeader::new( - Command::Move, + header: GripperCommandHeader::new( + GripperCommandEnum::Move, counter, size_of::() as u32, ), @@ -398,9 +409,9 @@ mod tests { .map(|(x, y)| generate_move_request(*x, *y)), )); - let requests_server = requests.clone(); - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let requests_server = requests; + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -413,10 +424,14 @@ mod tests { .iter() .zip(serialized_expected_request.iter()) .for_each(|(x, y)| assert_eq!(x, y)); - let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); + let req: MoveRequestWithHeader = deserialize(bytes).unwrap(); counter += 1; - let mut response = MoveResponse { - header: CommandHeader::new(Command::Move, req.header.command_id, 0), + let mut response = GripperResponse { + header: GripperCommandHeader::new( + GripperCommandEnum::Move, + req.header.command_id, + 0, + ), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; @@ -427,9 +442,8 @@ mod tests { mock_gripper.server_thread(&mut mock); }); { - std::thread::sleep(Duration::from_secs_f64(0.01)); let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure"); - assert_eq!(gripper.server_version(), VERSION); + assert_eq!(gripper.server_version(), GRIPPER_VERSION); for (width, speed) in move_request_values.iter() { gripper.move_gripper(*width, *speed).unwrap(); } @@ -440,31 +454,34 @@ mod tests { #[test] fn gripper_stop_test() -> FrankaResult<()> { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { - let req: StopRequestWithHeader = deserialize(&bytes).unwrap(); + let req: GripperCommandHeader = deserialize(bytes).unwrap(); match req.command { - Command::Stop => {} + GripperCommandEnum::Stop => {} _ => { assert!(false) } } - let mut response = StopResponse { - header: CommandHeader::new(Command::Stop, req.command_id, 0), + let mut response = GripperResponse { + header: GripperCommandHeader::new( + GripperCommandEnum::Stop, + req.command_id, + 0, + ), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; serialize(&response).unwrap() }) .times(1); - mock.expect_number_of_reactions().return_const(1 as usize); + mock.expect_number_of_reactions().return_const(1_usize); mock_gripper.server_thread(&mut mock); }); { - std::thread::sleep(Duration::from_secs_f64(0.01)); let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure"); gripper.stop().unwrap(); } @@ -474,31 +491,34 @@ mod tests { #[test] fn gripper_homing_test() -> FrankaResult<()> { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(move |bytes: &mut Vec| -> Vec { - let req: HomingRequestWithHeader = deserialize(&bytes).unwrap(); + let req: GripperCommandHeader = deserialize(bytes).unwrap(); match req.command { - Command::Homing => {} + GripperCommandEnum::Homing => {} _ => { assert!(false) } } - let mut response = HomingResponse { - header: CommandHeader::new(Command::Homing, req.command_id, 0), + let mut response = GripperResponse { + header: GripperCommandHeader::new( + GripperCommandEnum::Homing, + req.command_id, + 0, + ), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; serialize(&response).unwrap() }) .times(1); - mock.expect_number_of_reactions().return_const(1 as usize); + mock.expect_number_of_reactions().return_const(1_usize); mock_gripper.server_thread(&mut mock); }); { - std::thread::sleep(Duration::from_secs_f64(0.01)); let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure"); gripper.homing().unwrap(); } @@ -518,8 +538,8 @@ mod tests { -> GraspRequestWithHeader { counter += 1; GraspRequestWithHeader { - header: CommandHeader::new( - Command::Grasp, + header: GripperCommandHeader::new( + GripperCommandEnum::Grasp, counter, size_of::() as u32, ), @@ -531,9 +551,9 @@ mod tests { |(a, b, c, d, e)| generate_grasp_request(*a, *b, *c, *d, *e), ))); - let requests_server = requests.clone(); - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION); + let requests_server = requests; + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -546,10 +566,14 @@ mod tests { .iter() .zip(serialized_expected_request.iter()) .for_each(|(x, y)| assert_eq!(x, y)); - let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); + let req: MoveRequestWithHeader = deserialize(bytes).unwrap(); counter += 1; - let mut response = GraspResponse { - header: CommandHeader::new(Command::Grasp, req.header.command_id, 0), + let mut response = GripperResponse { + header: GripperCommandHeader::new( + GripperCommandEnum::Grasp, + req.header.command_id, + 0, + ), status: Status::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; @@ -560,7 +584,6 @@ mod tests { mock_gripper.server_thread(&mut mock); }); { - std::thread::sleep(Duration::from_secs_f64(0.01)); let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure"); for (width, speed, force, epsilon_inner, epsilon_outer) in grasp_request_values.iter() { gripper @@ -580,17 +603,16 @@ mod tests { #[test] fn incompatible_library() { - let thread = std::thread::spawn(|| { - let mut mock_gripper = GripperMockServer::new(VERSION + 1); + let mut mock_gripper = GripperMockServer::new(GRIPPER_VERSION + 1); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); - mock.expect_number_of_reactions().return_const(0 as usize); + mock.expect_number_of_reactions().return_const(0_usize); mock_gripper.server_thread(&mut mock); }); let gripper_result; { - std::thread::sleep(Duration::from_secs_f64(0.01)); gripper_result = Gripper::new("127.0.0.1") } thread.join().unwrap(); @@ -609,19 +631,37 @@ mod tests { #[test] fn gripper_read_once() { - let thread = std::thread::spawn(|| { - let mut server = GripperMockServer::new(VERSION); + let mut server = GripperMockServer::new(GRIPPER_VERSION); + let thread = std::thread::spawn(move || { let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); - mock.expect_number_of_reactions().return_const(0 as usize); + mock.expect_number_of_reactions().return_const(0_usize); server.server_thread(&mut mock); }); { - std::thread::sleep(Duration::from_secs_f64(0.01)); let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure"); let _state = gripper.read_once().expect("could not read gripper state"); } thread.join().unwrap(); } } + +impl DeviceData for Gripper { + type CommandHeader = GripperCommandHeader; + type CommandEnum = GripperCommandEnum; + + fn create_header( + command_id: &mut u32, + command: Self::CommandEnum, + size: usize, + ) -> Self::CommandHeader { + let header = GripperCommandHeader::new(command, *command_id, size as u32); + *command_id += 1; + header + } + + fn get_library_version() -> u16 { + GRIPPER_VERSION + } +} diff --git a/src/gripper/types.rs b/src/gripper/types.rs index fcbf839..ad6d1ed 100644 --- a/src/gripper/types.rs +++ b/src/gripper/types.rs @@ -8,14 +8,15 @@ use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; use crate::network::MessageCommand; +use serde::de::DeserializeOwned; use std::time::Duration; -pub static VERSION: u16 = 3; +pub static GRIPPER_VERSION: u16 = 3; pub static COMMAND_PORT: u16 = 1338; #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u16)] -pub enum Command { +pub enum GripperCommandEnum { Connect, Homing, Grasp, @@ -23,7 +24,7 @@ pub enum Command { Stop, } -#[derive(Serialize_repr, Deserialize_repr, Debug)] +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u16)] pub enum Status { Success, @@ -47,18 +48,33 @@ impl GripperStateIntern { Duration::from_millis(self.message_id as u64) } } +// TODO is static a problem? +pub trait CommandHeader: Serialize + MessageCommand + Debug + DeserializeOwned + 'static { + fn get_command_id(&self) -> u32; + fn get_size(&self) -> u32; +} #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct CommandHeader { - pub command: Command, +pub struct GripperCommandHeader { + pub command: GripperCommandEnum, pub command_id: u32, pub size: u32, } -impl CommandHeader { - pub fn new(command: Command, command_id: u32, size: u32) -> CommandHeader { - CommandHeader { +impl CommandHeader for GripperCommandHeader { + fn get_command_id(&self) -> u32 { + self.command_id + } + + fn get_size(&self) -> u32 { + self.size + } +} + +impl GripperCommandHeader { + pub fn new(command: GripperCommandEnum, command_id: u32, size: u32) -> GripperCommandHeader { + GripperCommandHeader { command, command_id, size, @@ -98,7 +114,7 @@ pub struct GraspRequest { impl ConnectRequest { pub fn new(udp_port: u16) -> Self { ConnectRequest { - version: VERSION, + version: GRIPPER_VERSION, udp_port, } } @@ -127,28 +143,25 @@ impl GraspRequest { #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct ConnectRequestWithHeader { - pub header: CommandHeader, + pub header: GripperCommandHeader, pub request: ConnectRequest, } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct MoveRequestWithHeader { - pub header: CommandHeader, + pub header: GripperCommandHeader, pub request: MoveRequest, } -pub type HomingRequestWithHeader = CommandHeader; -pub type StopRequestWithHeader = CommandHeader; - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct GraspRequestWithHeader { - pub header: CommandHeader, + pub header: GripperCommandHeader, pub request: GraspRequest, } -impl MessageCommand for CommandHeader { +impl MessageCommand for GripperCommandHeader { fn get_command_message_id(&self) -> u32 { self.command_id } @@ -174,17 +187,7 @@ impl MessageCommand for GraspRequestWithHeader { #[derive(Serialize, Deserialize, Debug)] pub struct ConnectResponse { - pub header: CommandHeader, + pub header: GripperCommandHeader, pub status: Status, pub version: u16, } - -#[derive(Serialize, Deserialize, Debug)] -pub struct MoveResponse { - pub header: CommandHeader, - pub status: Status, -} - -pub type GraspResponse = MoveResponse; -pub type HomingResponse = MoveResponse; -pub type StopResponse = HomingResponse; diff --git a/src/lib.rs b/src/lib.rs index 9ce3423..6d913b2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -23,9 +23,9 @@ //!```no_run //! use std::time::Duration; //! use std::f64::consts::PI; -//! use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! use franka::{JointPositions, MotionFinished, Robot, RobotState, Fr3, FrankaResult}; //! fn main() -> FrankaResult<()> { -//! let mut robot = Robot::new("robotik-bs.de", None, None)?; +//! let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! robot.set_default_behavior()?; //! 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], //! [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], @@ -57,12 +57,12 @@ //! The main function returns a FrankaResult<()> which means that it returns either Ok(()) //! or an Error of type FrankaException which correspond to the C++ exceptions in libfranka. //! -//! ```no_run +//!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! let mut robot = Robot::new("robotik-bs.de", None, None)?; +//! let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! # Ok(()) //! # } //! ``` @@ -77,12 +77,12 @@ //! ``` //! this specifies the default collision behavior, joint impedance and Cartesian impedance //! -//! ```no_run +//!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! # use franka::{JointPositions, MotionFinished, RobotState, Fr3, Robot, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! # let mut robot = Robot::new("robotik-bs.de", None, None)?; +//! # let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.]; //! robot.joint_motion(0.5, &q_goal)?; //! # Ok(()) @@ -124,12 +124,12 @@ //! to the robot, therefore we use a cosine function. Without it we would get a CommandException //! while running. //! -//! ```no_run -//! # use franka::{JointPositions, MotionFinished}; +//!```no_run +//! # use franka::{MotionFinished, JointPositions}; //! # fn joint_positions() -> JointPositions { //! # let time = 0.; //! # let mut out = JointPositions::new([0.;7]); -//! +//! # //! if time >= 5.0 { //! return out.motion_finished(); //! } @@ -150,12 +150,12 @@ //! and returns JointPositions. //! //! With this callback we can now control the joint positions of the robot: -//! ```no_run +//!```no_run //! # use std::time::Duration; //! # use std::f64::consts::PI; -//! # use franka::{JointPositions, MotionFinished, RobotState, Robot, FrankaResult}; +//! # use franka::{JointPositions, Fr3, Robot, RobotState, FrankaResult}; //! # fn main() -> FrankaResult<()> { -//! # let mut robot = Robot::new("robotik-bs.de", None, None)?; +//! # let mut robot = Fr3::new("robotik-bs.de", None, None)?; //! # let callback = |state: &RobotState, time_step: &Duration| -> JointPositions {JointPositions::new([0.;7])}; //! robot.control_joint_positions(callback, None, None, None) //! # } @@ -168,17 +168,21 @@ pub mod gripper; mod network; pub mod robot; +mod device_data; pub mod model; pub mod utils; pub use exception::FrankaResult; -pub use gripper::gripper_state::GripperState; pub use gripper::Gripper; +pub use gripper::GripperState; pub use model::Frame; pub use model::Model; +pub use model::RobotModel; pub use robot::control_types::*; pub use robot::low_pass_filter::DEFAULT_CUTOFF_FREQUENCY; pub use robot::low_pass_filter::MAX_CUTOFF_FREQUENCY; -pub use robot::robot_state::RobotState; +pub use robot::Fr3; +pub use robot::Panda; pub use robot::Robot; +pub use robot::RobotState; pub use utils::*; diff --git a/src/model.rs b/src/model.rs index 191fc13..98e3518 100644 --- a/src/model.rs +++ b/src/model.rs @@ -1,29 +1,46 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -//! Contains model library types. +//! Types for accessing the robots mass matrix, gravity torques and more. +//! +//! [RobotModel] is the Model for the FR3 and for the Panda. +//! It is a wrapper around a shared object that is received from the robot. +//! The shared object contains methods to calculate jacobians, mass matrix etc. for the robot. +//! [RobotModel] implements the [Model] trait that +//! contains the methods to get the needed info from the Model. use nalgebra::Matrix4; use crate::model::model_library::ModelLibrary; use crate::robot::robot_state::RobotState; use crate::FrankaResult; use std::fmt; -use std::path::Path; +use std::path::{Path, PathBuf}; pub(crate) mod library_downloader; mod model_library; /// Enumerates the seven joints, the flange, and the end effector of a robot. pub enum Frame { + /// First joint Joint1, + /// Second joint Joint2, + /// Third joint Joint3, + /// Fourth joint Joint4, + /// Fifth joint Joint5, + /// Sixth joint Joint6, + /// Seventh joint Joint7, + /// Connector between Robot and Gripper Flange, + /// End-effector frame that is used for the [cartesian pose interface](crate::Robot::control_cartesian_pose) + /// and inverse kinematics. Can be changed with [`set_EE`](crate::Robot::set_EE) EndEffector, + /// End-effector frame that is used for measuring cartesian forces. Can be changed with [`set_K`](crate::Robot::set_K) Stiffness, } @@ -64,17 +81,13 @@ impl fmt::Display for Frame { } } -/// Calculates poses of joints and dynamic properties of the robot. -pub struct Model { - library: ModelLibrary, -} - -#[allow(non_snake_case)] -impl Model { +/// Provides jacobians, mass matrix, coriolis torques, gravity torques and forward kinematics of +/// a robot. +pub trait Model { /// Creates a new Model instance from the shared object of the Model for offline usage. /// /// If you just want to use the model to control the Robot you should use - /// [`Robot::load_model`](`crate::Robot::load_model`). + /// [`Robot::load_model`](crate::Robot::load_model). /// /// If you do not have the model you can use the download_model example to download the model /// # Arguments @@ -99,14 +112,13 @@ impl Model { /// Why do we need it? - Because the model is not self contained and relies on some functions from libm /// /// How does the libfranka embed libm? They are not including - Apparently it gets somehow included when using \ ¯\_(ツ)_/¯ - pub fn new>( - model_filename: S, - libm_filename: Option<&Path>, - ) -> FrankaResult { - Ok(Model { - library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, - }) - } + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult + where + Self: Sized; + + /// Returns the path where model is stored if it exists. + fn get_model_path(&self) -> Option; + /// Gets the 4x4 pose matrix for the given frame in base frame. /// /// The pose is represented as a 4x4 matrix in column-major format. @@ -117,7 +129,194 @@ impl Model { /// * `EE_T_K` - Stiffness frame K in the end effector frame. /// # Return /// Vectorized 4x4 pose matrix, column-major. - pub fn pose( + #[allow(non_snake_case)] + fn pose( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 16]; + + /// Gets the 4x4 pose matrix for the given frame in base frame. + /// + /// The pose is represented as a 4x4 matrix in column-major format. + /// # Arguments + /// * `frame` - The desired frame. + /// * `robot_state` - State from which the pose should be calculated. + /// # Return + /// Vectorized 4x4 pose matrix, column-major. + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16]; + + /// Gets the 6x7 Jacobian for the given frame, relative to that frame. + /// + /// The Jacobian is represented as a 6x7 matrix in column-major format. + /// # Arguments + /// * `frame` - The desired frame. + /// * `q` - Joint position. + /// * `F_T_EE` - End effector in flange frame. + /// * `EE_T_K` - Stiffness frame K in the end effector frame. + /// # Return + /// Vectorized 6x7 Jacobian, column-major. + #[allow(non_snake_case)] + fn body_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42]; + + /// Gets the 6x7 Jacobian for the given frame, relative to that frame. + /// + /// The Jacobian is represented as a 6x7 matrix in column-major format. + /// # Arguments + /// * `frame` - The desired frame. + /// * `robot_state` - State from which the pose should be calculated. + /// # Return + /// Vectorized 6x7 Jacobian, column-major. + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; + + /// Gets the 6x7 Jacobian for the given joint relative to the base frame. + /// + /// The Jacobian is represented as a 6x7 matrix in column-major format. + /// # Arguments + /// * `frame` - The desired frame. + /// * `q` - Joint position. + /// * `F_T_EE` - End effector in flange frame. + /// * `EE_T_K` - Stiffness frame K in the end effector frame. + /// # Return + /// Vectorized 6x7 Jacobian, column-major. + #[allow(non_snake_case)] + fn zero_jacobian( + &self, + frame: &Frame, + q: &[f64; 7], + F_T_EE: &[f64; 16], + EE_T_K: &[f64; 16], + ) -> [f64; 42]; + + /// Gets the 6x7 Jacobian for the given joint relative to the base frame. + /// + /// The Jacobian is represented as a 6x7 matrix in column-major format. + /// # Arguments + /// * `frame` - The desired frame. + /// * `robot_state` - State from which the pose should be calculated. + /// # Return + /// Vectorized 6x7 Jacobian, column-major. + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42]; + + /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2]. + /// # Arguments + /// * `q` - Joint position. + /// * `I_total` - Inertia of the attached total load including end effector, relative to + /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2]. + /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. + /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. + /// # Return + /// Vectorized 7x7 mass matrix, column-major. + #[allow(non_snake_case)] + fn mass( + &self, + q: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 49]; + + /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2]. + /// # Arguments + /// * `robot_state` - State from which the mass matrix should be calculated. + /// # Return + /// Vectorized 7x7 mass matrix, column-major. + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49]; + + /// Calculates the Coriolis force vector (state-space equation): + /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), in \[Nm\]. + /// # Arguments + /// * `q` - Joint position. + /// * `dq` - Joint velocity. + /// * `I_total` - Inertia of the attached total load including end effector, relative to + /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2]. + /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. + /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. + /// # Return + /// Coriolis force vector. + #[allow(non_snake_case)] + fn coriolis( + &self, + q: &[f64; 7], + dq: &[f64; 7], + I_total: &[f64; 9], + m_total: f64, + F_x_Ctotal: &[f64; 3], + ) -> [f64; 7]; + + /// Calculates the Coriolis force vector (state-space equation): + /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), in \[Nm\]. + /// # Arguments + /// * `robot_state` - State from which the Coriolis force vector should be calculated. + /// # Return + /// Coriolis force vector. + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7]; + + /// Calculates the gravity vector. Unit: \[Nm\]. + /// # Arguments + /// * `q` - Joint position. + /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. + /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. + /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2] + /// Default to [0.0, 0.0, -9.81]. + /// # Return + /// Gravity vector. + #[allow(non_snake_case)] + fn gravity<'a, Grav: Into>>( + &self, + q: &[f64; 7], + m_total: f64, + F_x_Ctotal: &[f64; 3], + gravity_earth: Grav, + ) -> [f64; 7]; + + /// Calculates the gravity vector. Unit: \[Nm\]. + /// # Arguments + /// * `robot_state` - State from which the gravity vector should be calculated. + /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2]. + /// By default `gravity_earth` will be calculated from [`RobotState::O_ddP_O`](`crate::RobotState::O_ddP_O`). + /// # Return + /// Gravity vector. + #[allow(non_snake_case)] + fn gravity_from_state<'a, Grav: Into>>( + &self, + robot_state: &RobotState, + gravity_earth: Grav, + ) -> [f64; 7]; +} + +/// Calculates poses of joints and dynamic properties of a robot. +pub struct RobotModel { + library: ModelLibrary, + model_file: PathBuf, +} + +#[allow(non_snake_case)] +impl Model for RobotModel { + fn new>(model_filename: S, libm_filename: Option<&Path>) -> FrankaResult { + Ok(RobotModel { + library: ModelLibrary::new(model_filename.as_ref(), libm_filename)?, + model_file: model_filename.as_ref().to_path_buf(), + }) + } + + fn get_model_path(&self) -> Option { + if self.model_file.exists() { + Some(self.model_file.to_owned()) + } else { + None + } + } + + fn pose( &self, frame: &Frame, q: &[f64; 7], @@ -147,15 +346,8 @@ impl Model { } output } - /// Gets the 4x4 pose matrix for the given frame in base frame. - /// - /// The pose is represented as a 4x4 matrix in column-major format. - /// # Arguments - /// * `frame` - The desired frame. - /// * `robot_state` - State from which the pose should be calculated. - /// # Return - /// Vectorized 4x4 pose matrix, column-major. - pub fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] { + + fn pose_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 16] { self.pose( frame, &robot_state.q, @@ -163,17 +355,8 @@ impl Model { &robot_state.EE_T_K, ) } - /// Gets the 6x7 Jacobian for the given frame, relative to that frame. - /// - /// The Jacobian is represented as a 6x7 matrix in column-major format. - /// # Arguments - /// * `frame` - The desired frame. - /// * `q` - Joint position. - /// * `F_T_EE` - End effector in flange frame. - /// * `EE_T_K` - Stiffness frame K in the end effector frame. - /// # Return - /// Vectorized 6x7 Jacobian, column-major. - pub fn body_jacobian( + + fn body_jacobian( &self, frame: &Frame, q: &[f64; 7], @@ -205,15 +388,7 @@ impl Model { output } - /// Gets the 6x7 Jacobian for the given frame, relative to that frame. - /// - /// The Jacobian is represented as a 6x7 matrix in column-major format. - /// # Arguments - /// * `frame` - The desired frame. - /// * `robot_state` - State from which the pose should be calculated. - /// # Return - /// Vectorized 6x7 Jacobian, column-major. - pub fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { + fn body_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.body_jacobian( frame, &robot_state.q, @@ -221,17 +396,8 @@ impl Model { &robot_state.EE_T_K, ) } - /// Gets the 6x7 Jacobian for the given joint relative to the base frame. - /// - /// The Jacobian is represented as a 6x7 matrix in column-major format. - /// # Arguments - /// * `frame` - The desired frame. - /// * `q` - Joint position. - /// * `F_T_EE` - End effector in flange frame. - /// * `EE_T_K` - Stiffness frame K in the end effector frame. - /// # Return - /// Vectorized 6x7 Jacobian, column-major. - pub fn zero_jacobian( + + fn zero_jacobian( &self, frame: &Frame, q: &[f64; 7], @@ -262,15 +428,8 @@ impl Model { } output } - /// Gets the 6x7 Jacobian for the given joint relative to the base frame. - /// - /// The Jacobian is represented as a 6x7 matrix in column-major format. - /// # Arguments - /// * `frame` - The desired frame. - /// * `robot_state` - State from which the pose should be calculated. - /// # Return - /// Vectorized 6x7 Jacobian, column-major. - pub fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { + + fn zero_jacobian_from_state(&self, frame: &Frame, robot_state: &RobotState) -> [f64; 42] { self.zero_jacobian( frame, &robot_state.q, @@ -278,16 +437,8 @@ impl Model { &robot_state.EE_T_K, ) } - /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2]. - /// # Arguments - /// * `q` - Joint position. - /// * `I_total` - Inertia of the attached total load including end effector, relative to - /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2]. - /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. - /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. - /// # Return - /// Vectorized 7x7 mass matrix, column-major. - pub fn mass( + + fn mass( &self, q: &[f64; 7], I_total: &[f64; 9], @@ -299,12 +450,8 @@ impl Model { .mass(q, I_total, &m_total, F_x_Ctotal, &mut output); output } - /// Calculates the 7x7 mass matrix. Unit: [kg \times m^2]. - /// # Arguments - /// * `robot_state` - State from which the mass matrix should be calculated. - /// # Return - /// Vectorized 7x7 mass matrix, column-major. - pub fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] { + + fn mass_from_state(&self, robot_state: &RobotState) -> [f64; 49] { self.mass( &robot_state.q, &robot_state.I_total, @@ -312,18 +459,8 @@ impl Model { &robot_state.F_x_Ctotal, ) } - /// Calculates the Coriolis force vector (state-space equation): - /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), in \[Nm\]. - /// # Arguments - /// * `q` - Joint position. - /// * `dq` - Joint velocity. - /// * `I_total` - Inertia of the attached total load including end effector, relative to - /// center of mass, given as vectorized 3x3 column-major matrix. Unit: [kg * m^2]. - /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. - /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. - /// # Return - /// Coriolis force vector. - pub fn coriolis( + + fn coriolis( &self, q: &[f64; 7], dq: &[f64; 7], @@ -337,13 +474,7 @@ impl Model { output } - /// Calculates the Coriolis force vector (state-space equation): - /// ![c= C \times dq](https://latex.codecogs.com/png.latex?c=&space;C&space;\times&space;dq), in \[Nm\]. - /// # Arguments - /// * `robot_state` - State from which the Coriolis force vector should be calculated. - /// # Return - /// Coriolis force vector. - pub fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] { + fn coriolis_from_state(&self, robot_state: &RobotState) -> [f64; 7] { self.coriolis( &robot_state.q, &robot_state.dq, @@ -352,16 +483,8 @@ impl Model { &robot_state.F_x_Ctotal, ) } - /// Calculates the gravity vector. Unit: \[Nm\]. - /// # Arguments - /// * `q` - Joint position. - /// * `m_total` - Weight of the attached total load including end effector. Unit: \[kg\]. - /// * `F_x_Ctotal` - Translation from flange to center of mass of the attached total load. Unit: \[m\]. - /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2] - /// Default to [0.0, 0.0, -9.81]. - /// # Return - /// Gravity vector. - pub fn gravity<'a, Grav: Into>>( + + fn gravity<'a, Grav: Into>>( &self, q: &[f64; 7], m_total: f64, @@ -374,14 +497,8 @@ impl Model { .gravity(q, gravity_earth, &m_total, F_x_Ctotal, &mut output); output } - /// Calculates the gravity vector. Unit: \[Nm\]. - /// # Arguments - /// * `robot_state` - State from which the gravity vector should be calculated. - /// * `gravity_earth` - Earth's gravity vector. Unit: [m / s^2]. - /// By default `gravity_earth` will be calculated from [`RobotState::O_ddP_O`](`crate::RobotState::O_ddP_O`). - /// # Return - /// Gravity vector. - pub fn gravity_from_state<'a, Grav: Into>>( + + fn gravity_from_state<'a, Grav: Into>>( &self, robot_state: &RobotState, gravity_earth: Grav, diff --git a/src/model/library_downloader.rs b/src/model/library_downloader.rs index 5a7b99d..76cb013 100644 --- a/src/model/library_downloader.rs +++ b/src/model/library_downloader.rs @@ -1,68 +1,71 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + +use std::fs::File; +use std::io::Write; +use std::path::Path; + use crate::exception::FrankaException::ModelException; use crate::network::Network; use crate::robot::service_types::{ - LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryRequestWithHeader, - LoadModelLibraryResponse, LoadModelLibrarySystem, RobotCommandEnum, + LoadModelLibraryArchitecture, LoadModelLibraryRequest, LoadModelLibraryStatus, + LoadModelLibrarySystem, }; +use crate::robot::PrivateRobotData; use crate::FrankaResult; -use std::fmt; -use std::fmt::Display; -use std::fmt::Formatter; -use std::fs::File; -use std::io::Write; -use std::mem::size_of; -use std::path::Path; -pub struct LibraryDownloader {} +pub(crate) trait LibraryDownloader { + type Data: PrivateRobotData; + fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()>; +} -impl LibraryDownloader { - pub fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { - if cfg!(all(target_os = "linux", target_arch = "x86_64")) { - let command = LoadModelLibraryRequestWithHeader { - header: network.create_header_for_robot( - RobotCommandEnum::LoadModelLibrary, - size_of::(), - ), - request: LoadModelLibraryRequest { - architecture: LoadModelLibraryArchitecture::X64, - system: LoadModelLibrarySystem::Linux, - }, - }; - let command_id: u32 = network.tcp_send_request(command); - let mut buffer = Vec::::new(); - let _response: LoadModelLibraryResponse = - network.tcp_blocking_receive_load_library_response(command_id, &mut buffer)?; - let mut file = File::create(download_path).map_err(|_| ModelException { - message: "Error writing model to disk:".to_string(), - })?; - file.write(&buffer).map_err(|_| ModelException { - message: "Error writing model to disk:".to_string(), - })?; - Ok(()) +pub(crate) struct LibraryDownloaderGeneric { + data: std::marker::PhantomData, +} + +impl LibraryDownloader for LibraryDownloaderGeneric { + type Data = Data; + + fn download(network: &mut Network, download_path: &Path) -> FrankaResult<()> { + let request = if cfg!(all(target_os = "linux", target_arch = "x86_64")) { + LoadModelLibraryRequest { + architecture: LoadModelLibraryArchitecture::X64, + system: LoadModelLibrarySystem::Linux, + } + } else if cfg!(all(target_os = "linux", target_arch = "aarch64")) { + LoadModelLibraryRequest { + architecture: LoadModelLibraryArchitecture::Arm64, + system: LoadModelLibrarySystem::Linux, + } + } else if cfg!(all(target_os = "linux", target_arch = "x86")) { + LoadModelLibraryRequest { + architecture: LoadModelLibraryArchitecture::X86, + system: LoadModelLibrarySystem::Linux, + } + } else if cfg!(all(target_os = "linux", target_arch = "arm")) { + LoadModelLibraryRequest { + architecture: LoadModelLibraryArchitecture::Arm, + system: LoadModelLibrarySystem::Linux, + } } else { - Err(ModelException { + return Err(ModelException { message: "Your platform is not yet supported for Downloading models. Please use Linux on\ - x86_64 for now" + x86_64, x86, arm, or arm64 for now" .to_string(), - }) - } - } -} - -#[derive(Debug)] -pub struct UnsupportedPlatform {} - -impl std::error::Error for UnsupportedPlatform {} - -impl Display for UnsupportedPlatform { - fn fmt(&self, f: &mut Formatter) -> fmt::Result { - writeln!( - f, - "Your platform is not yet supported for Downloading models. Please use Linux on\ - x86_64 for now" - ) + }); + }; + let command = Data::create_model_library_request(&mut network.command_id, request); + let command_id: u32 = network.tcp_send_request(command); + let mut buffer = Vec::::new(); + let _status: LoadModelLibraryStatus = + network.tcp_blocking_receive_load_library_response(command_id, &mut buffer)?; + let mut file = File::create(download_path).map_err(|_| ModelException { + message: "Error writing model to disk:".to_string(), + })?; + file.write(&buffer).map_err(|_| ModelException { + message: "Error writing model to disk:".to_string(), + })?; + Ok(()) } } diff --git a/src/model/model_library.rs b/src/model/model_library.rs index ad7a489..f8f5dde 100644 --- a/src/model/model_library.rs +++ b/src/model/model_library.rs @@ -1,5 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + use crate::exception::FrankaException; use crate::exception::FrankaException::ModelException; use crate::FrankaResult; @@ -10,7 +11,7 @@ use libloading::os::unix::Symbol; use std::path::Path; #[allow(non_snake_case, dead_code)] -pub struct ModelLibrary { +pub(crate) struct ModelLibrary { libm: Library, lib_model: Library, M_NE: Symbol< diff --git a/src/network.rs b/src/network.rs index ec999ea..670234a 100644 --- a/src/network.rs +++ b/src/network.rs @@ -1,5 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + #![allow(dead_code)] extern crate libc; @@ -9,6 +10,7 @@ use std::collections::HashMap; use std::error::Error; use std::fmt::Debug; use std::io::{Read, Write}; +use std::marker::PhantomData; use std::mem::size_of; use std::net::TcpStream as StdTcpStream; use std::net::{IpAddr, SocketAddr, ToSocketAddrs}; @@ -26,16 +28,17 @@ use serde::de::DeserializeOwned; use serde::Serialize; use crate::exception::{FrankaException, FrankaResult}; -use crate::gripper; use crate::gripper::types::CommandHeader; -use crate::robot::service_types::{ - LoadModelLibraryResponse, LoadModelLibraryStatus, RobotCommandEnum, RobotCommandHeader, -}; + +use crate::robot::service_types::{LoadModelLibraryStatus, PandaCommandEnum, PandaCommandHeader}; + +use crate::device_data::DeviceData; const CLIENT: Token = Token(1); pub enum NetworkType { - Robot, + Panda, + Fr3, Gripper, } @@ -43,8 +46,7 @@ pub trait MessageCommand { fn get_command_message_id(&self) -> u32; } -pub struct Network { - network_type: NetworkType, +pub(crate) struct Network { tcp_socket: TcpStream, udp_socket: UdpSocket, udp_server_address: SocketAddr, @@ -60,14 +62,11 @@ pub struct Network { events: Events, poll_read_udp: Poll, events_udp: Events, + data: PhantomData, } -impl Network { - pub fn new( - network_type: NetworkType, - franka_address: &str, - franka_port: u16, - ) -> Result> { +impl Network { + pub fn new(franka_address: &str, franka_port: u16) -> Result, Box> { let address_str: String = format!("{}:{}", franka_address, franka_port); let sock_address = address_str.to_socket_addrs().unwrap().next().unwrap(); let mut tcp_socket = TcpStream::from_std(StdTcpStream::connect(sock_address)?); @@ -101,7 +100,6 @@ impl Network { let events = Events::with_capacity(128); let events_udp = Events::with_capacity(1); Ok(Network { - network_type, tcp_socket, udp_socket, udp_server_address, @@ -117,31 +115,29 @@ impl Network { events, poll_read_udp, events_udp, + data: PhantomData, }) } - pub fn create_header_for_gripper( + + pub fn create_header_for_panda( &mut self, - command: gripper::types::Command, + command: PandaCommandEnum, size: usize, - ) -> CommandHeader { - let header = gripper::types::CommandHeader::new(command, self.command_id, size as u32); + ) -> PandaCommandHeader { + let header = PandaCommandHeader::new(command, self.command_id, size as u32); self.command_id += 1; header } - pub fn create_header_for_robot( + + pub fn create_header( &mut self, - command: RobotCommandEnum, + command: Data::CommandEnum, size: usize, - ) -> RobotCommandHeader { - let header = RobotCommandHeader::new(command, self.command_id, size as u32); - self.command_id += 1; - header + ) -> Data::CommandHeader { + Data::create_header(&mut self.command_id, command, size) } - pub fn tcp_send_request( - &mut self, - request: T, - ) -> u32 { + pub fn tcp_send_request(&mut self, request: T) -> u32 { let encoded_request = serialize(&request); self.tcp_socket.write_all(&encoded_request).unwrap(); request.get_command_message_id() @@ -151,13 +147,26 @@ impl Network { /// /// # Arguments /// * `command_id` - Expected command ID of the Response. - pub fn tcp_blocking_receive_response( + pub fn tcp_blocking_receive_response( &mut self, command_id: u32, ) -> T { let response_bytes = self.wait_for_response_to_arrive(&command_id); deserialize(&response_bytes) } + /// Blocks until a Response message with the given command ID has been received and returns this + /// response. + /// + /// # Arguments + /// * `command_id` - Expected command ID of the Response. + pub fn tcp_blocking_receive_status( + &mut self, + command_id: u32, + ) -> T { + let response_bytes = self.wait_for_response_to_arrive(&command_id); + let (_, out): (Data::CommandHeader, T) = deserialize(&response_bytes); + out + } /// Blocks until a LoadModelLibraryResponse message with the given command ID has been received /// and returns this LoadModelLibraryResponse. /// # Arguments @@ -173,11 +182,13 @@ impl Network { &mut self, command_id: u32, buffer: &mut Vec, - ) -> FrankaResult { + ) -> FrankaResult { let response_bytes = self.wait_for_response_to_arrive(&command_id); - let lib_response: LoadModelLibraryResponse = - deserialize(&response_bytes[0..size_of::()]); - match lib_response.status { + let (header, status): (Data::CommandHeader, LoadModelLibraryStatus) = deserialize( + &response_bytes + [0..size_of::() + size_of::()], + ); + match status { LoadModelLibraryStatus::Success => {} LoadModelLibraryStatus::Error => { return Err(FrankaException::ModelException { @@ -187,13 +198,14 @@ impl Network { } } assert_ne!( - lib_response.header.size as usize, - size_of::() + header.get_size() as usize, + size_of::() + size_of::() ); buffer.append(&mut Vec::from( - &response_bytes[size_of::()..], + &response_bytes + [size_of::() + size_of::()..], )); - Ok(lib_response) + Ok(status) } /// Tries to receive a Response message with the given command ID (non-blocking). /// @@ -214,18 +226,18 @@ impl Network { ) -> Result where F: FnOnce(T) -> Result<(), FrankaException>, - T: DeserializeOwned + Debug + 'static, + T: DeserializeOwned + 'static, { self.tcp_read_from_buffer(Duration::from_micros(0)); let message = self.received_responses.get(&command_id); if message.is_none() { return Ok(false); } - if message.unwrap().len() != size_of::() { + if message.unwrap().len() != size_of::() + size_of::() { panic!("libfranka-rs: Incorrect TCP message size."); } - let message: T = deserialize(message.unwrap()); - let result = handler(message); + let message: (Data::CommandHeader, T) = deserialize(message.unwrap()); + let result = handler(message.1); match result { Ok(_) => { self.received_responses.remove(&command_id); @@ -237,7 +249,7 @@ impl Network { fn wait_for_response_to_arrive(&mut self, command_id: &u32) -> Vec { let mut response_bytes: Option> = None; - while response_bytes == None { + while response_bytes.is_none() { { self.tcp_read_from_buffer(Duration::from_millis(10)); response_bytes = self.received_responses.remove(command_id); @@ -323,7 +335,7 @@ impl Network { match event.token() { CLIENT => { if event.is_readable() { - let mut buffer = [0_u8; 70000]; + let mut buffer = [0_u8; 150000]; let available_bytes = self.tcp_socket.peek(&mut buffer); let available_bytes = match available_bytes { Ok(a) => a, @@ -334,39 +346,27 @@ impl Network { }; if self.pending_response.is_empty() { - let header_mem_size = match self.network_type { - NetworkType::Gripper => size_of::(), - NetworkType::Robot => size_of::(), - }; + let header_mem_size = size_of::(); if available_bytes >= header_mem_size { let mut header_bytes: Vec = vec![0; header_mem_size]; self.tcp_socket.read_exact(&mut header_bytes).unwrap(); self.pending_response.append(&mut header_bytes.clone()); - self.pending_response_offset = header_mem_size as usize; - match self.network_type { - NetworkType::Gripper => { - let header: CommandHeader = deserialize(&header_bytes); - self.pending_response_len = header.size as usize; - self.pending_command_id = header.command_id; - } - NetworkType::Robot => { - let header: RobotCommandHeader = deserialize(&header_bytes); - self.pending_response_len = header.size as usize; - self.pending_command_id = header.command_id; - } - }; + self.pending_response_offset = header_mem_size; + let header: Data::CommandHeader = deserialize(&header_bytes); + self.pending_response_len = header.get_size() as usize; + self.pending_command_id = header.get_command_id(); } } if !self.pending_response.is_empty() && available_bytes > 0 { let number_of_bytes_to_read = usize::min( available_bytes, - self.pending_response_len - self.pending_response_offset as usize, + self.pending_response_len - self.pending_response_offset, ); let mut response_buffer: Vec = vec![0; number_of_bytes_to_read]; self.tcp_socket.read_exact(&mut response_buffer).unwrap(); self.pending_response.append(&mut response_buffer); self.pending_response_offset += number_of_bytes_to_read; - if self.pending_response_offset == self.pending_response_len as usize { + if self.pending_response_offset == self.pending_response_len { self.received_responses .insert(self.pending_command_id, self.pending_response.clone()); self.pending_response.clear(); @@ -393,19 +393,19 @@ fn serialize(s: &T) -> Vec { bincode::serialize(s).unwrap() } -fn deserialize(encoded: &[u8]) -> T { +fn deserialize(encoded: &[u8]) -> T { bincode::deserialize(encoded).unwrap() } #[cfg(test)] mod tests { use crate::network::{deserialize, serialize}; - use crate::robot::types::RobotStateIntern; + use crate::robot::types::PandaStateIntern; #[test] fn can_serialize_and_deserialize() { - let state = RobotStateIntern::dummy(); - let state2: RobotStateIntern = deserialize(&serialize(&state)); + let state = PandaStateIntern::dummy(); + let state2: PandaStateIntern = deserialize(&serialize(&state)); assert_eq!(state, state2); } } diff --git a/src/robot.rs b/src/robot.rs index 05986bf..c932293 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -1,69 +1,60 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -//! Contains the franka::Robot type. -use std::mem::size_of; -use std::time::Duration; - -use std::fmt::Debug; +//! Contains the Robot trait and all modules that are necessary to use a robot. -use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::model::Model; -use crate::network::{Network, NetworkType}; -use crate::robot::control_loop::ControlLoop; -use crate::robot::control_types::{ - CartesianPose, CartesianVelocities, ControllerMode, Finishable, JointPositions, - JointVelocities, RealtimeConfig, Torques, -}; -use crate::robot::low_pass_filter::{DEFAULT_CUTOFF_FREQUENCY, MAX_CUTOFF_FREQUENCY}; -use crate::robot::motion_generator_traits::MotionGeneratorTrait; +use crate::exception::create_command_exception; +use crate::robot::private_robot::PrivateRobot; use crate::robot::robot_control::RobotControl; -use crate::robot::robot_impl::RobotImpl; +pub(crate) use crate::robot::robot_data::PrivateRobotData; +use crate::robot::robot_impl::RobotImplementation; use crate::robot::service_types::{ - AutomaticErrorRecoveryRequestWithHeader, AutomaticErrorRecoveryResponse, - AutomaticErrorRecoveryStatus, GetCartesianLimitRequest, GetCartesianLimitRequestWithHeader, - GetCartesianLimitResponse, GetterSetterStatus, RobotCommandEnum, SetCartesianImpedanceRequest, - SetCartesianImpedanceRequestWithHeader, SetCartesianImpedanceResponse, - SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, - SetCollisionBehaviorResponse, SetEeToKRequest, SetEeToKRequestWithHeader, SetEeToKResponse, - SetFiltersRequest, SetFiltersRequestWithHeader, SetFiltersResponse, SetGuidingModeRequest, - SetGuidingModeRequestWithHeader, SetGuidingModeResponse, SetJointImpedanceRequest, - SetJointImpedanceRequestWithHeader, SetJointImpedanceResponse, SetLoadRequest, - SetLoadRequestWithHeader, SetLoadResponse, SetNeToEeRequest, SetNeToEeRequestWithHeader, - SetNeToEeResponse, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus, + SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, + SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, + StopMoveStatusPanda, }; -use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; -use crate::utils::MotionGenerator; -use robot_state::RobotState; +use crate::{ + CartesianPose, CartesianVelocities, ControllerMode, FrankaResult, JointPositions, + JointVelocities, MotionGenerator, RobotModel, Torques, MAX_CUTOFF_FREQUENCY, +}; +use std::time::Duration; mod control_loop; mod control_tools; pub mod control_types; pub mod error; pub mod errors; +mod fr3; pub mod logger; pub mod low_pass_filter; mod motion_generator_traits; -mod rate_limiting; +mod panda; +mod private_robot; +pub mod rate_limiting; mod robot_control; +mod robot_data; mod robot_impl; -pub mod robot_state; +pub(crate) mod robot_state; pub(crate) mod service_types; pub(crate) mod types; pub mod virtual_wall_cuboid; -/// Maintains a network connection to the robot, provides the current robot state, gives access to -/// the model library and allows to control the robot. +pub use fr3::Fr3; +pub use panda::Panda; +pub use robot_data::RobotData; +pub use robot_state::RobotState; + +/// Contains all methods that are available for all robots. /// # Nominal end effector frame NE /// The nominal end effector frame is configured outside of libfranka-rs and cannot be changed here. /// # End effector frame EE /// By default, the end effector frame EE is the same as the nominal end effector frame NE /// (i.e. the transformation between NE and EE is the identity transformation). -/// With [`set_EE`](Self::set_EE), a custom transformation matrix can be set. +/// With [`set_EE`](`Robot::set_EE), a custom transformation matrix can be set. /// # Stiffness frame K /// The stiffness frame is used for Cartesian impedance control, and for measuring and applying /// forces. -/// It can be set with [`set_K`](`Self::set_K`). +/// It can be set with [`set_K`](Robot::set_K). /// /// /// # Motion generation and joint-level torque commands @@ -72,7 +63,7 @@ pub mod virtual_wall_cuboid; /// commands without gravity and friction by providing callback functions. /// /// Only one of these methods can be active at the same time; a -/// [`ControlException`](`crate::exception::FrankaException::ControlException`) is thrown otherwise. +/// [`ControlException`](crate::exception::FrankaException::ControlException) is thrown otherwise. /// /// When a robot state is received, the callback function is used to calculate the response: the /// desired values for that time step. After sending back the response, the callback function will @@ -80,14 +71,14 @@ pub mod virtual_wall_cuboid; /// a 1 kHz frequency, the callback functions have to compute their result in a short time frame /// in order to be accepted. Callback functions take two parameters: /// -/// * A &franka::RobotState showing the current robot state. -/// * A &std::time::Duration to indicate the time since the last callback invocation. Thus, the +/// * A &[RobotState] showing the current robot state. +/// * A &[Duration] to indicate the time since the last callback invocation. Thus, the /// duration is zero on the first invocation of the callback function! /// /// The following incomplete example shows the general structure of a callback function: /// /// ```no_run -/// use franka::robot::robot_state::RobotState; +/// use franka::RobotState; /// use franka::robot::control_types::{JointPositions, MotionFinished}; /// use std::time::Duration; /// # fn your_function_which_generates_joint_positions(time:f64) -> JointPositions {JointPositions::new([0.;7])} @@ -105,61 +96,27 @@ pub mod virtual_wall_cuboid; /// /// Commands are executed by communicating with the robot over the network. /// These functions should therefore not be called from within control or motion generator loops. -pub struct Robot { - robimpl: RobotImpl, -} - -impl Robot { - /// Establishes a connection with the robot. +/// +pub trait Robot { + /// Waits for a robot state update and returns it. /// - /// # Arguments - /// * `franka_address` - IP/hostname of the robot. - /// * `realtime_config` - if set to Enforce, an exception will be thrown if realtime priority - /// cannot be set when required. Setting realtime_config to Ignore disables this behavior. Default is Enforce - /// * `log_size` - sets how many last states should be kept for logging purposes. - /// The log is provided when a [`ControlException`](`crate::exception::FrankaException::ControlException`) is thrown. - /// # Example - /// ```no_run - /// use franka::{FrankaResult, RealtimeConfig, Robot}; - /// fn main() -> FrankaResult<()> { - /// // connects to the robot using real-time scheduling and a default log size of 50. - /// let mut robot = Robot::new("robotik-bs.de", None, None)?; - /// // connects to the robot without using real-time scheduling and a log size of 1. - /// let mut robot = Robot::new("robotik-bs.de", RealtimeConfig::Ignore, 1)?; - /// Ok(()) - /// } - /// ``` - /// # Errors - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is unsuccessful. - /// * IncompatibleVersionException if this version of `libfranka-rs` is not supported. - pub fn new>, LogSize: Into>>( - franka_address: &str, - realtime_config: RtConfig, - log_size: LogSize, - ) -> FrankaResult { - let realtime_config = realtime_config.into().unwrap_or(RealtimeConfig::Enforce); - let log_size = log_size.into().unwrap_or(50); - let network = Network::new( - NetworkType::Robot, - franka_address, - service_types::COMMAND_PORT, - ) - .map_err(|_| FrankaException::NetworkException { - message: "Connection could not be established".to_string(), - })?; - Ok(Robot { - robimpl: RobotImpl::new(network, log_size, realtime_config)?, - }) - } + /// # Return + /// Current robot state. + /// # Error + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// + /// See [`Robot::read`](`Self::read`) for a way to repeatedly receive the robot state. + fn read_once(&mut self) -> FrankaResult; + /// Starts a loop for reading the current robot state. /// /// Cannot be executed while a control or motion generator loop is running. /// /// This minimal example will print the robot state 100 times: /// ```no_run - /// use franka::{Robot, RobotState, FrankaResult}; + /// use franka::{Fr3, RobotState, Robot, FrankaResult}; /// fn main() -> FrankaResult<()> { - /// let mut robot = Robot::new("robotik-bs.de",None,None)?; + /// let mut robot = Fr3::new("robotik-bs.de",None,None)?; /// let mut count = 0; /// robot.read(| robot_state:&RobotState | -> bool { /// println!("{:?}", robot_state); @@ -169,33 +126,360 @@ impl Robot { /// } /// ``` /// # Arguments - /// * `read_callback` - Callback function for robot state reading. The callback hast to return true as long - /// as it wants to receive further robot states. - /// # Error + /// * `read_callback` - Callback function for robot state reading. The callback hast to return true as long + /// as it wants to receive further robot states. + /// # Error + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + fn read bool>(&mut self, read_callback: F) -> FrankaResult<()>; + + /// Executes a joint pose motion to a goal position. Adapted from: + /// Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots + /// (Kogan Page Science Paper edition). + /// # Arguments + /// * `speed_factor` - General speed factor in range [0, 1]. + /// * `q_goal` - Target joint positions. + fn joint_motion(&mut self, speed_factor: f64, q_goal: &[f64; 7]) -> FrankaResult<()>; + + /// Starts a control loop for a joint position motion generator with a given controller mode. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `controller_mode` - Controller to use to execute the motion. Default is joint impedance + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint position commands are NaN or infinity. + fn control_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for a joint velocity motion generator with a given controller mode. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `controller_mode` - Controller to use to execute the motion. Default is joint impedance + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint velocity commands are NaN or infinity. + fn control_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for a Cartesian pose motion generator with a given controller mode. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `controller_mode` - Controller to use to execute the motion. Default is joint impedance + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if Cartesian pose command elements are NaN or infinity. + fn control_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for a Cartesian velocity motion generator with a given controller mode. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `controller_mode` - Controller to use to execute the motion. Default is joint impedance + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if Cartesian velocity command elements are NaN or infinity. + fn control_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + CM: Into>, + L: Into>, + CF: Into>, + >( + &mut self, + motion_generator_callback: F, + controller_mode: CM, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for sending joint-level torque commands. + /// + /// Sets real-time priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `control_callback` - Callback function providing joint-level torque commands. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `limit_rate` - True if rate limiting should be activated. The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`) + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. Set to + /// [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) + /// to disable. Default is 100 Hz + /// + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) + /// if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) + /// if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) + /// if real-time priority cannot be set for the current thread. + /// # Panics + /// * if joint-level torque commands are NaN or infinity. + fn control_torques< + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + control_callback: T, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for sending joint-level torque commands and joint positions. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `control_callback` - Callback function providing joint-level torque commands. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint-level torque or joint position commands are NaN or infinity. + fn control_torques_and_joint_positions< + F: FnMut(&RobotState, &Duration) -> JointPositions, + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for sending joint-level torque commands and joint velocities. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `control_callback` - Callback function providing joint-level torque commands. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint-level torque or joint velocity commands are NaN or infinity. + fn control_torques_and_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for sending joint-level torque commands and Cartesian poses. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `control_callback` - Callback function providing joint-level torque commands. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint-level torque or Cartesian pose command elements are NaN or infinity. + fn control_torques_and_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( + &mut self, + control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Starts a control loop for sending joint-level torque commands and Cartesian velocities. + /// + /// Sets realtime priority for the current thread. + /// Cannot be executed while another control or motion generator loop is active. + /// + /// # Arguments + /// * `control_callback` - Callback function providing joint-level torque commands. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `motion_generator_callback` - Callback function for motion generation. + /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. + /// * `limit_rate` - Set to true to activate the rate limiter. + /// The default value is defined for each robot type over + /// [`RATE_LIMITING_ON_PER_DEFAULT`](`crate::robot::rate_limiting::RateLimiter::RATE_LIMITING_ON_PER_DEFAULT`). + /// Enabling the rate limiter can distort your motion! + /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on + /// the user commanded signal. + /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. + /// Default is 100 Hz + /// # Errors + /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn read bool>( + /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. + /// # Panics + /// * if joint-level torque or Cartesian velocity command elements are NaN or infinity. + fn control_torques_and_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + T: FnMut(&RobotState, &Duration) -> Torques, + L: Into>, + CF: Into>, + >( &mut self, - mut read_callback: F, - ) -> FrankaResult<()> { - loop { - let state = self.robimpl.update(None, None)?; - if !read_callback(&state) { - break; - } - } - Ok(()) - } - /// Waits for a robot state update and returns it. - /// + control_callback: T, + motion_generator_callback: F, + limit_rate: L, + cutoff_frequency: CF, + ) -> FrankaResult<()>; + + /// Loads the model library from the robot. + /// # Arguments + /// * `persistent` - If set to true the model will be stored at `/tmp/model.so` /// # Return - /// Current robot state. - /// # Error + /// Model instance. + /// # Errors + /// * [`ModelException`](`crate::exception::FrankaException::ModelException`) if the model library cannot be loaded. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// - /// See [`Robot::read`](`Self::read`) for a way to repeatedly receive the robot state. - pub fn read_once(&mut self) -> FrankaResult { - self.robimpl.read_once() - } + fn load_model(&mut self, persistent: bool) -> FrankaResult; /// Changes the collision behavior. /// @@ -229,7 +513,7 @@ impl Robot { /// * [`RobotState::joint_collision`](`crate::RobotState::joint_collision`) /// * [`automatic_error_recovery`](`Self::automatic_error_recovery`) for performing a reset after a collision. #[allow(clippy::too_many_arguments)] - pub fn set_collision_behavior( + fn set_collision_behavior( &mut self, lower_torque_thresholds_acceleration: [f64; 7], upper_torque_thresholds_acceleration: [f64; 7], @@ -239,30 +523,13 @@ impl Robot { upper_force_thresholds_acceleration: [f64; 6], lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6], - ) -> FrankaResult<()> { - let command = SetCollisionBehaviorRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetCollisionBehavior, - size_of::(), - ), - request: SetCollisionBehaviorRequest::new( - lower_torque_thresholds_acceleration, - upper_torque_thresholds_acceleration, - lower_torque_thresholds_nominal, - upper_torque_thresholds_nominal, - lower_force_thresholds_acceleration, - upper_force_thresholds_acceleration, - lower_force_thresholds_nominal, - upper_force_thresholds_nominal, - ), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetCollisionBehaviorResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + ) -> FrankaResult<()>; + + /// Sets a default collision behavior, joint impedance and Cartesian impedance. + /// # Errors + /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + fn set_default_behavior(&mut self) -> FrankaResult<()>; /// Sets the impedance for each joint in the internal controller. /// @@ -273,21 +540,7 @@ impl Robot { /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. #[allow(non_snake_case)] - pub fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { - let command = SetJointImpedanceRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetJointImpedance, - size_of::(), - ), - request: SetJointImpedanceRequest::new(K_theta), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetJointImpedanceResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()>; /// Sets the Cartesian impedance for (x, y, z, roll, pitch, yaw) in the internal controller. /// @@ -300,21 +553,30 @@ impl Robot { /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. #[allow(non_snake_case)] - pub fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { - let command = SetCartesianImpedanceRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetCartesianImpedance, - size_of::(), - ), - request: SetCartesianImpedanceRequest::new(K_x), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetCartesianImpedanceResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()>; + + /// Sets dynamic parameters of a payload. + /// + /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. + /// # Note + /// This is not for setting end effector parameters, which have to be set in the administrator's + /// interface. + /// # Arguments + /// * `load_mass` - Mass of the load in \[kg\] + /// * `F_x_Cload` - Translation from flange to center of mass of load + /// ![^Fx_{C_\text{load}}](https://latex.codecogs.com/png.latex?^Fx_{C_\text{load}}) in \[m\] + /// * `load_inertia` - Inertia matrix ![I_\text{load}](https://latex.codecogs.com/png.latex?I_\text{load}) in [kg \times m^2], column-major. + /// # Errors + /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + #[allow(non_snake_case)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()>; + /// Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw). /// /// If a flag is set to true, movement is unlocked. @@ -326,21 +588,7 @@ impl Robot { /// # Errors /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { - let command = SetGuidingModeRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetGuidingMode, - size_of::(), - ), - request: SetGuidingModeRequest::new(guiding_mode, elbow), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetGuidingModeResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()>; /// Sets the transformation ![^{EE}T_K](https://latex.codecogs.com/png.latex?^{EE}T_K) from end effector frame to stiffness frame. /// @@ -353,21 +601,7 @@ impl Robot { /// /// See [Stiffness frame K](#stiffness-frame-k) for an explanation of the stiffness frame. #[allow(non_snake_case)] - pub fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { - let command = SetEeToKRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetEeToK, - size_of::(), - ), - request: SetEeToKRequest::new(EE_T_K), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetEeToKResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()>; /// Sets the transformation ![^{NE}T_{EE}](https://latex.codecogs.com/png.latex?^{NE}T_{EE}) from nominal end effector to end effector frame. /// @@ -384,102 +618,7 @@ impl Robot { /// * [`RobotState::F_T_EE`](`crate::RobotState::F_T_EE`) /// * [NE](#nominal-end-effector-frame-ne) and [EE](#end-effector-frame-ee) for an explanation of those frames #[allow(non_snake_case)] - pub fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { - let command = SetNeToEeRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetNeToEe, - size_of::(), - ), - request: SetNeToEeRequest::new(NE_T_EE), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetNeToEeResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } - /// Sets dynamic parameters of a payload. - /// - /// The transformation matrix is represented as a vectorized 4x4 matrix in column-major format. - /// # Note - /// This is not for setting end effector parameters, which have to be set in the administrator's - /// interface. - /// # Arguments - /// * `load_mass` - Mass of the load in \[kg\] - /// * `F_x_Cload` - Translation from flange to center of mass of load - /// ![^Fx_{C_\text{load}}](https://latex.codecogs.com/png.latex?^Fx_{C_\text{load}}) in \[m\] - /// * `load_inertia` - Inertia matrix ![I_\text{load}](https://latex.codecogs.com/png.latex?I_\text{load}) in [kg \times m^2], column-major. - /// # Errors - /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - #[allow(non_snake_case)] - pub fn set_load( - &mut self, - load_mass: f64, - F_x_Cload: [f64; 3], - load_inertia: [f64; 9], - ) -> FrankaResult<()> { - let command = SetLoadRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetLoad, - size_of::(), - ), - request: SetLoadRequest::new(load_mass, F_x_Cload, load_inertia), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetLoadResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } - /// Sets the cut off frequency for the given motion generator or controller. - /// - /// Allowed input range for all the filters is between 1.0 Hz and 1000.0 Hz. - /// If the value is set to maximum (1000Hz) then no filtering is done. - /// # Arguments - /// * `joint_position_filter_frequency` - Frequency at which the commanded joint - /// position is cut off. - /// * `joint_velocity_filter_frequency` - TFrequency at which the commanded joint - /// velocity is cut off. - /// * `cartesian_position_filter_frequency` - Frequency at which the commanded - /// Cartesian position is cut off. - /// * `cartesian_velocity_filter_frequency` - Frequency at which the commanded - /// Cartesian velocity is cut off. - /// * `controller_filter_frequency` - Frequency at which the commanded torque is cut off - /// # Errors - /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - #[deprecated(note = "please use `low_pass_filter` instead")] - pub fn set_filters( - &mut self, - joint_position_filter_frequency: f64, - joint_velocity_filter_frequency: f64, - cartesian_position_filter_frequency: f64, - cartesian_velocity_filter_frequency: f64, - controller_filter_frequency: f64, - ) -> FrankaResult<()> { - let command = SetFiltersRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::SetFilters, - size_of::(), - ), - request: SetFiltersRequest::new( - joint_position_filter_frequency, - joint_velocity_filter_frequency, - cartesian_position_filter_frequency, - cartesian_velocity_filter_frequency, - controller_filter_frequency, - ), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: SetFiltersResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - handle_getter_setter_status(response.status) - } + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()>; ///Runs automatic error recovery on the robot. /// @@ -487,39 +626,7 @@ impl Robot { /// # Errors /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn automatic_error_recovery(&mut self) -> FrankaResult<()> { - let command = self.robimpl.network.create_header_for_robot( - RobotCommandEnum::AutomaticErrorRecovery, - size_of::(), - ); - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: AutomaticErrorRecoveryResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - match &response.status { - AutomaticErrorRecoveryStatus::Success => Ok(()), - AutomaticErrorRecoveryStatus::EmergencyAborted => Err(create_command_exception( - "libfranka-rs: command aborted: User Stop pressed!", - )), - AutomaticErrorRecoveryStatus::ReflexAborted => Err(create_command_exception( - "libfranka-rs: command aborted: motion aborted by reflex!", - )), - AutomaticErrorRecoveryStatus::CommandNotPossibleRejected => { - Err(create_command_exception( - "libfranka-rs: command rejected: command not possible in current mode", - )) - } - AutomaticErrorRecoveryStatus::ManualErrorRecoveryRequiredRejected => { - Err(create_command_exception( - "libfranka-rs: command rejected: manual error recovery required!", - )) - } - AutomaticErrorRecoveryStatus::Aborted => { - Err(create_command_exception("libfranka-rs: command aborted!")) - } - } - } + fn automatic_error_recovery(&mut self) -> FrankaResult<()>; /// Stops all currently running motions. /// @@ -528,303 +635,171 @@ impl Robot { /// # Errors /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn stop(&mut self) -> FrankaResult<()> { - let command = StopMoveRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::StopMove, - size_of::(), - ), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: StopMoveResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - match response.status { - StopMoveStatus::Success => Ok(()), - StopMoveStatus::CommandNotPossibleRejected | StopMoveStatus::Aborted => { - Err(create_command_exception( - "libfranka-rs: command rejected: command not possible in current mode", - )) - } - StopMoveStatus::EmergencyAborted => Err(create_command_exception( - "libfranka-rs: command aborted: User Stop pressed!", - )), - StopMoveStatus::ReflexAborted => Err(create_command_exception( - "libfranka-rs: command aborted: motion aborted by reflex!", - )), + fn stop(&mut self) -> FrankaResult<()>; + + /// Returns the software version reported by the connected server. + /// + /// # Return + /// Software version of the connected server. + fn server_version(&self) -> u16; +} + +impl Robot for R +where + RobotState: From<::State>, + CartesianPose: crate::ConvertMotion<::State>, + JointVelocities: crate::ConvertMotion<::State>, + JointPositions: crate::ConvertMotion<::State>, + CartesianVelocities: crate::ConvertMotion<::State>, +{ + fn read_once(&mut self) -> FrankaResult { + match ::get_rob_mut(self).read_once() { + Ok(state) => Ok(state.into()), + Err(e) => Err(e), } } - /// Returns the parameters of a virtual wall. - /// # Arguments - /// * `id` - ID of the virtual wall. - /// # Return - /// Parameters of virtual wall. - /// # Errors - /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn get_virtual_wall(&mut self, id: i32) -> FrankaResult { - let command = GetCartesianLimitRequestWithHeader { - header: self.robimpl.network.create_header_for_robot( - RobotCommandEnum::GetCartesianLimit, - size_of::(), - ), - request: GetCartesianLimitRequest::new(id), - }; - let command_id: u32 = self.robimpl.network.tcp_send_request(command); - let response: GetCartesianLimitResponse = self - .robimpl - .network - .tcp_blocking_receive_response(command_id); - match &response.status { - GetterSetterStatus::Success => Ok(VirtualWallCuboid::new(id, response)), - GetterSetterStatus::CommandNotPossibleRejected => Err(create_command_exception( - "libfranka-rs: command rejected: command not possible in current mode", - )), - GetterSetterStatus::InvalidArgumentRejected => Err(create_command_exception( - "libfranka-rs: command rejected: invalid argument!", - )), + + fn read bool>(&mut self, mut read_callback: F) -> FrankaResult<()> { + loop { + let state = ::get_rob_mut(self).update(None, None)?; + if !read_callback(&state.into()) { + break; + } } + Ok(()) } - /// Starts a control loop for a joint position motion generator with a given controller mode. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `controller_mode` Controller to use to execute the motion. Default is joint impedance - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint position commands are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_joint_positions< + + fn joint_motion(&mut self, speed_factor: f64, q_goal: &[f64; 7]) -> FrankaResult<()> { + let mut motion_generator = MotionGenerator::new(speed_factor, q_goal); + self.control_joint_positions( + |state, time| motion_generator.generate_motion(state, time), + Some(ControllerMode::JointImpedance), + Some(true), + Some(MAX_CUTOFF_FREQUENCY), + ) + } + + fn control_joint_positions< F: FnMut(&RobotState, &Duration) -> JointPositions, CM: Into>, L: Into>, CF: Into>, >( &mut self, - motion_generator_callback: F, + mut motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, + let cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, controller_mode.into(), limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for a joint velocity motion generator with a given controller mode. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `controller_mode` Controller to use to execute the motion. Default is joint impedance - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint velocity commands are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_joint_velocities< + + fn control_joint_velocities< F: FnMut(&RobotState, &Duration) -> JointVelocities, CM: Into>, L: Into>, CF: Into>, >( &mut self, - motion_generator_callback: F, + mut motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, + let cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) // todo make this without cloning + }; + ::control_motion_intern( + self, + cb, controller_mode.into(), limit_rate.into(), cutoff_frequency.into(), - ) - } - /// Starts a control loop for a Cartesian pose motion generator with a given controller mode. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `controller_mode` Controller to use to execute the motion. Default is joint impedance - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if Cartesian pose command elements are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_cartesian_pose< + ) + } + + fn control_cartesian_pose< F: FnMut(&RobotState, &Duration) -> CartesianPose, CM: Into>, L: Into>, CF: Into>, >( &mut self, - motion_generator_callback: F, + mut motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - self.control_motion_intern( - motion_generator_callback, + let cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + ::control_motion_intern( + self, + cb, controller_mode.into(), limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for a Cartesian velocity motion generator with a given controller mode. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `controller_mode` Controller to use to execute the motion. Default is joint impedance - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if Cartesian velocity command elements are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_cartesian_velocities< + + fn control_cartesian_velocities< F: FnMut(&RobotState, &Duration) -> CartesianVelocities, CM: Into>, L: Into>, CF: Into>, >( &mut self, - motion_generator_callback: F, + mut motion_generator_callback: F, controller_mode: CM, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { + let cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; self.control_motion_intern( - motion_generator_callback, + cb, controller_mode.into(), limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for sending joint-level torque commands and joint velocities. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint-level torque or joint velocity commands are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_torques_and_joint_velocities< - F: FnMut(&RobotState, &Duration) -> JointVelocities, + + fn control_torques< T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, mut control_callback: T, - motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { + let motion_generator_callback = + |_state: &::State, _time_step: &Duration| { + JointVelocities::new([0.; 7]) + }; + let mut cb = |state: &::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; self.control_torques_intern( - motion_generator_callback, - &mut control_callback, + &motion_generator_callback, + &mut cb, limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for sending joint-level torque commands and joint positions. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint-level torque or joint position commands are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_torques_and_joint_positions< + + fn control_torques_and_joint_positions< F: FnMut(&RobotState, &Duration) -> JointPositions, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, @@ -832,198 +807,139 @@ impl Robot { >( &mut self, mut control_callback: T, - motion_generator_callback: F, + mut motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { + let motion_generator_cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; self.control_torques_intern( - motion_generator_callback, - &mut control_callback, + motion_generator_cb, + &mut control_cb, limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for sending joint-level torque commands and Cartesian poses. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint-level torque or Cartesian pose command elements are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_torques_and_cartesian_pose< - F: FnMut(&RobotState, &Duration) -> CartesianPose, + fn control_torques_and_joint_velocities< + F: FnMut(&RobotState, &Duration) -> JointVelocities, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, mut control_callback: T, - motion_generator_callback: F, + mut motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { + let motion_generator_cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; self.control_torques_intern( - motion_generator_callback, - &mut control_callback, + motion_generator_cb, + &mut control_cb, limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for sending joint-level torque commands and Cartesian velocities. - /// - /// Sets realtime priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `control_callback` Callback function providing joint-level torque commands. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `motion_generator_callback` Callback function for motion generation. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `limit_rate` True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. - /// Set to [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) to disable. - /// Default is 100 Hz - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) if realtime priority cannot be set for the current thread. - /// # Panics - /// * if joint-level torque or Cartesian velocity command elements are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if realtime priority cannot be set. - pub fn control_torques_and_cartesian_velocities< - F: FnMut(&RobotState, &Duration) -> CartesianVelocities, + fn control_torques_and_cartesian_pose< + F: FnMut(&RobotState, &Duration) -> CartesianPose, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, mut control_callback: T, - motion_generator_callback: F, + mut motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { + let motion_generator_cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; self.control_torques_intern( - motion_generator_callback, - &mut control_callback, + motion_generator_cb, + &mut control_cb, limit_rate.into(), cutoff_frequency.into(), ) } - /// Starts a control loop for sending joint-level torque commands. - /// - /// Sets real-time priority for the current thread. - /// Cannot be executed while another control or motion generator loop is active. - /// - /// # Arguments - /// * `control_callback` - Callback function providing joint-level torque commands. - /// See [here](#motion-generation-and-joint-level-torque-commands) for more details. - /// * `limit_rate` - True if rate limiting should be activated. True by default. - /// This could distort your motion! - /// * `cutoff_frequency` - Cutoff frequency for a first order low-pass filter applied on - /// the user commanded signal. Set to - /// [`MAX_CUTOFF_FREQUENCY`](`crate::robot::low_pass_filter::MAX_CUTOFF_FREQUENCY`) - /// to disable. Default is 100 Hz - /// - /// # Errors - /// * [`ControlException`](`crate::exception::FrankaException::ControlException`) - /// if an error related to torque control or motion generation occurred. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) - /// if the connection is lost, e.g. after a timeout. - /// * [`RealTimeException`](`crate::exception::FrankaException::RealTimeException`) - /// if real-time priority cannot be set for the current thread. - /// # Panics - /// * if joint-level torque commands are NaN or infinity. - /// - /// See [`new`](`Self::new`) to change behavior if real-time priority cannot be set. - pub fn control_torques< + + fn control_torques_and_cartesian_velocities< + F: FnMut(&RobotState, &Duration) -> CartesianVelocities, T: FnMut(&RobotState, &Duration) -> Torques, L: Into>, CF: Into>, >( &mut self, mut control_callback: T, + mut motion_generator_callback: F, limit_rate: L, cutoff_frequency: CF, ) -> FrankaResult<()> { - let motion_generator_callback = - |_state: &RobotState, _time_step: &Duration| JointVelocities::new([0.; 7]); + let motion_generator_cb = |state: &::State, duration: &Duration| { + motion_generator_callback(&(state.clone().into()), duration) + }; + let mut control_cb = |state: &::State, duration: &Duration| { + control_callback(&(state.clone().into()), duration) + }; self.control_torques_intern( - &motion_generator_callback, - &mut control_callback, + motion_generator_cb, + &mut control_cb, limit_rate.into(), cutoff_frequency.into(), ) } - fn control_torques_intern< - F: FnMut(&RobotState, &Duration) -> U, - U: Finishable + Debug + MotionGeneratorTrait, - >( - &mut self, - motion_generator_callback: F, - control_callback: &mut dyn FnMut(&RobotState, &Duration) -> Torques, - limit_rate: Option, - cutoff_frequency: Option, - ) -> FrankaResult<()> { - let limit_rate = limit_rate.unwrap_or(true); - let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); - let mut control_loop = ControlLoop::new( - &mut self.robimpl, - control_callback, - motion_generator_callback, - limit_rate, - cutoff_frequency, - )?; - control_loop.run() + + fn load_model(&mut self, persistent: bool) -> FrankaResult { + ::get_rob_mut(self) + .load_model(persistent) + .map(|model| model.into()) } - fn control_motion_intern< - F: FnMut(&RobotState, &Duration) -> U, - U: Finishable + Debug + MotionGeneratorTrait, - >( + + #[allow(clippy::too_many_arguments)] + fn set_collision_behavior( &mut self, - motion_generator_callback: F, - controller_mode: Option, - limit_rate: Option, - cutoff_frequency: Option, + lower_torque_thresholds_acceleration: [f64; 7], + upper_torque_thresholds_acceleration: [f64; 7], + lower_torque_thresholds_nominal: [f64; 7], + upper_torque_thresholds_nominal: [f64; 7], + lower_force_thresholds_acceleration: [f64; 6], + upper_force_thresholds_acceleration: [f64; 6], + lower_force_thresholds_nominal: [f64; 6], + upper_force_thresholds_nominal: [f64; 6], ) -> FrankaResult<()> { - let controller_mode = controller_mode.unwrap_or(ControllerMode::JointImpedance); - let limit_rate = limit_rate.unwrap_or(true); - let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); - let mut control_loop = ControlLoop::from_control_mode( - &mut self.robimpl, - controller_mode, - motion_generator_callback, - limit_rate, - cutoff_frequency, - )?; - control_loop.run() + let command = ::create_set_collision_behavior_request( + &mut ::get_net(self).command_id, + SetCollisionBehaviorRequest::new( + lower_torque_thresholds_acceleration, + upper_torque_thresholds_acceleration, + lower_torque_thresholds_nominal, + upper_torque_thresholds_nominal, + lower_force_thresholds_acceleration, + upper_force_thresholds_acceleration, + lower_force_thresholds_nominal, + upper_force_thresholds_nominal, + ), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) } - /// Sets a default collision behavior, joint impedance and Cartesian impedance. - /// # Errors - /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn set_default_behavior(&mut self) -> FrankaResult<()> { + + fn set_default_behavior(&mut self) -> FrankaResult<()> { self.set_collision_behavior( [20.; 7], [20.; 7], [10.; 7], [10.; 7], [20.; 6], [20.; 6], [10.; 6], [10.; 6], )?; @@ -1031,52 +947,114 @@ impl Robot { self.set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])?; Ok(()) } - /// Executes a joint pose motion to a goal position. Adapted from: - /// Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots - /// (Kogan Page Science Paper edition). - /// # Arguments - /// * `speed_factor` - General speed factor in range [0, 1]. - /// * `q_goal` - Target joint positions. - pub fn joint_motion(&mut self, speed_factor: f64, q_goal: &[f64; 7]) -> FrankaResult<()> { - let mut motion_generator = MotionGenerator::new(speed_factor, q_goal); - self.control_joint_positions( - |state, time| motion_generator.generate_motion(state, time), - Some(ControllerMode::JointImpedance), - Some(true), - Some(MAX_CUTOFF_FREQUENCY), - ) + + #[allow(non_snake_case)] + fn set_joint_impedance(&mut self, K_theta: [f64; 7]) -> FrankaResult<()> { + let command = ::create_set_joint_impedance_request( + &mut ::get_net(self).command_id, + SetJointImpedanceRequest::new(K_theta), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) } - /// Loads the model library from the robot. - /// # Arguments - /// * `persistent` - If set to true the model will be stored at `/tmp/model.so` - /// # Return - /// Model instance. - /// # Errors - /// * [`ModelException`](`crate::exception::FrankaException::ModelException`) if the model library cannot be loaded. - /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. - pub fn load_model(&mut self, persistent: bool) -> FrankaResult { - self.robimpl.load_model(persistent) + + #[allow(non_snake_case)] + fn set_cartesian_impedance(&mut self, K_x: [f64; 6]) -> FrankaResult<()> { + let command = ::create_set_cartesian_impedance_request( + &mut ::get_net(self).command_id, + SetCartesianImpedanceRequest::new(K_x), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) } - /// Returns the software version reported by the connected server. - /// - /// # Return - /// Software version of the connected server. - pub fn server_version(&self) -> u16 { - self.robimpl.server_version() + + #[allow(non_snake_case)] + fn set_load( + &mut self, + load_mass: f64, + F_x_Cload: [f64; 3], + load_inertia: [f64; 9], + ) -> FrankaResult<()> { + let command = ::create_set_load_request( + &mut ::get_net(self).command_id, + SetLoadRequest::new(load_mass, F_x_Cload, load_inertia), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) + } + + fn set_guiding_mode(&mut self, guiding_mode: [bool; 6], elbow: bool) -> FrankaResult<()> { + let command = ::create_set_guiding_mode_request( + &mut ::get_net(self).command_id, + SetGuidingModeRequest::new(guiding_mode, elbow), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_K(&mut self, EE_T_K: [f64; 16]) -> FrankaResult<()> { + let command = ::create_set_ee_to_k_request( + &mut ::get_net(self).command_id, + SetEeToKRequest::new(EE_T_K), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) + } + + #[allow(non_snake_case)] + fn set_EE(&mut self, NE_T_EE: [f64; 16]) -> FrankaResult<()> { + let command = ::create_set_ne_to_ee_request( + &mut ::get_net(self).command_id, + SetNeToEeRequest::new(NE_T_EE), + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_getter_setter_status(status) + } + + fn automatic_error_recovery(&mut self) -> FrankaResult<()> { + let command = ::create_automatic_error_recovery_request( + &mut ::get_net(self).command_id, + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status = ::get_net(self).tcp_blocking_receive_status(command_id); + ::handle_automatic_error_recovery_status(status) + } + + fn stop(&mut self) -> FrankaResult<()> { + let command = ::create_stop_request( + &mut ::get_net(self).command_id, + ); + let command_id: u32 = ::get_net(self).tcp_send_request(command); + let status: StopMoveStatusPanda = + ::get_net(self).tcp_blocking_receive_status(command_id); + match status { + StopMoveStatusPanda::Success => Ok(()), + StopMoveStatusPanda::CommandNotPossibleRejected | StopMoveStatusPanda::Aborted => { + Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )) + } + StopMoveStatusPanda::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: command aborted: User Stop pressed!", + )), + StopMoveStatusPanda::ReflexAborted => Err(create_command_exception( + "libfranka-rs: command aborted: motion aborted by reflex!", + )), + } } -} -fn handle_getter_setter_status(status: GetterSetterStatus) -> FrankaResult<()> { - match status { - GetterSetterStatus::Success => Ok(()), - GetterSetterStatus::CommandNotPossibleRejected => Err(create_command_exception( - "libfranka-rs: command rejected: command not possible in current mode", - )), - GetterSetterStatus::InvalidArgumentRejected => Err(create_command_exception( - "libfranka-rs: command rejected: invalid argument!", - )), + fn server_version(&self) -> u16 { + ::get_rob(self).server_version() } } + #[cfg(test)] mod tests { use mockall::{automock, predicate::*}; @@ -1089,14 +1067,16 @@ mod tests { use crate::exception::FrankaException; use crate::network::MessageCommand; use crate::robot::service_types::{ - ConnectRequestWithHeader, ConnectResponse, ConnectStatus, GetterSetterStatus, - MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode, MoveRequest, - MoveRequestWithHeader, MoveResponse, MoveStatus, RobotCommandEnum, RobotCommandHeader, - SetCollisionBehaviorRequest, SetCollisionBehaviorRequestWithHeader, - SetCollisionBehaviorResponse, COMMAND_PORT, VERSION, + ConnectRequestWithPandaHeader, ConnectResponsePanda, ConnectStatus, Fr3CommandEnum, + Fr3CommandHeader, GetterSetterStatusPanda, MoveControllerMode, MoveDeviation, + MoveMotionGeneratorMode, MoveRequest, MoveRequestWithPandaHeader, MoveStatusPanda, + PandaCommandEnum, PandaCommandHeader, SetCollisionBehaviorRequest, + SetCollisionBehaviorRequestWithFr3Header, SetCollisionBehaviorRequestWithPandaHeader, + SetterResponseFr3, COMMAND_PORT, FR3_VERSION, }; - use crate::robot::types::RobotStateIntern; - use crate::{FrankaResult, JointPositions, MotionFinished, RealtimeConfig, Robot, RobotState}; + use crate::robot::types::PandaStateIntern; + use crate::{Fr3, Robot}; + use crate::{FrankaResult, JointPositions, MotionFinished, Panda, RealtimeConfig, RobotState}; use bincode::{deserialize, serialize, serialized_size}; use std::iter::FromIterator; use std::mem::size_of; @@ -1151,7 +1131,7 @@ mod tests { }, receive_bytes: |bytes| { let mut soc = tcp_socket.lock().unwrap(); - let mut buffer = vec![0 as u8; 3000]; + let mut buffer = vec![0_u8; 3000]; let num_bytes = soc.read(&mut buffer).unwrap(); buffer.resize(num_bytes, 0); assert_eq!(buffer.len(), num_bytes); @@ -1196,7 +1176,7 @@ mod tests { let mut counter = 1; let start = Instant::now(); while (Instant::now() - start).as_secs_f64() < 0.1 { - let mut state = RobotStateIntern::dummy(); + let mut state = PandaStateIntern::dummy(); state.message_id = counter; let bytes = serialize(&state).unwrap(); counter += 1; @@ -1221,7 +1201,7 @@ mod tests { F: Fn(&Vec), G: Fn(&mut Vec), { - let mut bytes = vec![0 as u8; 100]; + let mut bytes = vec![0_u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); let response = reaction.process_received_bytes(&mut bytes); (tcp_socket.send_bytes)(&response); @@ -1229,20 +1209,20 @@ mod tests { fn receive_robot_connect_request), G: Fn(&mut Vec)>( &self, tcp_socket: &mut Socket, - ) -> ConnectRequestWithHeader { - let mut bytes = vec![0 as u8; 100]; + ) -> ConnectRequestWithPandaHeader { + let mut bytes = vec![0_u8; 100]; (tcp_socket.receive_bytes)(&mut bytes); - let request: ConnectRequestWithHeader = deserialize(bytes.as_slice()).unwrap(); - return request; + let request: ConnectRequestWithPandaHeader = deserialize(bytes.as_slice()).unwrap(); + request } fn send_robot_connect_response), G: Fn(&mut Vec)>( &self, - request: ConnectRequestWithHeader, + request: ConnectRequestWithPandaHeader, tcp_socket: &mut Socket, ) { - let mut response = ConnectResponse { - header: RobotCommandHeader { - command: RobotCommandEnum::Connect, + let mut response = ConnectResponsePanda { + header: PandaCommandHeader { + command: PandaCommandEnum::Connect, command_id: request.get_command_message_id(), size: 0, }, @@ -1282,11 +1262,11 @@ mod tests { lower_force_thresholds_nominal: [f64; 6], upper_force_thresholds_nominal: [f64; 6]| { counter += 1; - SetCollisionBehaviorRequestWithHeader { - header: RobotCommandHeader::new( - RobotCommandEnum::SetCollisionBehavior, + SetCollisionBehaviorRequestWithFr3Header { + header: Fr3CommandHeader::new( + Fr3CommandEnum::SetCollisionBehavior, counter, - size_of::() as u32, + size_of::() as u32, ), request: SetCollisionBehaviorRequest::new( lower_torque_thresholds_acceleration, @@ -1307,9 +1287,9 @@ mod tests { generate_collision_behavior_request(*a, *b, *c, *d, *e, *f, *g, *h) }), )); - let requests_server = requests.clone(); + let requests_server = requests; let thread = std::thread::spawn(|| { - let mut robot_server = RobotMockServer::new(VERSION); + let mut robot_server = RobotMockServer::new(FR3_VERSION); let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -1322,15 +1302,16 @@ mod tests { .iter() .zip(serialized_expected_request.iter()) .for_each(|(x, y)| assert_eq!(x, y)); - let req: SetCollisionBehaviorRequestWithHeader = deserialize(&bytes).unwrap(); + let req: SetCollisionBehaviorRequestWithPandaHeader = + deserialize(bytes).unwrap(); counter += 1; - let mut response = SetCollisionBehaviorResponse { - header: RobotCommandHeader::new( - RobotCommandEnum::SetCollisionBehavior, + let mut response = SetterResponseFr3 { + header: Fr3CommandHeader::new( + Fr3CommandEnum::SetCollisionBehavior, req.header.command_id, 0, ), - status: GetterSetterStatus::Success, + status: GetterSetterStatusPanda::Success, }; response.header.size = serialized_size(&response).unwrap() as u32; serialize(&response).unwrap() @@ -1341,8 +1322,8 @@ mod tests { }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Robot::new("127.0.0.1", None, None).expect("robot failure"); - assert_eq!(robot.server_version(), VERSION); + let mut robot = Fr3::new("127.0.0.1", None, None).expect("robot failure"); + assert_eq!(robot.server_version(), FR3_VERSION); for (a, b, c, d, e, f, g, h) in collision_behavior_request_values.iter() { robot .set_collision_behavior(*a, *b, *c, *d, *e, *f, *g, *h) @@ -1356,11 +1337,11 @@ mod tests { #[test] fn fail_start_motion_test() { - let requests = Arc::new(vec![MoveRequestWithHeader { - header: RobotCommandHeader::new( - RobotCommandEnum::Move, + let requests = Arc::new(vec![MoveRequestWithPandaHeader { + header: PandaCommandHeader::new( + PandaCommandEnum::Move, 1, - size_of::() as u32, + size_of::() as u32, ), request: MoveRequest::new( MoveControllerMode::JointImpedance, @@ -1377,9 +1358,9 @@ mod tests { }, ), }]); - let requests_server = requests.clone(); + let requests_server = requests; let thread = std::thread::spawn(|| { - let mut robot_server = RobotMockServer::new(VERSION); + let mut robot_server = RobotMockServer::new(FR3_VERSION); let mut mock = MockServerReaction::default(); let num_requests = requests_server.len(); let mut counter = 0; @@ -1387,22 +1368,19 @@ mod tests { .returning(move |bytes: &mut Vec| -> Vec { let expected_request = requests_server.get(counter).unwrap(); let serialized_expected_request = serialize(expected_request).unwrap(); - let req: MoveRequestWithHeader = deserialize(&bytes).unwrap(); + let req: MoveRequestWithPandaHeader = deserialize(bytes).unwrap(); assert_eq!(bytes.len(), serialized_expected_request.len()); bytes .iter() .zip(serialized_expected_request.iter()) .for_each(|(x, y)| assert_eq!(x, y)); counter += 1; - let mut response = MoveResponse { - header: RobotCommandHeader::new( - RobotCommandEnum::Move, - req.header.command_id, - 0, - ), - status: MoveStatus::Aborted, - }; - response.header.size = serialized_size(&response).unwrap() as u32; + + let mut response = ( + PandaCommandHeader::new(PandaCommandEnum::Move, req.header.command_id, 0), + MoveStatusPanda::Aborted, + ); + response.0.size = serialized_size(&response).unwrap() as u32; serialize(&response).unwrap() }) .times(num_requests); @@ -1412,7 +1390,7 @@ mod tests { { std::thread::sleep(Duration::from_secs_f64(0.01)); let mut robot = - Robot::new("127.0.0.1", RealtimeConfig::Ignore, None).expect("robot failure"); + Fr3::new("127.0.0.1", RealtimeConfig::Ignore, None).expect("robot failure"); let mut counter = 0; let result = robot.control_joint_positions( |_, _| { @@ -1440,15 +1418,15 @@ mod tests { #[test] fn incompatible_library() { let thread = std::thread::spawn(|| { - let mut robot_server = RobotMockServer::new(VERSION + 1); + let mut robot_server = RobotMockServer::new(FR3_VERSION + 1); let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); - mock.expect_number_of_reactions().return_const(0 as usize); + mock.expect_number_of_reactions().return_const(0_usize); robot_server.server_thread(&mut mock); }); std::thread::sleep(Duration::from_secs_f64(0.01)); - let robot_result = Robot::new("127.0.0.1", None, None); + let robot_result = Panda::new("127.0.0.1", None, None); thread.join().unwrap(); match robot_result { @@ -1467,17 +1445,17 @@ mod tests { #[test] fn robot_read_once() -> FrankaResult<()> { let thread = std::thread::spawn(|| { - let mut robot_server = RobotMockServer::new(VERSION); + let mut robot_server = RobotMockServer::new(FR3_VERSION); let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); - mock.expect_number_of_reactions().return_const(0 as usize); + mock.expect_number_of_reactions().return_const(0_usize); robot_server.server_thread(&mut mock); }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Robot::new("127.0.0.1", None, None)?; + let mut robot = Fr3::new("127.0.0.1", None, None)?; let _state = robot.read_once().unwrap(); } thread.join().unwrap(); @@ -1487,16 +1465,16 @@ mod tests { #[test] fn robot_read() -> FrankaResult<()> { let thread = std::thread::spawn(|| { - let mut robot_server = RobotMockServer::new(VERSION); + let mut robot_server = RobotMockServer::new(FR3_VERSION); let mut mock = MockServerReaction::default(); mock.expect_process_received_bytes() .returning(|_bytes| Vec::::new()); - mock.expect_number_of_reactions().return_const(0 as usize); + mock.expect_number_of_reactions().return_const(0_usize); robot_server.server_thread(&mut mock); }); { std::thread::sleep(Duration::from_secs_f64(0.01)); - let mut robot = Robot::new("127.0.0.1", None, None)?; + let mut robot = Fr3::new("127.0.0.1", None, None)?; let mut counter = 0; let mut first_time = true; let mut start_counter = 0; diff --git a/src/robot/control_loop.rs b/src/robot/control_loop.rs index b5ded97..ac493fe 100644 --- a/src/robot/control_loop.rs +++ b/src/robot/control_loop.rs @@ -1,5 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + use std::fmt::Debug; use std::time::Duration; @@ -7,27 +8,30 @@ use crate::exception::{FrankaException, FrankaResult}; use crate::robot::control_tools::{ has_realtime_kernel, set_current_thread_to_highest_scheduler_priority, }; -use crate::robot::control_types::{ControllerMode, Finishable, RealtimeConfig, Torques}; +use crate::robot::control_types::{ControllerMode, ConvertMotion, RealtimeConfig, Torques}; use crate::robot::low_pass_filter::{low_pass_filter, MAX_CUTOFF_FREQUENCY}; use crate::robot::motion_generator_traits::MotionGeneratorTrait; -use crate::robot::rate_limiting::{limit_rate_torques, DELTA_T, MAX_TORQUE_RATE}; -use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::RobotState; +use crate::robot::rate_limiting::{limit_rate_torques, RateLimiter, DELTA_T}; +use crate::robot::robot_data::RobotData; +use crate::robot::robot_impl::RobotImplementation; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; +use crate::Finishable; -type ControlCallback<'b> = &'b mut dyn FnMut(&RobotState, &Duration) -> Torques; -pub struct ControlLoop< +type ControlCallback<'b, State> = &'b mut dyn FnMut(&State, &Duration) -> Torques; +pub(crate) struct ControlLoop< 'a, 'b, - T: RobotControl, - U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&RobotState, &Duration) -> U, + Data: RobotData, + T: RobotImplementation, + U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, + F: FnMut(&Data::State, &Duration) -> U, > { pub default_deviation: MoveDeviation, robot: &'a mut T, motion_callback: F, - control_callback: Option>, + control_callback: Option>, limit_rate: bool, cutoff_frequency: f64, pub motion_id: u32, @@ -36,14 +40,15 @@ pub struct ControlLoop< impl< 'a, 'b, - T: RobotControl, - U: Finishable + Debug + MotionGeneratorTrait, - F: FnMut(&RobotState, &Duration) -> U, - > ControlLoop<'a, 'b, T, U, F> + Data: RobotData, + T: RobotImplementation, + U: ConvertMotion + Debug + MotionGeneratorTrait + Finishable, + F: FnMut(&Data::State, &Duration) -> U, + > ControlLoop<'a, 'b, Data, T, U, F> { pub fn new( robot: &'a mut T, - control_callback: ControlCallback<'b>, + control_callback: ControlCallback<'b, Data::State>, motion_callback: F, limit_rate: bool, cutoff_frequency: f64, @@ -87,7 +92,7 @@ impl< fn new_intern( robot: &'a mut T, motion_callback: F, - control_callback: Option>, + control_callback: Option>, limit_rate: bool, cutoff_frequency: f64, ) -> FrankaResult { @@ -132,21 +137,21 @@ impl< let mut robot_state = self.robot.update(None, None)?; self.robot .throw_on_motion_error(&robot_state, self.motion_id)?; - let mut previous_time = robot_state.time; + let mut previous_time = robot_state.get_time(); let mut motion_command = MotionGeneratorCommand::new([0.; 7], [0.; 7], [0.; 16], [0.; 6], [0.; 2]); if self.control_callback.is_some() { let mut control_command = ControllerCommand { tau_J_d: [0.; 7] }; while self.spin_motion( &robot_state, - &(robot_state.time - previous_time), + &(robot_state.get_time() - previous_time), &mut motion_command, ) && self.spin_control( &robot_state, - &(robot_state.time - previous_time), + &(robot_state.get_time() - previous_time), &mut control_command, ) { - previous_time = robot_state.time; + previous_time = robot_state.get_time(); robot_state = self .robot .update(Some(&motion_command), Some(&control_command))?; @@ -161,10 +166,10 @@ impl< } else { while self.spin_motion( &robot_state, - &(robot_state.time - previous_time), + &(robot_state.get_time() - previous_time), &mut motion_command, ) { - previous_time = robot_state.time; + previous_time = robot_state.get_time(); robot_state = self.robot.update(Some(&motion_command), None)?; self.robot .throw_on_motion_error(&robot_state, self.motion_id)?; @@ -177,7 +182,7 @@ impl< fn spin_control( &mut self, - robot_state: &RobotState, + robot_state: &Data::State, time_step: &Duration, command: &mut ControllerCommand, ) -> bool { @@ -188,16 +193,16 @@ impl< control_output.tau_J[i] = low_pass_filter( DELTA_T, control_output.tau_J[i], - robot_state.tau_J_d[i], + robot_state.get_tau_J_d()[i], self.cutoff_frequency, ); } } if self.limit_rate { control_output.tau_J = limit_rate_torques( - &MAX_TORQUE_RATE, + &::MAX_TORQUE_RATE, &control_output.tau_J, - &robot_state.tau_J_d, + &robot_state.get_tau_J_d(), ); } command.tau_J_d = control_output.tau_J; @@ -206,12 +211,17 @@ impl< } fn spin_motion( &mut self, - robot_state: &RobotState, + robot_state: &T::State, time_step: &Duration, command: &mut MotionGeneratorCommand, ) -> bool { let motion_output = (self.motion_callback)(robot_state, time_step); - motion_output.convert_motion(robot_state, command, self.cutoff_frequency, self.limit_rate); + motion_output.convert_motion::( + robot_state, + command, + self.cutoff_frequency, + self.limit_rate, + ); !motion_output.is_finished() } } diff --git a/src/robot/control_types.rs b/src/robot/control_types.rs index 634be30..6ac5ca5 100644 --- a/src/robot/control_types.rs +++ b/src/robot/control_types.rs @@ -13,12 +13,9 @@ use crate::robot::low_pass_filter::{ use crate::robot::motion_generator_traits::MotionGeneratorTrait; use crate::robot::rate_limiting::{ limit_rate_cartesian_pose, limit_rate_cartesian_velocity, limit_rate_joint_positions, - limit_rate_joint_velocities, limit_rate_position, DELTA_T, MAX_ELBOW_ACCELERATION, - MAX_ELBOW_JERK, MAX_ELBOW_VELOCITY, MAX_JOINT_ACCELERATION, MAX_JOINT_JERK, MAX_JOINT_VELOCITY, - MAX_ROTATIONAL_ACCELERATION, MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, - MAX_TRANSLATIONAL_ACCELERATION, MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, + limit_rate_joint_velocities, limit_rate_position, RateLimiter, DELTA_T, }; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::{AbstractRobotState, RobotState}; use crate::robot::service_types::MoveMotionGeneratorMode; use crate::robot::types::MotionGeneratorCommand; use crate::utils::Vector7; @@ -31,13 +28,26 @@ pub enum ControllerMode { } /// Used to decide whether to enforce realtime mode for a control loop thread. -/// see [`Robot`](`crate::Robot`) +/// See constructors of [Fr3](crate::Fr3::new) and [Panda](crate::Panda::new). #[derive(Copy, Clone, PartialEq)] pub enum RealtimeConfig { + /// Use real-time scheduling and priority Enforce, + /// Dont change scheduling and priority Ignore, } +pub(crate) trait ConvertMotion { + /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering + fn convert_motion( + &self, + robot_state: &State, + command: &mut MotionGeneratorCommand, + cutoff_frequency: f64, + limit_rate: bool, + ); +} + /// Helper type for control and motion generation loops. /// /// Used to determine whether to terminate a loop after the control callback has returned. @@ -46,14 +56,6 @@ pub trait Finishable { fn is_finished(&self) -> bool; /// Sets the attribute which decide if the currently running motion should be finished fn set_motion_finished(&mut self, finished: bool); - /// converts the motion type to a MotionGeneratorCommand and applies rate limiting and filtering - fn convert_motion( - &self, - robot_state: &RobotState, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ); } /// A trait for a Finshable control type to finish the motion @@ -62,7 +64,7 @@ pub trait MotionFinished { fn motion_finished(self) -> Self; } -impl MotionFinished for T { +impl MotionFinished for T { fn motion_finished(mut self) -> Self { self.set_motion_finished(true); self @@ -103,17 +105,6 @@ impl Finishable for Torques { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - #[allow(unused_variables)] - //todo pull convert motion out of the Finishable trait - fn convert_motion( - &self, - robot_state: &RobotState, - command: &mut MotionGeneratorCommand, - cutoff_frequency: f64, - limit_rate: bool, - ) { - unimplemented!() - } } /// Stores values for joint position motion generation. @@ -142,7 +133,6 @@ impl JointPositions { } } } - impl Finishable for JointPositions { fn is_finished(&self) -> bool { self.motion_finished @@ -150,8 +140,9 @@ impl Finishable for JointPositions { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - - fn convert_motion( +} +impl ConvertMotion for JointPositions { + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -171,9 +162,10 @@ impl Finishable for JointPositions { } if limit_rate { command.q_c = limit_rate_joint_positions( - &MAX_JOINT_VELOCITY, - &MAX_JOINT_ACCELERATION, - &MAX_JOINT_JERK, + &Params::compute_upper_limits_joint_velocity(&robot_state.q_d), + &Params::compute_lower_limits_joint_velocity(&robot_state.q_d), + &Params::MAX_JOINT_ACCELERATION, + &Params::MAX_JOINT_JERK, &command.q_c, &robot_state.q_d, &robot_state.dq_d, @@ -216,7 +208,6 @@ impl JointVelocities { } } } - impl Finishable for JointVelocities { fn is_finished(&self) -> bool { self.motion_finished @@ -224,8 +215,9 @@ impl Finishable for JointVelocities { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - - fn convert_motion( +} +impl ConvertMotion for JointVelocities { + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -245,9 +237,10 @@ impl Finishable for JointVelocities { } if limit_rate { command.dq_c = limit_rate_joint_velocities( - &MAX_JOINT_VELOCITY, - &MAX_JOINT_ACCELERATION, - &MAX_JOINT_JERK, + &Params::compute_upper_limits_joint_velocity(&robot_state.q_d), + &Params::compute_lower_limits_joint_velocity(&robot_state.q_d), + &Params::MAX_JOINT_ACCELERATION, + &Params::MAX_JOINT_JERK, &command.dq_c, &robot_state.dq_d, &robot_state.ddq_d, @@ -337,8 +330,10 @@ impl Finishable for CartesianPose { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } +} - fn convert_motion( +impl ConvertMotion for CartesianPose { + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -357,12 +352,12 @@ impl Finishable for CartesianPose { if limit_rate { command.O_T_EE_c = limit_rate_cartesian_pose( - MAX_TRANSLATIONAL_VELOCITY, - MAX_TRANSLATIONAL_ACCELERATION, - MAX_TRANSLATIONAL_JERK, - MAX_ROTATIONAL_VELOCITY, - MAX_ROTATIONAL_ACCELERATION, - MAX_ROTATIONAL_JERK, + Params::MAX_TRANSLATIONAL_VELOCITY, + Params::MAX_TRANSLATIONAL_ACCELERATION, + Params::MAX_TRANSLATIONAL_JERK, + Params::MAX_ROTATIONAL_VELOCITY, + Params::MAX_ROTATIONAL_ACCELERATION, + Params::MAX_ROTATIONAL_JERK, &command.O_T_EE_c, &robot_state.O_T_EE_c, &robot_state.O_dP_EE_c, @@ -384,9 +379,10 @@ impl Finishable for CartesianPose { } if limit_rate { command.elbow_c[0] = limit_rate_position( - MAX_ELBOW_VELOCITY, - MAX_ELBOW_ACCELERATION, - MAX_ELBOW_JERK, + Params::MAX_ELBOW_VELOCITY, + -Params::MAX_ELBOW_VELOCITY, + Params::MAX_ELBOW_ACCELERATION, + Params::MAX_ELBOW_JERK, command.elbow_c[0], robot_state.elbow_c[0], robot_state.delbow_c[0], @@ -450,7 +446,6 @@ impl CartesianVelocities { self.elbow.is_some() } } - impl Finishable for CartesianVelocities { fn is_finished(&self) -> bool { self.motion_finished @@ -458,8 +453,9 @@ impl Finishable for CartesianVelocities { fn set_motion_finished(&mut self, finished: bool) { self.motion_finished = finished; } - - fn convert_motion( +} +impl ConvertMotion for CartesianVelocities { + fn convert_motion( &self, robot_state: &RobotState, command: &mut MotionGeneratorCommand, @@ -479,12 +475,12 @@ impl Finishable for CartesianVelocities { } if limit_rate { command.O_dP_EE_c = limit_rate_cartesian_velocity( - MAX_TRANSLATIONAL_VELOCITY, - MAX_TRANSLATIONAL_ACCELERATION, - MAX_TRANSLATIONAL_JERK, - MAX_ROTATIONAL_VELOCITY, - MAX_ROTATIONAL_ACCELERATION, - MAX_ROTATIONAL_JERK, + Params::MAX_TRANSLATIONAL_VELOCITY, + Params::MAX_TRANSLATIONAL_ACCELERATION, + Params::MAX_TRANSLATIONAL_JERK, + Params::MAX_ROTATIONAL_VELOCITY, + Params::MAX_ROTATIONAL_ACCELERATION, + Params::MAX_ROTATIONAL_JERK, &command.O_dP_EE_c, &robot_state.O_dP_EE_c, &robot_state.O_ddP_EE_c, @@ -508,9 +504,10 @@ impl Finishable for CartesianVelocities { } if limit_rate { command.elbow_c[0] = limit_rate_position( - MAX_ELBOW_VELOCITY, - MAX_ELBOW_ACCELERATION, - MAX_ELBOW_JERK, + Params::MAX_ELBOW_VELOCITY, + -Params::MAX_ELBOW_VELOCITY, + Params::MAX_ELBOW_ACCELERATION, + Params::MAX_ELBOW_JERK, command.elbow_c[0], robot_state.elbow_c[0], robot_state.delbow_c[0], diff --git a/src/robot/error.rs b/src/robot/error.rs index 7727649..dc0f4a6 100644 --- a/src/robot/error.rs +++ b/src/robot/error.rs @@ -1,5 +1,6 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + //! Defines the errors that can occur while controlling the robot. use std::fmt::{Debug, Display, Formatter, Result}; diff --git a/src/robot/errors.rs b/src/robot/errors.rs index 8b2d077..a54b0d4 100644 --- a/src/robot/errors.rs +++ b/src/robot/errors.rs @@ -15,7 +15,7 @@ pub struct FrankaErrors { pub franka_errors: Vec, } -pub enum FrankaErrorKind { +pub(crate) enum FrankaErrorKind { Error, ReflexReason, } diff --git a/src/robot/fr3.rs b/src/robot/fr3.rs new file mode 100644 index 0000000..cfd6b42 --- /dev/null +++ b/src/robot/fr3.rs @@ -0,0 +1,498 @@ +// Copyright (c) 2023 Marco Boneberger +// Licensed under the EUPL-1.2-or-later + +use crate::device_data::DeviceData; +use crate::exception::{create_command_exception, FrankaException}; +use crate::network::Network; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; +use crate::robot::private_robot::PrivateRobot; +use crate::robot::rate_limiting::{RateLimiter, DELTA_T, LIMIT_EPS}; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; +use crate::robot::robot_impl::RobotImplGeneric; +use crate::robot::service_types; +use crate::robot::service_types::{ + AutomaticErrorRecoveryStatusFr3, ConnectRequest, ConnectRequestWithFr3Header, Fr3CommandEnum, + Fr3CommandHeader, GetterSetterStatusFr3, LoadModelLibraryRequestWithFr3Header, + MoveRequestWithFr3Header, MoveStatusFr3, SetCartesianImpedanceRequestWithFr3Header, + SetCollisionBehaviorRequestWithFr3Header, SetEeToKRequestWithFr3Header, + SetGuidingModeRequestWithFr3Header, SetJointImpedanceRequestWithFr3Header, + SetLoadRequestWithFr3Header, SetNeToEeRequestWithFr3Header, StopMoveStatusFr3, FR3_VERSION, +}; +use crate::robot::types::Fr3StateIntern; +use crate::{FrankaResult, RealtimeConfig, RobotModel, RobotState}; +use std::mem::size_of; + +/// Maintains a network connection to the FR3 robot, provides the current robot state, gives access to +/// the model library and allows to control the robot. See the [`Robot`](crate::Robot) trait for +/// methods to control the robot. +/// +/// # Rate Limiting +/// For FR3 the rate limiter is turned off by default. You can enable it by setting the `limit_rate` +/// argument in the control methods to `true`. The implementation is based on +/// [https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3](https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3). +/// That means the maximum velocity depends on the current joint configuration of the robot. +pub struct Fr3(RobotImplGeneric); + +impl RobotData for Fr3 { + type Model = RobotModel; + type StateIntern = Fr3StateIntern; + type State = RobotState; +} + +impl PrivateRobot for Fr3 { + type Rob = RobotImplGeneric; + + fn get_rob_mut(&mut self) -> &mut Self::Rob { + &mut self.0 + } + fn get_rob(&self) -> &Self::Rob { + &self.0 + } + fn get_net(&mut self) -> &mut Network { + &mut self.0.network + } +} + +impl Fr3 { + /// Establishes a connection with a FR3 robot. + /// + /// # Arguments + /// * `franka_address` - IP/hostname of the robot. + /// * `realtime_config` - if set to Enforce, an exception will be thrown if realtime priority + /// cannot be set when required. Setting realtime_config to Ignore disables this behavior. Default is Enforce + /// * `log_size` - sets how many last states should be kept for logging purposes. + /// The log is provided when a [`ControlException`](`crate::exception::FrankaException::ControlException`) is thrown. + /// # Example + /// ```no_run + /// use franka::{FrankaResult, RealtimeConfig, Fr3}; + /// fn main() -> FrankaResult<()> { + /// // connects to the robot using real-time scheduling and a default log size of 50. + /// let mut robot = Fr3::new("robotik-bs.de", None, None)?; + /// // connects to the robot without using real-time scheduling and a log size of 1. + /// let mut robot = Fr3::new("robotik-bs.de", RealtimeConfig::Ignore, 1)?; + /// Ok(()) + /// } + /// ``` + /// # Errors + /// * [`NetworkException`](FrankaException::NetworkException) if the connection is unsuccessful. + /// * [`IncompatibleLibraryVersionError`](FrankaException::IncompatibleLibraryVersionError) if this version of `libfranka-rs` is not supported or + /// if the robot is not a FR3. + pub fn new>, LogSize: Into>>( + franka_address: &str, + realtime_config: RtConfig, + log_size: LogSize, + ) -> FrankaResult { + let realtime_config = realtime_config.into().unwrap_or(RealtimeConfig::Enforce); + let log_size = log_size.into().unwrap_or(50); + let network = Network::new(franka_address, service_types::COMMAND_PORT).map_err(|_| { + FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + } + })?; + Ok(Fr3(RobotImplGeneric::new( + network, + log_size, + realtime_config, + )?)) + } +} + +impl DeviceData for Fr3 { + type CommandHeader = Fr3CommandHeader; + type CommandEnum = Fr3CommandEnum; + fn create_header( + command_id: &mut u32, + command: Self::CommandEnum, + size: usize, + ) -> Self::CommandHeader { + let header = Fr3CommandHeader::new(command, *command_id, size as u32); + *command_id += 1; + header + } + + fn get_library_version() -> u16 { + FR3_VERSION + } +} + +impl PrivateRobotData for Fr3 { + const MODEL_NAME: &'static str = "fr3"; + type Header = Fr3CommandHeader; + type LoadModelRequestWithHeader = LoadModelLibraryRequestWithFr3Header; + type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithFr3Header; + type SetLoadRequestWithHeader = SetLoadRequestWithFr3Header; + type SetJointImpedanceRequestWithHeader = SetJointImpedanceRequestWithFr3Header; + type SetCartesianImpedanceRequestWithHeader = SetCartesianImpedanceRequestWithFr3Header; + type SetGuidingModeRequestWithHeader = SetGuidingModeRequestWithFr3Header; + type ConnectRequestWithHeader = ConnectRequestWithFr3Header; + type SetEeToKRequestWithHeader = SetEeToKRequestWithFr3Header; + type SetNeToEeRequestWithHeader = SetNeToEeRequestWithFr3Header; + type MoveRequestWithHeader = MoveRequestWithFr3Header; + type MoveStatus = MoveStatusFr3; + type GetterSetterStatus = GetterSetterStatusFr3; + type StopMoveStatus = StopMoveStatusFr3; + type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusFr3; + + fn create_connect_request( + command_id: &mut u32, + udp_port: u16, + ) -> Self::ConnectRequestWithHeader { + let request = ConnectRequest { + version: FR3_VERSION, + udp_port, + }; + ConnectRequestWithFr3Header { + header: Self::create_header( + command_id, + Fr3CommandEnum::Connect, + size_of::(), + ), + request, + } + } + + fn create_automatic_error_recovery_request(command_id: &mut u32) -> Self::CommandHeader { + Self::create_header( + command_id, + Fr3CommandEnum::AutomaticErrorRecovery, + size_of::(), + ) + } + + fn create_stop_request(command_id: &mut u32) -> Self::CommandHeader { + Self::create_header( + command_id, + Fr3CommandEnum::StopMove, + size_of::(), + ) + } + + fn handle_command_move_status(status: Self::MoveStatus) -> Result<(), FrankaException> { + match status { + MoveStatusFr3::Success => Ok(()), + MoveStatusFr3::MotionStarted => { + //todo handle motion_generator_running == true + Ok(()) + } + MoveStatusFr3::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: User Stop pressed!", + )), + MoveStatusFr3::ReflexAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: motion aborted by reflex!", + )), + MoveStatusFr3::InputErrorAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: invalid input provided!", + )), + MoveStatusFr3::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: command not possible in the current mode!", + )), + MoveStatusFr3::StartAtSingularPoseRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: cannot start at singular pose!", + )), + MoveStatusFr3::InvalidArgumentRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: maximum path deviation out of range!", + )), + MoveStatusFr3::Preempted => Err(create_command_exception( + "libfranka-rs: Move command preempted!", + )), + MoveStatusFr3::Aborted => Err(create_command_exception( + "libfranka-rs: Move command aborted!", + )), + MoveStatusFr3::PreemptedDueToActivatedSafetyFunctions =>Err(create_command_exception( + "libfranka-rs: Move command preempted due to activated safety function! Please disable all safety functions.", + )), + MoveStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => + Err(create_command_exception( + "libfranka-rs: Move command rejected due to activated safety function! Please disable all safety functions.", + )) + + + } + } + + fn create_control_exception( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaException { + let mut exception_string = String::from(&message); + if move_status == MoveStatusFr3::ReflexAborted { + exception_string += " "; + exception_string += reflex_reasons.to_string().as_str(); + if log.len() >= 2 { + let lost_packets: u128 = + (log.last().unwrap().state.time - log[log.len() - 2].state.time).as_millis() + - 1; + exception_string += format!( + "\ncontrol_command_success_rate: {}", + log[log.len() - 2].state.control_command_success_rate + * (1. - lost_packets as f64 / 100.) + ) + .as_str(); + if lost_packets > 0 { + exception_string += format!( + " packets lost in a row in the last sample: {}", + lost_packets + ) + .as_str(); + } + } + } + FrankaException::ControlException { + error: exception_string, + log: Some(log), + } + } + fn create_control_exception_if_reflex_aborted( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaResult<()> { + if move_status == MoveStatusFr3::ReflexAborted { + return Err(Self::create_control_exception( + message, + move_status, + reflex_reasons, + log, + )); + } + Ok(()) + } + + fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()> { + match status { + GetterSetterStatusFr3::Success => Ok(()), + GetterSetterStatusFr3::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )), + GetterSetterStatusFr3::InvalidArgumentRejected => Err(create_command_exception( + "libfranka-rs: command rejected: invalid argument!", + )), + GetterSetterStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + "libfranka-rs: command rejected due to activated safety function! Please disable all safety functions.", + )), + } + } + + fn handle_automatic_error_recovery_status( + status: Self::AutomaticErrorRecoveryStatus, + ) -> FrankaResult<()> { + match &status { + AutomaticErrorRecoveryStatusFr3::Success => Ok(()), + AutomaticErrorRecoveryStatusFr3::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: command aborted: User Stop pressed!", + )), + AutomaticErrorRecoveryStatusFr3::ReflexAborted => Err(create_command_exception( + "libfranka-rs: command aborted: motion aborted by reflex!", + )), + AutomaticErrorRecoveryStatusFr3::CommandNotPossibleRejected => { + Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )) + } + AutomaticErrorRecoveryStatusFr3::ManualErrorRecoveryRequiredRejected => { + Err(create_command_exception( + "libfranka-rs: command rejected: manual error recovery required!", + )) + } + AutomaticErrorRecoveryStatusFr3::Aborted => { + Err(create_command_exception("libfranka-rs: command aborted!")) + } + AutomaticErrorRecoveryStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + "libfranka-rs: command rejected due to activated safety function! Please disable all safety functions.", + )), + } + } + + fn handle_command_stop_move_status( + status: Self::StopMoveStatus, + ) -> Result<(), FrankaException> { + match status { + StopMoveStatusFr3::Success => Ok(()), + StopMoveStatusFr3::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted: User Stop pressed!", + )), + StopMoveStatusFr3::ReflexAborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted: motion aborted by reflex!", + )), + StopMoveStatusFr3::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: Stop command rejected: command not possible in the current mode!", + )), + StopMoveStatusFr3::Aborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted!", + )), + StopMoveStatusFr3::CommandRejectedDueToActivatedSafetyFunctions => Err(create_command_exception( + "libfranka-rs: Move command rejected due to activated safety function! Please disable all safety functions.", + )) + } + } +} + +/// This is the position based velocity rate limiter for Fr3. The implementation is based on +/// [https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3](https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3). +impl Fr3 { + /// Tolerance value for joint velocity limits to deal with numerical errors and data losses. + pub const JOINT_VELOCITY_LIMITS_TOLERANCE: [f64; 7] = [ + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[0], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[1], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[2], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[3], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[4], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[5], + LIMIT_EPS + Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[6], + ]; + + fn upper_limits(a: f64, b: f64, c: f64, d: f64, e: f64, tolerance: f64) -> f64 { + f64::min(a, f64::max(0., b + f64::sqrt(f64::max(0., c * (d - e))))) - tolerance + } + + fn lower_limits(a: f64, b: f64, c: f64, d: f64, e: f64, tolerance: f64) -> f64 { + f64::max(a, f64::min(0., b - f64::sqrt(f64::max(0., c * (d + e))))) + tolerance + } +} + +impl RateLimiter for Fr3 { + const RATE_LIMITING_ON_PER_DEFAULT: bool = false; + const TOL_NUMBER_PACKETS_LOST: f64 = 0.; + const MAX_JOINT_JERK: [f64; 7] = [5000.0 - LIMIT_EPS; 7]; + const MAX_JOINT_ACCELERATION: [f64; 7] = [10.0000 - LIMIT_EPS; 7]; + const MAX_TRANSLATIONAL_JERK: f64 = 4500.0 - LIMIT_EPS; + const MAX_TRANSLATIONAL_ACCELERATION: f64 = 9.0000 - LIMIT_EPS; + const MAX_TRANSLATIONAL_VELOCITY: f64 = 3.0000 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_TRANSLATIONAL_ACCELERATION; + const MAX_ROTATIONAL_JERK: f64 = 8500.0 - LIMIT_EPS; + const MAX_ROTATIONAL_ACCELERATION: f64 = 17.0000 - LIMIT_EPS; + const MAX_ROTATIONAL_VELOCITY: f64 = 2.5000 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_ROTATIONAL_ACCELERATION; + const MAX_ELBOW_JERK: f64 = 5000. - LIMIT_EPS; + const MAX_ELBOW_ACCELERATION: f64 = 10.0000 - LIMIT_EPS; + const MAX_ELBOW_VELOCITY: f64 = + 1.5 - LIMIT_EPS - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_ELBOW_ACCELERATION; + + fn compute_upper_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7] { + [ + Self::upper_limits( + 2.62, + -0.3, + 12., + 2.75010, + q[0], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[0], + ), + Self::upper_limits( + 2.62, + -0.2, + 5.17, + 1.79180, + q[1], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[1], + ), + Self::upper_limits( + 2.62, + -0.2, + 7., + 2.90650, + q[2], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[2], + ), + Self::upper_limits( + 2.62, + -0.3, + 8., + -0.1458, + q[3], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[3], + ), + Self::upper_limits( + 5.26, + -0.35, + 34., + 2.81010, + q[4], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[4], + ), + Self::upper_limits( + 4.18, + -0.35, + 11., + 4.52050, + q[5], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[5], + ), + Self::upper_limits( + 5.26, + -0.35, + 34., + 3.01960, + q[6], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[6], + ), + ] + } + + fn compute_lower_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7] { + [ + Self::lower_limits( + -2.62, + -0.3, + 12., + 2.75010, + q[0], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[0], + ), + Self::lower_limits( + -2.62, + -0.2, + 5.17, + 1.791800, + q[1], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[1], + ), + Self::lower_limits( + -2.62, + -0.2, + 7., + 2.906500, + q[2], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[2], + ), + Self::lower_limits( + -2.62, + -0.3, + 8., + 3.048100, + q[3], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[3], + ), + Self::lower_limits( + -5.26, + -0.35, + 34., + 2.810100, + q[4], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[4], + ), + Self::lower_limits( + -4.18, + -0.35, + 11., + -0.54092, + q[5], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[5], + ), + Self::lower_limits( + -5.26, + -0.35, + 34., + 3.019600, + q[6], + Self::JOINT_VELOCITY_LIMITS_TOLERANCE[6], + ), + ] + } +} diff --git a/src/robot/logger.rs b/src/robot/logger.rs index db4fb76..7d2354a 100644 --- a/src/robot/logger.rs +++ b/src/robot/logger.rs @@ -5,9 +5,10 @@ use crate::robot::control_types::{ CartesianPose, CartesianVelocities, JointPositions, JointVelocities, Torques, }; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::types::RobotCommand; use std::collections::VecDeque; +use std::fmt::Debug; /// Command sent to the robot. Structure used only for logging purposes. #[derive(Debug, Copy, Clone)] @@ -32,29 +33,29 @@ pub struct RobotCommandLog { /// corresponding robot state of timestamp n+1. /// Provided by the [`ControlException`](`crate::exception::FrankaException::ControlException`). #[derive(Debug, Clone)] -pub struct Record { +pub struct Record { /// Robot state of timestamp n+1. - pub state: RobotState, + pub state: State, /// Robot command of timestamp n, after rate limiting (if activated). pub command: RobotCommandLog, } -impl Record { +impl Record { /// creates a string representation based on the debug formatter pub fn log(&self) -> String { - format!("{:?}", self.clone()) + format!("{:?}", self) } } -pub(crate) struct Logger { - states: VecDeque, +pub(crate) struct Logger { + states: VecDeque, commands: VecDeque, ring_front: usize, ring_size: usize, log_size: usize, } -impl Logger { +impl Logger { pub fn new(log_size: usize) -> Self { Logger { states: VecDeque::with_capacity(log_size), @@ -64,7 +65,7 @@ impl Logger { log_size, } } - pub fn log(&mut self, state: &RobotState, command: &RobotCommand) { + pub fn log(&mut self, state: &State, command: &RobotCommand) { self.states.push_back(state.clone()); self.commands.push_back(*command); self.ring_front = (self.ring_front + 1) % self.log_size; @@ -74,8 +75,8 @@ impl Logger { } self.ring_size = usize::min(self.log_size, self.ring_size + 1); } - pub fn flush(&mut self) -> Vec { - let mut out: Vec = Vec::new(); + pub fn flush(&mut self) -> Vec> { + let mut out: Vec> = Vec::new(); for i in 0..self.ring_size { let index = (self.ring_front + i) % self.ring_size; let cmd = self.commands.get(index).unwrap(); diff --git a/src/robot/panda.rs b/src/robot/panda.rs new file mode 100644 index 0000000..fbcd2e5 --- /dev/null +++ b/src/robot/panda.rs @@ -0,0 +1,475 @@ +// Copyright (c) 2023 Marco Boneberger +// Licensed under the EUPL-1.2-or-later + +use crate::device_data::DeviceData; +use crate::exception::{create_command_exception, FrankaException}; +use crate::network::Network; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; +use crate::robot::private_robot::PrivateRobot; +use crate::robot::rate_limiting::{RateLimiter, DELTA_T, LIMIT_EPS}; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; +use crate::robot::robot_impl::RobotImplGeneric; +use crate::robot::service_types; +use crate::robot::service_types::{ + AutomaticErrorRecoveryStatusPanda, ConnectRequest, ConnectRequestWithPandaHeader, + GetCartesianLimitRequest, GetCartesianLimitRequestWithPandaHeader, GetCartesianLimitResponse, + GetterSetterStatusPanda, LoadModelLibraryRequestWithPandaHeader, MoveRequestWithPandaHeader, + MoveStatusPanda, PandaCommandEnum, PandaCommandHeader, + SetCartesianImpedanceRequestWithPandaHeader, SetCollisionBehaviorRequestWithPandaHeader, + SetEeToKRequestWithPandaHeader, SetFiltersRequest, SetFiltersRequestWithPandaHeader, + SetFiltersResponse, SetGuidingModeRequestWithPandaHeader, + SetJointImpedanceRequestWithPandaHeader, SetLoadRequestWithPandaHeader, + SetNeToEeRequestWithPandaHeader, StopMoveStatusPanda, PANDA_VERSION, +}; +use crate::robot::types::PandaStateIntern; +use crate::robot::virtual_wall_cuboid::VirtualWallCuboid; +use crate::{FrankaResult, RealtimeConfig, RobotModel, RobotState}; +use std::mem::size_of; + +/// Maintains a network connection to the Panda robot, provides the current robot state, gives access to +/// the model library and allows to control the robot. See the [`Robot`](crate::Robot) trait for +/// methods to control the robot. +/// +/// # Rate Limiting +/// For Panda the rate limiter is enabled by default. You can disable it by setting the `limit_rate` +/// argument in the control methods to `false`. +/// The implementation is based on the rate limiter in the original libfranka library +/// that was used for Panda robots (version 0.9.2 and lower). +/// Here, the joint position is completely ignored for calculating the maximum and minimum +/// joint velocities. +pub struct Panda(RobotImplGeneric); + +impl Panda { + /// Establishes a connection with a Panda robot. + /// + /// # Arguments + /// * `franka_address` - IP/hostname of the robot. + /// * `realtime_config` - if set to Enforce, an exception will be thrown if realtime priority + /// cannot be set when required. Setting realtime_config to Ignore disables this behavior. Default is Enforce + /// * `log_size` - sets how many last states should be kept for logging purposes. + /// The log is provided when a [`ControlException`](`crate::exception::FrankaException::ControlException`) is thrown. + /// # Example + /// ```no_run + /// use franka::{FrankaResult, RealtimeConfig, Panda}; + /// fn main() -> FrankaResult<()> { + /// // connects to the robot using real-time scheduling and a default log size of 50. + /// let mut robot = Panda::new("robotik-bs.de", None, None)?; + /// // connects to the robot without using real-time scheduling and a log size of 1. + /// let mut robot = Panda::new("robotik-bs.de", RealtimeConfig::Ignore, 1)?; + /// Ok(()) + /// } + /// ``` + /// # Errors + /// * [`NetworkException`](FrankaException::NetworkException) if the connection is unsuccessful. + /// * [`IncompatibleLibraryVersionError`](FrankaException::IncompatibleLibraryVersionError) if this version of `libfranka-rs` is not supported or + /// if the robot is not a Panda. + pub fn new>, LogSize: Into>>( + franka_address: &str, + realtime_config: RtConfig, + log_size: LogSize, + ) -> FrankaResult { + let realtime_config = realtime_config.into().unwrap_or(RealtimeConfig::Enforce); + let log_size = log_size.into().unwrap_or(50); + let network = Network::new(franka_address, service_types::COMMAND_PORT).map_err(|_| { + FrankaException::NetworkException { + message: "Connection could not be established".to_string(), + } + })?; + Ok(Panda(RobotImplGeneric::new( + network, + log_size, + realtime_config, + )?)) + } + + /// Sets the cut off frequency for the given motion generator or controller. + /// + /// Allowed input range for all the filters is between 1.0 Hz and 1000.0 Hz. + /// If the value is set to maximum (1000Hz) then no filtering is done. + /// # Arguments + /// * `joint_position_filter_frequency` - Frequency at which the commanded joint + /// position is cut off. + /// * `joint_velocity_filter_frequency` - TFrequency at which the commanded joint + /// velocity is cut off. + /// * `cartesian_position_filter_frequency` - Frequency at which the commanded + /// Cartesian position is cut off. + /// * `cartesian_velocity_filter_frequency` - Frequency at which the commanded + /// Cartesian velocity is cut off. + /// * `controller_filter_frequency` - Frequency at which the commanded torque is cut off + /// # Errors + /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + #[deprecated(note = "please use `low_pass_filter` instead")] + pub fn set_filters( + &mut self, + joint_position_filter_frequency: f64, + joint_velocity_filter_frequency: f64, + cartesian_position_filter_frequency: f64, + cartesian_velocity_filter_frequency: f64, + controller_filter_frequency: f64, + ) -> FrankaResult<()> { + let command = SetFiltersRequestWithPandaHeader { + header: self.0.network.create_header_for_panda( + PandaCommandEnum::SetFilters, + size_of::(), + ), + request: SetFiltersRequest::new( + joint_position_filter_frequency, + joint_velocity_filter_frequency, + cartesian_position_filter_frequency, + cartesian_velocity_filter_frequency, + controller_filter_frequency, + ), + }; + let command_id: u32 = self.0.network.tcp_send_request(command); + let response: SetFiltersResponse = self.0.network.tcp_blocking_receive_response(command_id); + Panda::handle_getter_setter_status(response.status) + } + + /// Returns the parameters of a virtual wall. + /// # Arguments + /// * `id` - ID of the virtual wall. + /// # Return + /// Parameters of virtual wall. + /// # Errors + /// * [`CommandException`](`crate::exception::FrankaException::CommandException`) if the Control reports an error. + /// * [`NetworkException`](`crate::exception::FrankaException::NetworkException`) if the connection is lost, e.g. after a timeout. + pub fn get_virtual_wall(&mut self, id: i32) -> FrankaResult { + let command = GetCartesianLimitRequestWithPandaHeader { + header: self.0.network.create_header_for_panda( + PandaCommandEnum::GetCartesianLimit, + size_of::(), + ), + request: GetCartesianLimitRequest::new(id), + }; + let command_id: u32 = self.0.network.tcp_send_request(command); + let response: GetCartesianLimitResponse = + self.0.network.tcp_blocking_receive_response(command_id); + match &response.status { + GetterSetterStatusPanda::Success => Ok(VirtualWallCuboid::new(id, response)), + GetterSetterStatusPanda::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )), + GetterSetterStatusPanda::InvalidArgumentRejected => Err(create_command_exception( + "libfranka-rs: command rejected: invalid argument!", + )), + } + } +} + +impl PrivateRobot for Panda { + type Rob = RobotImplGeneric; + + fn get_rob_mut(&mut self) -> &mut Self::Rob { + &mut self.0 + } + fn get_rob(&self) -> &Self::Rob { + &self.0 + } + fn get_net(&mut self) -> &mut Network { + &mut self.0.network + } +} + +impl DeviceData for Panda { + type CommandHeader = PandaCommandHeader; + type CommandEnum = PandaCommandEnum; + fn create_header( + command_id: &mut u32, + command: Self::CommandEnum, + size: usize, + ) -> Self::CommandHeader { + let header = PandaCommandHeader::new(command, *command_id, size as u32); + *command_id += 1; + header + } + + fn get_library_version() -> u16 { + PANDA_VERSION + } +} + +impl RobotData for Panda { + type Model = RobotModel; + type StateIntern = PandaStateIntern; + type State = RobotState; +} + +impl Panda { + /// Maximum joint velocity + pub const MAX_JOINT_VELOCITY: [f64; 7] = [ + 2.1750 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[0], + 2.1750 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[1], + 2.1750 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[2], + 2.1750 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[3], + 2.6100 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[4], + 2.6100 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[5], + 2.6100 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_JOINT_ACCELERATION[6], + ]; + + /// Minimum joint velocity + pub const MIN_JOINT_VELOCITY: [f64; 7] = [ + -Self::MAX_JOINT_VELOCITY[0], + -Self::MAX_JOINT_VELOCITY[1], + -Self::MAX_JOINT_VELOCITY[2], + -Self::MAX_JOINT_VELOCITY[3], + -Self::MAX_JOINT_VELOCITY[4], + -Self::MAX_JOINT_VELOCITY[5], + -Self::MAX_JOINT_VELOCITY[6], + ]; +} + +impl RateLimiter for Panda { + const RATE_LIMITING_ON_PER_DEFAULT: bool = true; + const TOL_NUMBER_PACKETS_LOST: f64 = 3.; + const MAX_JOINT_JERK: [f64; 7] = [ + 7500.0 - LIMIT_EPS, + 3750.0 - LIMIT_EPS, + 5000.0 - LIMIT_EPS, + 6250.0 - LIMIT_EPS, + 7500.0 - LIMIT_EPS, + 10000.0 - LIMIT_EPS, + 10000.0 - LIMIT_EPS, + ]; + const MAX_JOINT_ACCELERATION: [f64; 7] = [ + 15.0000 - LIMIT_EPS, + 7.500 - LIMIT_EPS, + 10.0000 - LIMIT_EPS, + 12.5000 - LIMIT_EPS, + 15.0000 - LIMIT_EPS, + 20.0000 - LIMIT_EPS, + 20.0000 - LIMIT_EPS, + ]; + const MAX_TRANSLATIONAL_JERK: f64 = 6500.0 - LIMIT_EPS; + const MAX_TRANSLATIONAL_ACCELERATION: f64 = 13.0000 - LIMIT_EPS; + const MAX_TRANSLATIONAL_VELOCITY: f64 = 2.0000 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_TRANSLATIONAL_ACCELERATION; + const MAX_ROTATIONAL_JERK: f64 = 12500.0 - LIMIT_EPS; + const MAX_ROTATIONAL_ACCELERATION: f64 = 25.0000 - LIMIT_EPS; + const MAX_ROTATIONAL_VELOCITY: f64 = 2.5000 + - LIMIT_EPS + - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_ROTATIONAL_ACCELERATION; + const MAX_ELBOW_JERK: f64 = 5000. - LIMIT_EPS; + const MAX_ELBOW_ACCELERATION: f64 = 10.0000 - LIMIT_EPS; + const MAX_ELBOW_VELOCITY: f64 = + 2.1750 - LIMIT_EPS - Self::TOL_NUMBER_PACKETS_LOST * DELTA_T * Self::MAX_ELBOW_ACCELERATION; + + fn compute_upper_limits_joint_velocity(_q: &[f64; 7]) -> [f64; 7] { + Self::MAX_JOINT_VELOCITY + } + + fn compute_lower_limits_joint_velocity(_q: &[f64; 7]) -> [f64; 7] { + Self::MIN_JOINT_VELOCITY + } +} + +impl PrivateRobotData for Panda { + const MODEL_NAME: &'static str = "panda"; + type Header = PandaCommandHeader; + type LoadModelRequestWithHeader = LoadModelLibraryRequestWithPandaHeader; + type SetCollisionBehaviorRequestWithHeader = SetCollisionBehaviorRequestWithPandaHeader; + type SetLoadRequestWithHeader = SetLoadRequestWithPandaHeader; + type SetJointImpedanceRequestWithHeader = SetJointImpedanceRequestWithPandaHeader; + type SetCartesianImpedanceRequestWithHeader = SetCartesianImpedanceRequestWithPandaHeader; + type SetGuidingModeRequestWithHeader = SetGuidingModeRequestWithPandaHeader; + type ConnectRequestWithHeader = ConnectRequestWithPandaHeader; + type SetEeToKRequestWithHeader = SetEeToKRequestWithPandaHeader; + type SetNeToEeRequestWithHeader = SetNeToEeRequestWithPandaHeader; + type MoveRequestWithHeader = MoveRequestWithPandaHeader; + type MoveStatus = MoveStatusPanda; + type GetterSetterStatus = GetterSetterStatusPanda; + type StopMoveStatus = StopMoveStatusPanda; + type AutomaticErrorRecoveryStatus = AutomaticErrorRecoveryStatusPanda; + + fn create_connect_request( + command_id: &mut u32, + udp_port: u16, + ) -> Self::ConnectRequestWithHeader { + let request = ConnectRequest::new(udp_port, PANDA_VERSION); + ConnectRequestWithPandaHeader { + header: Self::create_header( + command_id, + PandaCommandEnum::Connect, + size_of::(), + ), + request, + } + } + + fn create_automatic_error_recovery_request(command_id: &mut u32) -> Self::CommandHeader { + Self::create_header( + command_id, + PandaCommandEnum::AutomaticErrorRecovery, + size_of::(), + ) + } + + fn create_stop_request(command_id: &mut u32) -> Self::CommandHeader { + Self::create_header( + command_id, + PandaCommandEnum::StopMove, + size_of::(), + ) + } + + fn handle_command_move_status(status: Self::MoveStatus) -> Result<(), FrankaException> { + match status { + MoveStatusPanda::Success => Ok(()), + MoveStatusPanda::MotionStarted => { + //todo handle motion_generator_running == true + Ok(()) + } + MoveStatusPanda::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: User Stop pressed!", + )), + MoveStatusPanda::ReflexAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: motion aborted by reflex!", + )), + MoveStatusPanda::InputErrorAborted => Err(create_command_exception( + "libfranka-rs: Move command aborted: invalid input provided!", + )), + MoveStatusPanda::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: command not possible in the current mode!", + )), + MoveStatusPanda::StartAtSingularPoseRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: cannot start at singular pose!", + )), + MoveStatusPanda::InvalidArgumentRejected => Err(create_command_exception( + "libfranka-rs: Move command rejected: maximum path deviation out of range!", + )), + MoveStatusPanda::Preempted => Err(create_command_exception( + "libfranka-rs: Move command preempted!", + )), + MoveStatusPanda::Aborted => Err(create_command_exception( + "libfranka-rs: Move command aborted!", + )), + } + } + + fn create_control_exception( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaException { + let mut exception_string = String::from(&message); + if move_status == MoveStatusPanda::ReflexAborted { + exception_string += " "; + exception_string += reflex_reasons.to_string().as_str(); + if log.len() >= 2 { + let lost_packets: u128 = + (log.last().unwrap().state.time - log[log.len() - 2].state.time).as_millis() + - 1; + exception_string += format!( + "\ncontrol_command_success_rate: {}", + log[log.len() - 2].state.control_command_success_rate + * (1. - lost_packets as f64 / 100.) + ) + .as_str(); + if lost_packets > 0 { + exception_string += format!( + " packets lost in a row in the last sample: {}", + lost_packets + ) + .as_str(); + } + } + } + FrankaException::ControlException { + error: exception_string, + log: Some(log), + } + } + fn create_control_exception_if_reflex_aborted( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaResult<()> { + // todo think about if option is a good return type + if move_status == MoveStatusPanda::ReflexAborted { + return Err(Self::create_control_exception( + message, + move_status, + reflex_reasons, + log, + )); + } + Ok(()) + } + + fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()> { + match status { + GetterSetterStatusPanda::Success => Ok(()), + GetterSetterStatusPanda::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )), + GetterSetterStatusPanda::InvalidArgumentRejected => Err(create_command_exception( + "libfranka-rs: command rejected: invalid argument!", + )), + } + } + + fn handle_automatic_error_recovery_status( + status: Self::AutomaticErrorRecoveryStatus, + ) -> FrankaResult<()> { + match status { + AutomaticErrorRecoveryStatusPanda::Success => Ok(()), + AutomaticErrorRecoveryStatusPanda::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: command aborted: User Stop pressed!", + )), + AutomaticErrorRecoveryStatusPanda::ReflexAborted => Err(create_command_exception( + "libfranka-rs: command aborted: motion aborted by reflex!", + )), + AutomaticErrorRecoveryStatusPanda::CommandNotPossibleRejected => { + Err(create_command_exception( + "libfranka-rs: command rejected: command not possible in current mode", + )) + } + AutomaticErrorRecoveryStatusPanda::ManualErrorRecoveryRequiredRejected => { + Err(create_command_exception( + "libfranka-rs: command rejected: manual error recovery required!", + )) + } + AutomaticErrorRecoveryStatusPanda::Aborted => { + Err(create_command_exception("libfranka-rs: command aborted!")) + } + } + } + + fn handle_command_stop_move_status( + status: Self::StopMoveStatus, + ) -> Result<(), FrankaException> { + match status { + StopMoveStatusPanda::Success => Ok(()), + StopMoveStatusPanda::EmergencyAborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted: User Stop pressed!", + )), + StopMoveStatusPanda::ReflexAborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted: motion aborted by reflex!", + )), + StopMoveStatusPanda::CommandNotPossibleRejected => Err(create_command_exception( + "libfranka-rs: Stop command rejected: command not possible in the current mode!", + )), + StopMoveStatusPanda::Aborted => Err(create_command_exception( + "libfranka-rs: Stop command aborted!", + )), + } + } +} diff --git a/src/robot/private_robot.rs b/src/robot/private_robot.rs new file mode 100644 index 0000000..8fed25f --- /dev/null +++ b/src/robot/private_robot.rs @@ -0,0 +1,66 @@ +// Copyright (c) 2023 Marco Boneberger +// Licensed under the EUPL-1.2-or-later + +use crate::network::Network; +use crate::robot::control_loop::ControlLoop; +use crate::robot::motion_generator_traits::MotionGeneratorTrait; +use crate::robot::rate_limiting::RateLimiter; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; +use crate::robot::robot_impl::RobotImplementation; +use crate::{ + ControllerMode, ConvertMotion, Finishable, FrankaResult, Torques, DEFAULT_CUTOFF_FREQUENCY, +}; +use std::fmt::Debug; +use std::time::Duration; + +pub(crate) trait PrivateRobot: PrivateRobotData + Sized { + type Rob: RobotImplementation; + fn get_rob_mut(&mut self) -> &mut Self::Rob; + fn get_rob(&self) -> &Self::Rob; + fn get_net(&mut self) -> &mut Network; + + fn control_motion_intern< + F: FnMut(&::State, &Duration) -> U, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, + >( + &mut self, + motion_generator_callback: F, + controller_mode: Option, + limit_rate: Option, + cutoff_frequency: Option, + ) -> FrankaResult<()> { + let controller_mode = controller_mode.unwrap_or(ControllerMode::JointImpedance); + let limit_rate = limit_rate.unwrap_or(::RATE_LIMITING_ON_PER_DEFAULT); + let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); + let mut control_loop = ControlLoop::from_control_mode( + self.get_rob_mut(), + controller_mode, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } + + fn control_torques_intern< + F: FnMut(&::State, &Duration) -> U, + U: ConvertMotion<::State> + Debug + MotionGeneratorTrait + Finishable, + >( + &mut self, + motion_generator_callback: F, + control_callback: &mut dyn FnMut(&::State, &Duration) -> Torques, + limit_rate: Option, + cutoff_frequency: Option, + ) -> FrankaResult<()> { + let limit_rate = limit_rate.unwrap_or(::RATE_LIMITING_ON_PER_DEFAULT); + let cutoff_frequency = cutoff_frequency.unwrap_or(DEFAULT_CUTOFF_FREQUENCY); + let mut control_loop = ControlLoop::new( + self.get_rob_mut(), + control_callback, + motion_generator_callback, + limit_rate, + cutoff_frequency, + )?; + control_loop.run() + } +} diff --git a/src/robot/rate_limiting.rs b/src/robot/rate_limiting.rs index a621576..2f00e1c 100644 --- a/src/robot/rate_limiting.rs +++ b/src/robot/rate_limiting.rs @@ -5,83 +5,75 @@ //! joint position and joint velocity. use nalgebra::{ - Isometry3, Matrix3, Matrix3x1, Matrix6x1, Rotation3, Translation3, UnitQuaternion, Vector3, U1, - U3, + Isometry3, Matrix3, Matrix3x1, Matrix6x1, Rotation3, Translation3, UnitQuaternion, Vector3, }; use crate::robot::control_tools::is_homogeneous_transformation; use crate::utils::array_to_isometry; -///Sample time constant -pub static DELTA_T: f64 = 1e-3; +/// Sample time constant +pub const DELTA_T: f64 = 1e-3; ///Epsilon value for checking limits -pub static LIMIT_EPS: f64 = 1e-3; +pub const LIMIT_EPS: f64 = 1e-3; /// Epsilon value for limiting Cartesian accelerations/jerks or not -pub static NORM_EPS: f64 = f64::EPSILON; -/// Number of packets lost considered for the definition of velocity limits. -/// When a packet is lost, FCI assumes a constant acceleration model -pub static TOL_NUMBER_PACKETS_LOST: f64 = 1e-3; +pub const NORM_EPS: f64 = f64::EPSILON; /// Factor for the definition of rotational limits using the Cartesian Pose interface -pub static FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE: f64 = 0.99; -/// Maximum torque rate -pub static MAX_TORQUE_RATE: [f64; 7] = [1000. - LIMIT_EPS; 7]; -/// Maximum joint jerk -pub static MAX_JOINT_JERK: [f64; 7] = [ - 7500.0 - LIMIT_EPS, - 3750.0 - LIMIT_EPS, - 5000.0 - LIMIT_EPS, - 6250.0 - LIMIT_EPS, - 7500.0 - LIMIT_EPS, - 10000.0 - LIMIT_EPS, - 10000.0 - LIMIT_EPS, -]; -/// Maximum joint acceleration -pub static MAX_JOINT_ACCELERATION: [f64; 7] = [ - 15.0000 - LIMIT_EPS, - 7.500 - LIMIT_EPS, - 10.0000 - LIMIT_EPS, - 12.5000 - LIMIT_EPS, - 15.0000 - LIMIT_EPS, - 20.0000 - LIMIT_EPS, - 20.0000 - LIMIT_EPS, -]; -/// Maximum joint velocity -pub static MAX_JOINT_VELOCITY: [f64; 7] = [ - 2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[0], - 2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[1], - 2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[2], - 2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[3], - 2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[4], - 2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[5], - 2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[6], -]; -/// Maximum translational jerk -pub static MAX_TRANSLATIONAL_JERK: f64 = 6500.0 - LIMIT_EPS; -/// Maximum translational acceleration -pub static MAX_TRANSLATIONAL_ACCELERATION: f64 = 13.0000 - LIMIT_EPS; -/// Maximum translational velocity -pub static MAX_TRANSLATIONAL_VELOCITY: f64 = - 2.0000 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_TRANSLATIONAL_ACCELERATION; -/// Maximum rotational jerk -pub static MAX_ROTATIONAL_JERK: f64 = 12500.0 - LIMIT_EPS; -/// Maximum rotational acceleration -pub static MAX_ROTATIONAL_ACCELERATION: f64 = 25.0000 - LIMIT_EPS; -/// Maximum rotational velocity -pub static MAX_ROTATIONAL_VELOCITY: f64 = - 2.5000 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ROTATIONAL_ACCELERATION; -/// Maximum elbow jerk -pub static MAX_ELBOW_JERK: f64 = 5000. - LIMIT_EPS; -/// Maximum elbow acceleration -pub static MAX_ELBOW_ACCELERATION: f64 = 10.0000 - LIMIT_EPS; -/// Maximum elbow velocity -pub static MAX_ELBOW_VELOCITY: f64 = - 2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ELBOW_ACCELERATION; +const FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE: f64 = 0.99; + +/// The rate limiter can limit the rate of torques, Cartesian pose, Cartesian velocity, +/// joint position and joint velocity. The rate limiter trait is implemented for [Fr3](crate::Fr3#impl-RateLimiter-for-Fr3) and [Panda](crate::Panda#impl-RateLimiter-for-Panda) +/// in different ways. +pub trait RateLimiter { + /// Defines if the rate limiter is used when `None` is passed to the `limit_rate` argument in + /// control methods like [`control_joint_positions`](crate::robot::Robot::control_joint_positions) + const RATE_LIMITING_ON_PER_DEFAULT: bool; + /// Number of packets lost considered for the definition of velocity limits. + /// When a packet is lost, FCI assumes a constant acceleration model + const TOL_NUMBER_PACKETS_LOST: f64; + /// Maximum torque rate + const MAX_TORQUE_RATE: [f64; 7] = [1000. - LIMIT_EPS; 7]; + /// Maximum joint jerk + const MAX_JOINT_JERK: [f64; 7]; + /// Maximum joint acceleration + const MAX_JOINT_ACCELERATION: [f64; 7]; + /// Maximum translational jerk + const MAX_TRANSLATIONAL_JERK: f64; + /// Maximum translational acceleration + const MAX_TRANSLATIONAL_ACCELERATION: f64; + /// Maximum translational velocity + const MAX_TRANSLATIONAL_VELOCITY: f64; + /// Maximum rotational jerk + const MAX_ROTATIONAL_JERK: f64; + /// Maximum rotational acceleration + const MAX_ROTATIONAL_ACCELERATION: f64; + /// Maximum rotational velocity + const MAX_ROTATIONAL_VELOCITY: f64; + /// Maximum elbow jerk + const MAX_ELBOW_JERK: f64; + /// Maximum elbow acceleration + const MAX_ELBOW_ACCELERATION: f64; + /// Maximum elbow velocity + const MAX_ELBOW_VELOCITY: f64; + /// Computes the maximum joint velocity based on joint position + /// # Arguments + /// * `q` - joint position + /// # Return + /// Upper limits of joint velocity at the given joint position. + fn compute_upper_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7]; + /// Computes the minimum joint velocity based on joint position + /// # Arguments + /// * `q` - joint position + /// # Return + /// Lower limits of joint velocity at the given joint position. + fn compute_lower_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7]; +} /// Limits the rate of a desired joint position considering the limits provided. /// # Note /// FCI filters must be deactivated to work properly. /// # Arguments /// * `max_velocity` - Per-joint maximum allowed velocity. +/// * `min_velocity` - Per-joint minimum allowed velocity. /// * `max_acceleration` - Per-joint maximum allowed acceleration. /// * `max_jerk` - Per-joint maximum allowed jerk. /// * `commanded_positions` - Commanded joint positions of the current time step. @@ -92,8 +84,10 @@ pub static MAX_ELBOW_VELOCITY: f64 = /// * if commanded_positions are infinite or NaN. /// # Return /// Rate-limited vector of desired joint positions. +#[allow(clippy::too_many_arguments)] pub fn limit_rate_joint_positions( max_velocity: &[f64; 7], + min_velocity: &[f64; 7], max_acceleration: &[f64; 7], max_jerk: &[f64; 7], commanded_positions: &[f64; 7], @@ -108,6 +102,7 @@ pub fn limit_rate_joint_positions( for i in 0..7 { limited_commanded_positions[i] = limit_rate_position( max_velocity[i], + min_velocity[i], max_acceleration[i], max_jerk[i], commanded_positions[i], @@ -124,6 +119,7 @@ pub fn limit_rate_joint_positions( /// FCI filters must be deactivated to work properly. /// # Arguments /// * `max_velocity` - Maximum allowed velocity. +/// * `min_velocity` - Minimum allowed velocity. /// * `max_acceleration` - Maximum allowed acceleration. /// * `max_jerk` - Maximum allowed jerk. /// * `commanded_position` - Commanded joint position of the current time step. @@ -133,8 +129,10 @@ pub fn limit_rate_joint_positions( /// * if commanded_values are infinite or NaN. /// # Return /// Rate-limited desired joint position. +#[allow(clippy::too_many_arguments)] pub fn limit_rate_position( max_velocity: f64, + min_velocity: f64, max_acceleration: f64, max_jerk: f64, commanded_position: f64, @@ -146,6 +144,7 @@ pub fn limit_rate_position( last_commanded_position + limit_rate_velocity( max_velocity, + min_velocity, max_acceleration, max_jerk, (commanded_position - last_commanded_position) / DELTA_T, @@ -159,6 +158,7 @@ pub fn limit_rate_position( /// FCI filters must be deactivated to work properly. /// # Arguments /// * `max_velocity` - Maximum allowed velocity. +/// * `min_velocity` - Minimum allowed velocity. /// * `max_acceleration` - Maximum allowed acceleration. /// * `max_jerk` - Maximum allowed jerk. /// * `commanded_velocity` - Commanded joint velocity of the current time step. @@ -170,6 +170,7 @@ pub fn limit_rate_position( /// Rate-limited desired joint velocity. fn limit_rate_velocity( max_velocity: f64, + min_velocity: f64, max_acceleration: f64, max_jerk: f64, commanded_velocity: f64, @@ -187,7 +188,7 @@ fn limit_rate_velocity( max_acceleration, ); let safe_min_acceleration = f64::max( - (max_jerk / max_acceleration) * (-max_velocity - last_commanded_velocity), + (max_jerk / max_acceleration) * (min_velocity - last_commanded_velocity), -max_acceleration, ); last_commanded_velocity @@ -232,6 +233,7 @@ pub fn limit_rate_torques( /// FCI filters must be deactivated to work properly. /// # Arguments /// * `max_velocity` - Per-joint maximum allowed velocity. +/// * `min_velocity` - Per-joint minimum allowed velocity. /// * `max_acceleration` - Per-joint maximum allowed acceleration. /// * `max_jerk` - Per-joint maximum allowed jerk. /// * `commanded_velocities` - Commanded joint velocity of the current time step. @@ -243,6 +245,7 @@ pub fn limit_rate_torques( /// Rate-limited vector of desired joint velocities. pub fn limit_rate_joint_velocities( max_velocity: &[f64; 7], + min_velocity: &[f64; 7], max_acceleration: &[f64; 7], max_jerk: &[f64; 7], commanded_velocities: &[f64; 7], @@ -256,6 +259,7 @@ pub fn limit_rate_joint_velocities( for i in 0..7 { limited_commanded_velocities[i] = limit_rate_velocity( max_velocity[i], + min_velocity[i], max_acceleration[i], max_jerk[i], commanded_velocities[i], @@ -317,10 +321,10 @@ pub fn limit_rate_cartesian_pose( let mut commanded_O_dP_EE_c = [0.; 6]; for i in 0..3 { - commanded_O_dP_EE_c[i] = dx_head[(i)]; + commanded_O_dP_EE_c[i] = dx_head[i]; } for i in 0..3 { - commanded_O_dP_EE_c[i + 3] = dx_tail[(i)]; + commanded_O_dP_EE_c[i + 3] = dx_tail[i]; } commanded_O_dP_EE_c = limit_rate_cartesian_velocity( max_translational_velocity, @@ -335,7 +339,7 @@ pub fn limit_rate_cartesian_pose( ); let dx: Matrix6x1 = Matrix6x1::::from_column_slice(&commanded_O_dP_EE_c); limited_commanded_pose.translation = Translation3::from( - last_commanded_pose.translation.vector + Vector3::new(dx[(0)], dx[(1)], dx[(2)]) * DELTA_T, + last_commanded_pose.translation.vector + Vector3::new(dx[0], dx[1], dx[2]) * DELTA_T, ); limited_commanded_pose.rotation = last_commanded_pose.rotation; let dx_tail = dx.remove_row(0).remove_row(0).remove_row(0); @@ -343,15 +347,7 @@ pub fn limit_rate_cartesian_pose( let w_norm = dx_tail.normalize(); let theta = DELTA_T * dx_tail.norm(); let omega_skew = Matrix3::new( - 0., - -w_norm[(2)], - w_norm[(1)], - w_norm[(2)], - 0., - -w_norm[(0)], - -w_norm[(1)], - w_norm[(0)], - 0., + 0., -w_norm[2], w_norm[1], w_norm[2], 0., -w_norm[0], -w_norm[1], w_norm[0], 0., ); let R = Matrix3::identity() + f64::sin(theta) * omega_skew @@ -403,25 +399,25 @@ pub fn limit_rate_cartesian_velocity( max_translational_velocity, max_translational_acceleration, max_translational_jerk, - &Vector3::from(dx.fixed_slice::(0, 0)), - &Vector3::from(last_dx.fixed_slice::(0, 0)), - &Vector3::from(last_ddx.fixed_slice::(0, 0)), + &Vector3::from(dx.fixed_view::<3, 1>(0, 0)), + &Vector3::from(last_dx.fixed_view::<3, 1>(0, 0)), + &Vector3::from(last_ddx.fixed_view::<3, 1>(0, 0)), ); let dx_tail = limit_rate_single_cartesian_velocity( max_rotational_velocity, max_rotational_acceleration, max_rotational_jerk, - &Vector3::from(dx.fixed_slice::(3, 0)), - &Vector3::from(last_dx.fixed_slice::(3, 0)), - &Vector3::from(last_ddx.fixed_slice::(3, 0)), + &Vector3::from(dx.fixed_view::<3, 1>(3, 0)), + &Vector3::from(last_dx.fixed_view::<3, 1>(3, 0)), + &Vector3::from(last_ddx.fixed_view::<3, 1>(3, 0)), ); let mut limited_values = [0.; 6]; for i in 0..3 { - limited_values[i] = dx_head[(i)]; + limited_values[i] = dx_head[i]; } for i in 0..3 { - limited_values[i + 3] = dx_tail[(i)]; + limited_values[i + 3] = dx_tail[i]; } limited_values } @@ -465,14 +461,11 @@ fn limit_rate_single_cartesian_velocity( #[cfg(test)] mod tests { - use nalgebra::{Translation3, Unit, UnitQuaternion, Vector3, Vector6, U3}; + use crate::{Fr3, Panda}; + use nalgebra::{Translation3, Unit, UnitQuaternion, Vector3, Vector6}; use num_traits::Float; - use crate::robot::rate_limiting::{ - limit_rate_cartesian_pose, DELTA_T, LIMIT_EPS, MAX_ROTATIONAL_ACCELERATION, - MAX_ROTATIONAL_JERK, MAX_ROTATIONAL_VELOCITY, MAX_TRANSLATIONAL_ACCELERATION, - MAX_TRANSLATIONAL_JERK, MAX_TRANSLATIONAL_VELOCITY, - }; + use crate::robot::rate_limiting::{limit_rate_cartesian_pose, RateLimiter, DELTA_T, LIMIT_EPS}; use crate::utils::array_to_isometry; fn integrate_one_sample(last_pose: &[f64; 16], dx: &[f64; 6], delta_t: f64) -> [f64; 16] { @@ -512,7 +505,7 @@ mod tests { eps: f64, delta_t: f64, ) -> [f64; 6] { - let mut result = last_cmd_values.clone(); + let mut result = *last_cmd_values; result[0] += (max_translational_derivative - f64::min( f64::max(f64::abs(eps), 0.), @@ -535,7 +528,7 @@ mod tests { eps: f64, delta_t: f64, ) -> [f64; 6] { - let mut result = last_cmd_values.clone(); + let mut result = *last_cmd_values; result[0] += (max_translational_derivative + f64::max(f64::abs(eps), LIMIT_EPS)) * delta_t; result[3] += (max_rotational_derivative + f64::max(f64::abs(eps), LIMIT_EPS)) * delta_t; result @@ -581,12 +574,12 @@ mod tests { let dddx = (ddx - last_ddx) / delta_t; let violates_limits = |desired_value: f64, max_value: f64| desired_value.abs() > max_value; - violates_limits(dx.fixed_rows::(0).norm(), max_translational_dx) - || violates_limits(ddx.fixed_rows::(0).norm(), max_translational_ddx) - || violates_limits(dddx.fixed_rows::(0).norm(), max_translational_dddx) - || violates_limits(dx.fixed_rows::(3).norm(), max_rotational_dx) - || violates_limits(ddx.fixed_rows::(3).norm(), max_rotational_ddx) - || violates_limits(dddx.fixed_rows::(3).norm(), max_rotational_dddx) + violates_limits(dx.fixed_rows::<3>(0).norm(), max_translational_dx) + || violates_limits(ddx.fixed_rows::<3>(0).norm(), max_translational_ddx) + || violates_limits(dddx.fixed_rows::<3>(0).norm(), max_translational_dddx) + || violates_limits(dx.fixed_rows::<3>(3).norm(), max_rotational_dx) + || violates_limits(ddx.fixed_rows::<3>(3).norm(), max_rotational_ddx) + || violates_limits(dddx.fixed_rows::<3>(3).norm(), max_rotational_dddx) } #[test] #[allow(non_snake_case)] @@ -645,12 +638,12 @@ mod tests { ]; let out = limit_rate_cartesian_pose( - MAX_TRANSLATIONAL_VELOCITY, - MAX_TRANSLATIONAL_ACCELERATION, - MAX_TRANSLATIONAL_JERK, - MAX_ROTATIONAL_VELOCITY, - MAX_ROTATIONAL_ACCELERATION, - MAX_ROTATIONAL_JERK, + Panda::MAX_TRANSLATIONAL_VELOCITY, + Panda::MAX_TRANSLATIONAL_ACCELERATION, + Panda::MAX_TRANSLATIONAL_JERK, + Panda::MAX_ROTATIONAL_VELOCITY, + Panda::MAX_ROTATIONAL_ACCELERATION, + Panda::MAX_ROTATIONAL_JERK, &O_T_EE_c, &last_O_T_EE_c, &O_dP_EE_c, @@ -682,12 +675,12 @@ mod tests { 1.0, ]; let out = limit_rate_cartesian_pose( - MAX_TRANSLATIONAL_VELOCITY, - MAX_TRANSLATIONAL_ACCELERATION, - MAX_TRANSLATIONAL_JERK, - MAX_ROTATIONAL_VELOCITY, - MAX_ROTATIONAL_ACCELERATION, - MAX_ROTATIONAL_JERK, + Panda::MAX_TRANSLATIONAL_VELOCITY, + Panda::MAX_TRANSLATIONAL_ACCELERATION, + Panda::MAX_TRANSLATIONAL_JERK, + Panda::MAX_ROTATIONAL_VELOCITY, + Panda::MAX_ROTATIONAL_ACCELERATION, + Panda::MAX_ROTATIONAL_JERK, &O_T_EE_c, &O_T_EE_c, &[0.; 6], @@ -915,4 +908,46 @@ mod tests { .zip(last_cmd_velocity.iter()) .for_each(|(&x, &y)| assert!(f64::abs(x - y) < 1e-6)); } + + #[test] + fn position_based_velocity_limit_boundary_check_negative_velocity() { + let q_lower_limits = [-2.9007, -1.8361, -2.9107, -3.0770, -2.8763, 0.4398, -3.0508]; + let dq_upper_limits = [2.62, 2.62, 2.62, 2.62, 5.26, 4.18, 5.26]; + let dq_lower_limits = [0.; 7]; + + let dq_max = Fr3::compute_upper_limits_joint_velocity(&q_lower_limits); + assert!(dq_max + .iter() + .zip(dq_upper_limits.iter()) + .all(|(dq, dq_limit)| dq < dq_limit)); + + let dq_min = Fr3::compute_lower_limits_joint_velocity(&q_lower_limits); + assert!(dq_max + .iter() + .zip(dq_lower_limits.iter()) + .all(|(dq, dq_limit)| dq > dq_limit)); + + assert!(dq_max.iter().zip(dq_min.iter()).all(|(max, min)| max > min)); + } + + #[test] + fn position_based_velocity_limit_boundary_check_positive_velocity() { + let q_upper_limits = [2.9007, 1.8361, 2.9107, -0.1169, 2.8763, 4.6216, 3.0508]; + let dq_upper_limits = [0.; 7]; + let dq_lower_limits = [-2.62, -2.62, -2.62, -2.62, -5.26, -4.18, -5.26]; + + let dq_max = Fr3::compute_upper_limits_joint_velocity(&q_upper_limits); + assert!(dq_max + .iter() + .zip(dq_upper_limits.iter()) + .all(|(dq, dq_limit)| dq < dq_limit)); + + let dq_min = Fr3::compute_lower_limits_joint_velocity(&q_upper_limits); + assert!(dq_max + .iter() + .zip(dq_lower_limits.iter()) + .all(|(dq, dq_limit)| dq > dq_limit)); + + assert!(dq_max.iter().zip(dq_min.iter()).all(|(max, min)| max > min)); + } } diff --git a/src/robot/robot_control.rs b/src/robot/robot_control.rs index 3be44fb..347e424 100644 --- a/src/robot/robot_control.rs +++ b/src/robot/robot_control.rs @@ -1,12 +1,14 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + use crate::exception::FrankaResult; use crate::robot::control_types::RealtimeConfig; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{MoveControllerMode, MoveDeviation, MoveMotionGeneratorMode}; use crate::robot::types::{ControllerCommand, MotionGeneratorCommand}; -pub trait RobotControl { +pub(crate) trait RobotControl { + type State: AbstractRobotState; fn start_motion( &mut self, controller_mode: MoveControllerMode, @@ -25,11 +27,11 @@ pub trait RobotControl { &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, - ) -> FrankaResult; + ) -> FrankaResult; fn realtime_config(&self) -> RealtimeConfig; fn throw_on_motion_error( &mut self, - robot_state: &RobotState, + robot_state: &Self::State, motion_id: u32, ) -> FrankaResult<()>; } diff --git a/src/robot/robot_data.rs b/src/robot/robot_data.rs new file mode 100644 index 0000000..0cb6abf --- /dev/null +++ b/src/robot/robot_data.rs @@ -0,0 +1,158 @@ +// Copyright (c) 2023 Marco Boneberger +// Licensed under the EUPL-1.2-or-later + +use crate::device_data::DeviceData; +use crate::exception::FrankaException; +use crate::network::MessageCommand; +use crate::robot::errors::FrankaErrors; +use crate::robot::logger::Record; +use crate::robot::rate_limiting::RateLimiter; +use crate::robot::robot_state::AbstractRobotState; +use crate::robot::service_types::{ + ConnectRequest, LoadModelLibraryRequest, MoveRequest, RobotHeader, + SetCartesianImpedanceRequest, SetCollisionBehaviorRequest, SetEeToKRequest, + SetGuidingModeRequest, SetJointImpedanceRequest, SetLoadRequest, SetNeToEeRequest, +}; +use crate::robot::types::AbstractRobotStateIntern; +use crate::{FrankaResult, Model, RobotModel, RobotState}; +use serde::de::DeserializeOwned; +use serde::Serialize; +use std::fmt::Debug; + +/// Contains the types that defines how the data of a robot looks like. +pub trait RobotData: RateLimiter { + /// Dynamic model of the robot. + type Model: Model + Into; + /// Internal state that comes from the Robot over UDP. + type StateIntern: Debug + DeserializeOwned + Serialize + AbstractRobotStateIntern + 'static; + /// State that the user will interact with. + type State: AbstractRobotState + From + From; +} + +pub(crate) trait PrivateRobotData: DeviceData + RobotData { + const MODEL_NAME: &'static str; + type Header: RobotHeader; + type LoadModelRequestWithHeader: MessageCommand + + Serialize + + From<(u32, LoadModelLibraryRequest)>; + type SetCollisionBehaviorRequestWithHeader: MessageCommand + + Serialize + + From<(u32, SetCollisionBehaviorRequest)>; + type SetLoadRequestWithHeader: MessageCommand + Serialize + From<(u32, SetLoadRequest)>; + type SetJointImpedanceRequestWithHeader: MessageCommand + + Serialize + + From<(u32, SetJointImpedanceRequest)>; + type SetCartesianImpedanceRequestWithHeader: MessageCommand + + Serialize + + From<(u32, SetCartesianImpedanceRequest)>; + type SetGuidingModeRequestWithHeader: MessageCommand + + Serialize + + From<(u32, SetGuidingModeRequest)>; + type ConnectRequestWithHeader: MessageCommand + Serialize + From<(u32, ConnectRequest)>; + type SetEeToKRequestWithHeader: MessageCommand + Serialize + From<(u32, SetEeToKRequest)>; + type SetNeToEeRequestWithHeader: MessageCommand + Serialize + From<(u32, SetNeToEeRequest)>; + type MoveRequestWithHeader: MessageCommand + Serialize + From<(u32, MoveRequest)>; + type MoveStatus: DeserializeOwned + PartialEq + Copy + Clone + 'static; // todo is this static fine here? + type GetterSetterStatus: DeserializeOwned + Copy + Clone + 'static; // todo is this static fine here? + type StopMoveStatus: DeserializeOwned + Copy + Clone + 'static; // todo is this static fine here? + type AutomaticErrorRecoveryStatus: DeserializeOwned + Copy + Clone + 'static; // todo is this static fine here? + + fn create_model_library_request( + command_id: &mut u32, + request: LoadModelLibraryRequest, + ) -> Self::LoadModelRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + fn create_set_collision_behavior_request( + command_id: &mut u32, + request: SetCollisionBehaviorRequest, + ) -> Self::SetCollisionBehaviorRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + fn create_set_load_request( + command_id: &mut u32, + request: SetLoadRequest, + ) -> Self::SetLoadRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_set_joint_impedance_request( + command_id: &mut u32, + request: SetJointImpedanceRequest, + ) -> Self::SetJointImpedanceRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_set_cartesian_impedance_request( + command_id: &mut u32, + request: SetCartesianImpedanceRequest, + ) -> Self::SetCartesianImpedanceRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_set_guiding_mode_request( + command_id: &mut u32, + request: SetGuidingModeRequest, + ) -> Self::SetGuidingModeRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_set_ee_to_k_request( + command_id: &mut u32, + request: SetEeToKRequest, + ) -> Self::SetEeToKRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_set_ne_to_ee_request( + command_id: &mut u32, + request: SetNeToEeRequest, + ) -> Self::SetNeToEeRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_move_request( + command_id: &mut u32, + request: MoveRequest, + ) -> Self::MoveRequestWithHeader { + *command_id += 1; + (*command_id - 1, request).into() + } + + fn create_connect_request( + command_id: &mut u32, + udp_port: u16, + ) -> Self::ConnectRequestWithHeader; + + fn create_automatic_error_recovery_request(command_id: &mut u32) -> Self::CommandHeader; + fn create_stop_request(command_id: &mut u32) -> Self::CommandHeader; + + fn handle_command_move_status(status: Self::MoveStatus) -> Result<(), FrankaException>; + fn create_control_exception( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaException; + fn create_control_exception_if_reflex_aborted( + message: String, + move_status: Self::MoveStatus, + reflex_reasons: &FrankaErrors, + log: Vec>, + ) -> FrankaResult<()>; + + fn handle_getter_setter_status(status: Self::GetterSetterStatus) -> FrankaResult<()>; + fn handle_automatic_error_recovery_status( + status: Self::AutomaticErrorRecoveryStatus, + ) -> FrankaResult<()>; + fn handle_command_stop_move_status(status: Self::StopMoveStatus) + -> Result<(), FrankaException>; +} diff --git a/src/robot/robot_impl.rs b/src/robot/robot_impl.rs index 1db48ad..b427c36 100644 --- a/src/robot/robot_impl.rs +++ b/src/robot/robot_impl.rs @@ -1,33 +1,39 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later -use std::mem::size_of; -use crate::exception::{create_command_exception, FrankaException, FrankaResult}; -use crate::model::library_downloader::LibraryDownloader; -use crate::model::Model; +use crate::exception::{FrankaException, FrankaResult}; +use crate::model::library_downloader::{LibraryDownloader, LibraryDownloaderGeneric}; use crate::network::Network; use crate::robot::control_types::RealtimeConfig; -use crate::robot::errors::FrankaErrors; -use crate::robot::logger::{Logger, Record}; -use crate::robot::motion_generator_traits::MotionGeneratorTrait; +use crate::robot::logger::Logger; +use std::env::consts::{ARCH, DLL_EXTENSION, OS}; + use crate::robot::robot_control::RobotControl; -use crate::robot::robot_state::RobotState; +use crate::robot::robot_data::{PrivateRobotData, RobotData}; +use crate::robot::robot_state::AbstractRobotState; use crate::robot::service_types::{ - ConnectRequest, ConnectRequestWithHeader, ConnectResponse, ConnectStatus, MoveControllerMode, - MoveDeviation, MoveMotionGeneratorMode, MoveRequest, MoveRequestWithHeader, MoveResponse, - MoveStatus, RobotCommandEnum, StopMoveRequestWithHeader, StopMoveResponse, StopMoveStatus, - VERSION, + ConnectResponseWithoutHeader, ConnectStatus, MoveControllerMode, MoveDeviation, + MoveMotionGeneratorMode, MoveRequest, }; use crate::robot::types::{ - ControllerCommand, ControllerMode, MotionGeneratorCommand, MotionGeneratorMode, RobotCommand, - RobotMode, RobotStateIntern, + AbstractRobotStateIntern, ControllerCommand, ControllerMode, MotionGeneratorCommand, + MotionGeneratorMode, RobotCommand, }; +use crate::Model; use std::fs::remove_file; use std::path::Path; -pub struct RobotImpl { - pub network: Network, - logger: Logger, +pub(crate) trait RobotImplementation: + RobotControl::State> +{ + fn read_once(&mut self) -> FrankaResult<::State>; + fn load_model(&mut self, persistent: bool) -> FrankaResult<::Model>; + fn server_version(&self) -> u16; +} + +pub(crate) struct RobotImplGeneric { + pub network: Network, + logger: Logger<::State>, realtime_config: RealtimeConfig, ri_version: Option, motion_generator_mode: Option, @@ -37,253 +43,9 @@ pub struct RobotImpl { message_id: u64, } -fn handle_command_response_move(response: &MoveResponse) -> Result<(), FrankaException> { - match response.status { - MoveStatus::Success => Ok(()), - MoveStatus::MotionStarted => { - //todo handle motion_generator_running == true - Ok(()) - } - MoveStatus::EmergencyAborted => Err(create_command_exception( - "libfranka-rs: Move command aborted: User Stop pressed!", - )), - MoveStatus::ReflexAborted => Err(create_command_exception( - "libfranka-rs: Move command aborted: motion aborted by reflex!", - )), - MoveStatus::InputErrorAborted => Err(create_command_exception( - "libfranka-rs: Move command aborted: invalid input provided!", - )), - MoveStatus::CommandNotPossibleRejected => Err(create_command_exception( - "libfranka-rs: Move command rejected: command not possible in the current mode!", - )), - MoveStatus::StartAtSingularPoseRejected => Err(create_command_exception( - "libfranka-rs: Move command rejected: cannot start at singular pose!", - )), - MoveStatus::InvalidArgumentRejected => Err(create_command_exception( - "libfranka-rs: Move command rejected: maximum path deviation out of range!", - )), - MoveStatus::Preempted => Err(create_command_exception( - "libfranka-rs: Move command preempted!", - )), - MoveStatus::Aborted => Err(create_command_exception( - "libfranka-rs: Move command aborted!", - )), - } -} - -fn handle_command_response_stop(response: &StopMoveResponse) -> Result<(), FrankaException> { - match response.status { - StopMoveStatus::Success => Ok(()), - StopMoveStatus::EmergencyAborted => Err(create_command_exception( - "libfranka-rs: Stop command aborted: User Stop pressed!", - )), - StopMoveStatus::ReflexAborted => Err(create_command_exception( - "libfranka-rs: Stop command aborted: motion aborted by reflex!", - )), - StopMoveStatus::CommandNotPossibleRejected => Err(create_command_exception( - "libfranka-rs: Stop command rejected: command not possible in the current mode!", - )), - StopMoveStatus::Aborted => Err(create_command_exception( - "libfranka-rs: Stop command aborted!", - )), - } -} - -impl RobotImpl { - pub fn new( - network: Network, - log_size: usize, - realtime_config: RealtimeConfig, - ) -> FrankaResult { - let current_move_generator_mode = MotionGeneratorMode::Idle; - let controller_mode = ControllerMode::Other; - let logger = Logger::new(log_size); - let mut robot_impl = RobotImpl { - network, - logger, - realtime_config, - ri_version: None, - motion_generator_mode: None, - current_move_motion_generator_mode: current_move_generator_mode, - controller_mode, - current_move_controller_mode: None, - message_id: 0, - }; - robot_impl.connect_robot()?; - let state = robot_impl - .network - .udp_blocking_receive::() - .unwrap(); - robot_impl.update_state_with(&state); - Ok(robot_impl) - } - fn connect_robot(&mut self) -> Result<(), FrankaException> { - let connect_command = ConnectRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Connect, - size_of::(), - ), - request: ConnectRequest::new(self.network.get_udp_port()), - }; - let command_id: u32 = self.network.tcp_send_request(connect_command); - let connect_response: ConnectResponse = - self.network.tcp_blocking_receive_response(command_id); - match connect_response.status { - ConnectStatus::Success => { - self.ri_version = Some(connect_response.version); - Ok(()) - } - _ => Err(FrankaException::IncompatibleLibraryVersionError { - server_version: connect_response.version, - library_version: VERSION, - }), - } - } - fn update_state_with(&mut self, robot_state: &RobotStateIntern) { - self.motion_generator_mode = Some(robot_state.motion_generator_mode); - self.controller_mode = robot_state.controller_mode; - self.message_id = robot_state.message_id; - } - pub fn read_once(&mut self) -> FrankaResult { - while self.network.udp_receive::().is_some() {} - Ok(RobotState::from(self.receive_robot_state()?)) - } - fn receive_robot_state(&mut self) -> FrankaResult { - let mut latest_accepted_state: Option = None; - let mut received_state = self.network.udp_receive::(); - while received_state.is_some() { - if received_state.unwrap().message_id - > match latest_accepted_state { - Some(s) => s.message_id, - None => self.message_id, - } - { - latest_accepted_state = Some(received_state.unwrap()); - } - received_state = self.network.udp_receive::(); - } - while latest_accepted_state.is_none() { - received_state = Some(self.network.udp_blocking_receive()?); - if received_state.unwrap().message_id > self.message_id { - latest_accepted_state = received_state; - } - } - self.update_state_with(&latest_accepted_state.unwrap()); - Ok(latest_accepted_state.unwrap()) - } - fn send_robot_command( - &mut self, - motion_command: Option<&MotionGeneratorCommand>, - control_command: Option<&ControllerCommand>, - ) -> FrankaResult> { - let motion_is_some = motion_command.is_some(); - let controller_is_some = control_command.is_some(); - if motion_is_some || controller_is_some { - if motion_is_some - && self.current_move_motion_generator_mode == MotionGeneratorMode::Idle - { - return Err(FrankaException::NoMotionGeneratorRunningError); - } - if controller_is_some - && self.current_move_controller_mode.unwrap() != ControllerMode::ExternalController - { - return Err(FrankaException::NoControllerRunningError); - } - if self.current_move_motion_generator_mode != MotionGeneratorMode::Idle - && self.current_move_controller_mode.unwrap() == ControllerMode::ExternalController - && (!motion_is_some || !controller_is_some) - { - return Err(FrankaException::PartialCommandError); - } - if motion_is_some && controller_is_some { - let command = RobotCommand { - message_id: self.message_id, - motion: (*motion_command.unwrap()).pack(), - control: (*control_command.unwrap()).pack(), - }; - return match self.network.udp_send(&command) { - Ok(_) => Ok(Some(command)), - Err(e) => Err(FrankaException::NetworkException { - message: format!("libfranka-rs: UDP send: {}", e), - }), - }; - } else if motion_is_some { - let command = RobotCommand { - message_id: self.message_id, - motion: (*motion_command.unwrap()).pack(), - control: ControllerCommand { tau_J_d: [0.; 7] }.pack(), - }; - return match self.network.udp_send(&command) { - Ok(_) => Ok(Some(command)), - Err(e) => Err(FrankaException::NetworkException { - message: format!("libfranka-rs: UDP send: {}", e), - }), - }; - } - } +impl RobotControl for RobotImplGeneric { + type State = ::State; - Ok(None) - } - fn motion_generator_running(&self) -> bool { - self.motion_generator_mode.unwrap() != MotionGeneratorMode::Idle - } - fn controller_running(&self) -> bool { - self.controller_mode == ControllerMode::ExternalController - } - pub fn load_model(&mut self, persistent: bool) -> FrankaResult { - let model_file = Path::new("/tmp/model.so"); - let model_already_downloaded = model_file.exists(); - if !model_already_downloaded { - LibraryDownloader::download(&mut self.network, model_file)?; - } - let model = Model::new(model_file, None)?; - if !persistent && model_already_downloaded { - remove_file(model_file).expect("Could not delete file"); - } - Ok(model) - } - pub fn server_version(&self) -> u16 { - self.ri_version.unwrap() - } - fn execute_move_command( - &mut self, - controller_mode: &MoveControllerMode, - motion_generator_mode: &MoveMotionGeneratorMode, - maximum_path_deviation: &MoveDeviation, - maximum_goal_deviation: &MoveDeviation, - ) -> FrankaResult { - let connect_command = MoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::Move, - size_of::(), - ), - request: MoveRequest::new( - *controller_mode, - *motion_generator_mode, - *maximum_path_deviation, - *maximum_goal_deviation, - ), - }; - let command_id: u32 = self.network.tcp_send_request(connect_command); - let response: MoveResponse = self.network.tcp_blocking_receive_response(command_id); - handle_command_response_move(&response)?; - Ok(command_id) - } - fn execute_stop_command(&mut self) -> FrankaResult { - let command = StopMoveRequestWithHeader { - header: self.network.create_header_for_robot( - RobotCommandEnum::StopMove, - size_of::(), - ), - }; - let command_id: u32 = self.network.tcp_send_request(command); - let response: StopMoveResponse = self.network.tcp_blocking_receive_response(command_id); - handle_command_response_stop(&response)?; - Ok(command_id) - } -} - -impl RobotControl for RobotImpl { fn start_motion( &mut self, controller_mode: MoveControllerMode, @@ -317,7 +79,7 @@ impl RobotControl for RobotImpl { { match self .network - .tcp_receive_response(move_command_id, |x| handle_command_response_move(&x)) + .tcp_receive_response(move_command_id, |x| Data::handle_command_move_status(x)) { Ok(received_message) => { if received_message { @@ -365,22 +127,23 @@ impl RobotControl for RobotImpl { robot_state = Some(self.update(Some(&motion_finished_command), control_command)?); } let robot_state = robot_state.unwrap(); - let response: MoveResponse = self.network.tcp_blocking_receive_response(motion_id); - if response.status == MoveStatus::ReflexAborted { - return Err(create_control_exception( - String::from("Motion finished commanded, but the robot is still moving!"), - &response.status, - &robot_state.last_motion_errors, - self.logger.flush(), - )); - } - match handle_command_response_move(&response) { + + let status = self.network.tcp_blocking_receive_status(motion_id); + + Data::create_control_exception_if_reflex_aborted( + String::from("Motion finished commanded, but the robot is still moving!"), + status, + robot_state.get_last_motion_errors(), + self.logger.flush(), + )?; + + match Data::handle_command_move_status(status) { Ok(_) => {} Err(FrankaException::CommandException { message }) => { - return Err(create_control_exception( + return Err(Data::create_control_exception( message, - &response.status, - &robot_state.last_motion_errors, + status, + robot_state.get_last_motion_errors(), self.logger.flush(), )); } @@ -407,7 +170,7 @@ impl RobotControl for RobotImpl { // TODO (FWA): It is not guaranteed that the Move response won't come later self.network - .tcp_receive_response(motion_id, |_x: MoveResponse| Ok(())) + .tcp_receive_response(motion_id, |_x: Data::MoveStatus| Ok(())) .expect("This should be impossible as the handler always returns Ok(())"); self.current_move_motion_generator_mode = MotionGeneratorMode::Idle; self.current_move_controller_mode = Some(ControllerMode::Other); @@ -417,9 +180,9 @@ impl RobotControl for RobotImpl { &mut self, motion_command: Option<&MotionGeneratorCommand>, control_command: Option<&ControllerCommand>, - ) -> FrankaResult { + ) -> FrankaResult { let robot_command = self.send_robot_command(motion_command, control_command)?; - let state = RobotState::from(self.receive_robot_state()?); + let state = Data::State::from(self.receive_robot_state()?); if let Some(command) = robot_command { self.logger.log(&state, &command); } @@ -432,20 +195,20 @@ impl RobotControl for RobotImpl { fn throw_on_motion_error( &mut self, - robot_state: &RobotState, + robot_state: &Data::State, motion_id: u32, ) -> FrankaResult<()> { - if robot_state.robot_mode != RobotMode::Move + if !robot_state.is_moving() || self.motion_generator_mode.unwrap() != self.current_move_motion_generator_mode || self.controller_mode != self.current_move_controller_mode.unwrap() { - let response = self.network.tcp_blocking_receive_response(motion_id); - let result = handle_command_response_move(&response); + let status = self.network.tcp_blocking_receive_status(motion_id); + let result = Data::handle_command_move_status(status); return match result { - Err(error) => Err(create_control_exception( + Err(error) => Err(Data::create_control_exception( error.to_string(), - &response.status, - &robot_state.last_motion_errors, + status, + robot_state.get_last_motion_errors(), self.logger.flush(), )), Ok(_) => panic!("Unexpected reply to a Move command"), @@ -455,42 +218,197 @@ impl RobotControl for RobotImpl { } } -impl MotionGeneratorTrait for RobotImpl { - fn get_motion_generator_mode() -> MoveMotionGeneratorMode { - MoveMotionGeneratorMode::JointVelocity +impl RobotImplementation for RobotImplGeneric { + fn read_once(&mut self) -> FrankaResult { + while self.network.udp_receive::().is_some() {} + Ok(Data::State::from(self.receive_robot_state()?)) + } + fn load_model(&mut self, persistent: bool) -> FrankaResult { + let download_path = format!( + "/tmp/{}_model_{}_{}.{}", + Data::MODEL_NAME, + OS, + ARCH, + DLL_EXTENSION + ); + let model_file = Path::new(download_path.as_str()); + let model_already_downloaded = model_file.exists(); + if !model_already_downloaded { + LibraryDownloaderGeneric::::download(&mut self.network, model_file)?; + } + let model = Data::Model::new(model_file, None)?; + if !persistent && model_already_downloaded { + remove_file(model_file).expect("Could not delete file"); + } + Ok(model) + } + + fn server_version(&self) -> u16 { + self.ri_version.unwrap() } } -fn create_control_exception( - message: String, - move_status: &MoveStatus, - reflex_reasons: &FrankaErrors, - log: Vec, -) -> FrankaException { - let mut exception_string = String::from(&message); - if move_status == &MoveStatus::ReflexAborted { - exception_string += " "; - exception_string += reflex_reasons.to_string().as_str(); - if log.len() >= 2 { - let lost_packets: u128 = - (log.last().unwrap().state.time - log[log.len() - 2].state.time).as_millis() - 1; - exception_string += format!( - "\ncontrol_command_success_rate: {}", - log[log.len() - 2].state.control_command_success_rate - * (1. - lost_packets as f64 / 100.) - ) - .as_str(); - if lost_packets > 0 { - exception_string += format!( - " packets lost in a row in the last sample: {}", - lost_packets - ) - .as_str(); +impl RobotImplGeneric { + pub fn new( + network: Network, + log_size: usize, + realtime_config: RealtimeConfig, + ) -> FrankaResult { + let current_move_generator_mode = MotionGeneratorMode::Idle; + let controller_mode = ControllerMode::Other; + let logger: Logger = Logger::new(log_size); + let mut robot_impl = RobotImplGeneric { + network, + logger, + realtime_config, + ri_version: None, + motion_generator_mode: None, + current_move_motion_generator_mode: current_move_generator_mode, + controller_mode, + current_move_controller_mode: None, + message_id: 0, + }; + robot_impl.connect_robot()?; + let state = robot_impl + .network + .udp_blocking_receive::() + .unwrap(); + robot_impl.update_state_with(&state); + Ok(robot_impl) + } + + fn connect_robot(&mut self) -> Result<(), FrankaException> { + let udp_port = self.network.get_udp_port(); + let connect_command = Data::create_connect_request(&mut self.network.command_id, udp_port); + let command_id: u32 = self.network.tcp_send_request(connect_command); + let connect_response: ConnectResponseWithoutHeader = + self.network.tcp_blocking_receive_status(command_id); + match connect_response.status { + ConnectStatus::Success => { + self.ri_version = Some(connect_response.version); + Ok(()) + } + _ => Err(FrankaException::IncompatibleLibraryVersionError { + server_version: connect_response.version, + library_version: Data::get_library_version(), + }), + } + } + fn update_state_with(&mut self, robot_state: &Data::StateIntern) { + self.motion_generator_mode = Some(robot_state.get_motion_generator_mode()); + self.controller_mode = robot_state.get_controller_mode(); + self.message_id = robot_state.get_message_id(); + } + + fn receive_robot_state(&mut self) -> FrankaResult { + let mut latest_accepted_state: Option = None; + let mut received_state = self.network.udp_receive::(); + while received_state.is_some() { + if received_state.unwrap().get_message_id() + > match latest_accepted_state { + Some(s) => s.get_message_id(), + None => self.message_id, + } + { + latest_accepted_state = Some(received_state.unwrap()); } + received_state = self.network.udp_receive::(); } + while latest_accepted_state.is_none() { + received_state = Some(self.network.udp_blocking_receive()?); + if received_state.unwrap().get_message_id() > self.message_id { + latest_accepted_state = received_state; + } + } + self.update_state_with(&latest_accepted_state.unwrap()); + Ok(latest_accepted_state.unwrap()) } - FrankaException::ControlException { - error: exception_string, - log: Some(log), + fn send_robot_command( + &mut self, + motion_command: Option<&MotionGeneratorCommand>, + control_command: Option<&ControllerCommand>, + ) -> FrankaResult> { + let motion_is_some = motion_command.is_some(); + let controller_is_some = control_command.is_some(); + if motion_is_some || controller_is_some { + if motion_is_some + && self.current_move_motion_generator_mode == MotionGeneratorMode::Idle + { + return Err(FrankaException::NoMotionGeneratorRunningError); + } + if controller_is_some + && self.current_move_controller_mode.unwrap() != ControllerMode::ExternalController + { + return Err(FrankaException::NoControllerRunningError); + } + if self.current_move_motion_generator_mode != MotionGeneratorMode::Idle + && self.current_move_controller_mode.unwrap() == ControllerMode::ExternalController + && (!motion_is_some || !controller_is_some) + { + return Err(FrankaException::PartialCommandError); + } + if motion_is_some && controller_is_some { + let command = RobotCommand { + message_id: self.message_id, + motion: (*motion_command.unwrap()).pack(), + control: (*control_command.unwrap()).pack(), + }; + return match self.network.udp_send(&command) { + Ok(_) => Ok(Some(command)), + Err(e) => Err(FrankaException::NetworkException { + message: format!("libfranka-rs: UDP send: {}", e), + }), + }; + } else if motion_is_some { + let command = RobotCommand { + message_id: self.message_id, + motion: (*motion_command.unwrap()).pack(), + control: ControllerCommand { tau_J_d: [0.; 7] }.pack(), + }; + return match self.network.udp_send(&command) { + Ok(_) => Ok(Some(command)), + Err(e) => Err(FrankaException::NetworkException { + message: format!("libfranka-rs: UDP send: {}", e), + }), + }; + } + } + + Ok(None) + } + fn motion_generator_running(&self) -> bool { + self.motion_generator_mode.unwrap() != MotionGeneratorMode::Idle + } + fn controller_running(&self) -> bool { + self.controller_mode == ControllerMode::ExternalController + } + + fn execute_move_command( + &mut self, + controller_mode: &MoveControllerMode, + motion_generator_mode: &MoveMotionGeneratorMode, + maximum_path_deviation: &MoveDeviation, + maximum_goal_deviation: &MoveDeviation, + ) -> FrankaResult { + let command = Data::create_move_request( + &mut self.network.command_id, + MoveRequest::new( + *controller_mode, + *motion_generator_mode, + *maximum_path_deviation, + *maximum_goal_deviation, + ), + ); + let command_id: u32 = self.network.tcp_send_request(command); + let status = self.network.tcp_blocking_receive_status(command_id); + Data::handle_command_move_status(status)?; + Ok(command_id) + } + fn execute_stop_command(&mut self) -> FrankaResult { + let command = Data::create_stop_request(&mut self.network.command_id); + let command_id: u32 = self.network.tcp_send_request(command); + let status = self.network.tcp_blocking_receive_status(command_id); + Data::handle_command_stop_move_status(status)?; + Ok(command_id) } } diff --git a/src/robot/robot_state.rs b/src/robot/robot_state.rs index 4b4d1d3..a33e45e 100644 --- a/src/robot/robot_state.rs +++ b/src/robot/robot_state.rs @@ -2,13 +2,23 @@ // Licensed under the EUPL-1.2-or-later //! Contains the franka::RobotState types. +use std::fmt::Debug; use std::time::Duration; use crate::robot::errors::{FrankaErrorKind, FrankaErrors}; -use crate::robot::types::{RobotMode, RobotStateIntern}; +use crate::robot::types::{PandaStateIntern, RobotMode}; use nalgebra::{Matrix3, Vector3}; -/// Describes the robot state. +pub trait AbstractRobotState: Clone + Debug { + fn get_time(&self) -> Duration; + #[allow(non_snake_case)] + fn get_tau_J_d(&self) -> [f64; 7]; + fn get_last_motion_errors(&self) -> &FrankaErrors; + fn is_moving(&self) -> bool; + fn get_q_d(&self) -> [f64; 7]; +} + +/// Describes the state of the robot at a given time. #[derive(Debug, Clone, Default)] #[allow(non_snake_case)] pub struct RobotState { @@ -254,9 +264,10 @@ pub struct RobotState { /// instead pub time: Duration, } -impl From for RobotState { + +impl From for RobotState { #[allow(non_snake_case)] - fn from(robot_state: RobotStateIntern) -> Self { + fn from(robot_state: PandaStateIntern) -> Self { let O_T_EE = robot_state.O_T_EE; let O_T_EE_d = robot_state.O_T_EE_d; let F_T_NE = robot_state.F_T_NE; @@ -438,6 +449,28 @@ fn skew_symmetric_matrix_from_vector(vector: &Vector3) -> Matrix3 { ) } +impl AbstractRobotState for RobotState { + fn get_time(&self) -> Duration { + self.time + } + + fn get_tau_J_d(&self) -> [f64; 7] { + self.tau_J_d + } + + fn get_last_motion_errors(&self) -> &FrankaErrors { + &self.last_motion_errors + } + + fn is_moving(&self) -> bool { + self.robot_mode == RobotMode::Move + } + + fn get_q_d(&self) -> [f64; 7] { + self.q_d + } +} + #[cfg(test)] mod tests { use crate::robot::robot_state::{combine_center_of_mass, combine_inertia_tensor}; diff --git a/src/robot/service_types.rs b/src/robot/service_types.rs index f6e2e43..761ec50 100644 --- a/src/robot/service_types.rs +++ b/src/robot/service_types.rs @@ -1,20 +1,110 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later +macro_rules! define_panda_request_with_header { + ($name: ident, $request: ty, $command: expr) => { + #[derive(Serialize, Deserialize, Debug, Copy, Clone)] + #[repr(packed)] + pub struct $name { + pub header: PandaCommandHeader, + pub request: $request, + } + impl MessageCommand for $name { + fn get_command_message_id(&self) -> u32 { + self.header.get_command_message_id() + } + } + impl From<(u32, $request)> for $name { + fn from(tuple: (u32, $request)) -> Self { + let command_id = tuple.0; + $name { + header: PandaCommandHeader::new( + $command, + command_id, + std::mem::size_of::() as u32, + ), + request: tuple.1, + } + } + } + }; +} +macro_rules! define_fr3_request_with_header { + ($name: ident, $request: ty, $command: expr) => { + #[derive(Serialize, Deserialize, Debug, Copy, Clone)] + #[repr(packed)] + pub struct $name { + pub header: Fr3CommandHeader, + pub request: $request, + } + impl MessageCommand for $name { + fn get_command_message_id(&self) -> u32 { + self.header.get_command_message_id() + } + } + impl From<(u32, $request)> for $name { + fn from(tuple: (u32, $request)) -> Self { + let command_id = tuple.0; + $name { + header: Fr3CommandHeader::new( + $command, + command_id, + std::mem::size_of::() as u32, + ), + request: tuple.1, + } + } + } + }; +} use std::fmt::Debug; +use crate::gripper::types::CommandHeader; use serde::Deserialize; use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; use crate::network::MessageCommand; -pub static VERSION: u16 = 5; +pub static PANDA_VERSION: u16 = 5; +pub static FR3_VERSION: u16 = 6; pub static COMMAND_PORT: u16 = 1337; +pub trait RobotHeader: MessageCommand + Serialize + Debug + Copy + Clone {} + +impl RobotHeader for PandaCommandHeader {} + +impl MessageCommand for Fr3CommandHeader { + fn get_command_message_id(&self) -> u32 { + self.command_id + } +} + +impl RobotHeader for Fr3CommandHeader {} + +impl CommandHeader for Fr3CommandHeader { + fn get_command_id(&self) -> u32 { + self.command_id + } + + fn get_size(&self) -> u32 { + self.size + } +} + +impl CommandHeader for PandaCommandHeader { + fn get_command_id(&self) -> u32 { + self.command_id + } + + fn get_size(&self) -> u32 { + self.size + } +} + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u32)] -pub enum RobotCommandEnum { +pub enum PandaCommandEnum { Connect, Move, StopMove, @@ -31,6 +121,23 @@ pub enum RobotCommandEnum { LoadModelLibrary, } +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] +#[repr(u32)] +pub enum Fr3CommandEnum { + Connect, + Move, + StopMove, + SetCollisionBehavior, + SetJointImpedance, + SetCartesianImpedance, + SetGuidingMode, + SetEeToK, + SetNeToEe, + SetLoad, + AutomaticErrorRecovery, + LoadModelLibrary, +} + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] pub enum DefaultStatus { @@ -47,7 +154,7 @@ pub enum ConnectStatus { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] #[repr(u8)] -pub enum MoveStatus { +pub enum MoveStatusPanda { Success, MotionStarted, Preempted, @@ -60,9 +167,26 @@ pub enum MoveStatus { Aborted, } +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] +#[repr(u8)] +pub enum MoveStatusFr3 { + Success, + MotionStarted, + Preempted, + PreemptedDueToActivatedSafetyFunctions, + CommandRejectedDueToActivatedSafetyFunctions, + CommandNotPossibleRejected, + StartAtSingularPoseRejected, + InvalidArgumentRejected, + ReflexAborted, + EmergencyAborted, + InputErrorAborted, + Aborted, +} + #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum StopMoveStatus { +pub enum StopMoveStatusPanda { Success, CommandNotPossibleRejected, EmergencyAborted, @@ -72,9 +196,32 @@ pub enum StopMoveStatus { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum AutomaticErrorRecoveryStatus { +pub enum StopMoveStatusFr3 { + Success, + CommandNotPossibleRejected, + CommandRejectedDueToActivatedSafetyFunctions, + EmergencyAborted, + ReflexAborted, + Aborted, +} + +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] +#[repr(u8)] +pub enum AutomaticErrorRecoveryStatusPanda { + Success, + CommandNotPossibleRejected, + ManualErrorRecoveryRequiredRejected, + ReflexAborted, + EmergencyAborted, + Aborted, +} + +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] +#[repr(u8)] +pub enum AutomaticErrorRecoveryStatusFr3 { Success, CommandNotPossibleRejected, + CommandRejectedDueToActivatedSafetyFunctions, ManualErrorRecoveryRequiredRejected, ReflexAborted, EmergencyAborted, @@ -90,12 +237,21 @@ pub enum LoadModelLibraryStatus { #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] #[repr(u8)] -pub enum GetterSetterStatus { +pub enum GetterSetterStatusPanda { Success, CommandNotPossibleRejected, InvalidArgumentRejected, } +#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] +#[repr(u8)] +pub enum GetterSetterStatusFr3 { + Success, + CommandNotPossibleRejected, + InvalidArgumentRejected, + CommandRejectedDueToActivatedSafetyFunctions, +} + #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct ConnectRequest { @@ -104,30 +260,33 @@ pub struct ConnectRequest { } impl ConnectRequest { - pub fn new(udp_port: u16) -> Self { - ConnectRequest { - version: VERSION, - udp_port, - } + pub fn new(udp_port: u16, version: u16) -> Self { + ConnectRequest { version, udp_port } } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct ConnectRequestWithHeader { - pub header: RobotCommandHeader, - pub request: ConnectRequest, -} +define_panda_request_with_header!( + ConnectRequestWithPandaHeader, + ConnectRequest, + PandaCommandEnum::Connect +); +define_fr3_request_with_header!( + ConnectRequestWithFr3Header, + ConnectRequest, + Fr3CommandEnum::Connect +); -impl MessageCommand for ConnectRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } +#[derive(Serialize, Deserialize)] +#[repr(packed)] +pub struct ConnectResponsePanda { + pub header: PandaCommandHeader, + pub status: ConnectStatus, + pub version: u16, } -#[derive(Serialize, Deserialize, Debug)] -pub struct ConnectResponse { - pub header: RobotCommandHeader, +#[derive(Serialize, Deserialize)] +#[repr(packed)] +pub struct ConnectResponseWithoutHeader { pub status: ConnectStatus, pub version: u16, } @@ -182,43 +341,31 @@ impl MoveRequest { } } +define_panda_request_with_header!( + MoveRequestWithPandaHeader, + MoveRequest, + PandaCommandEnum::Move +); +define_fr3_request_with_header!(MoveRequestWithFr3Header, MoveRequest, Fr3CommandEnum::Move); + #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct MoveRequestWithHeader { - pub header: RobotCommandHeader, - pub request: MoveRequest, -} - -impl MessageCommand for MoveRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -#[derive(Serialize, Deserialize, Debug)] -pub struct MoveResponse { - pub header: RobotCommandHeader, - pub status: MoveStatus, +pub struct StopMoveRequestWithPandaHeader { + pub header: PandaCommandHeader, } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct StopMoveRequestWithHeader { - pub header: RobotCommandHeader, +pub struct StopMoveRequestWithFr3Header { + pub header: Fr3CommandHeader, } -impl MessageCommand for StopMoveRequestWithHeader { +impl MessageCommand for StopMoveRequestWithPandaHeader { fn get_command_message_id(&self) -> u32 { self.header.get_command_message_id() } } -#[derive(Serialize, Deserialize, Debug)] -pub struct StopMoveResponse { - pub header: RobotCommandHeader, - pub status: StopMoveStatus, -} - #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] pub struct GetCartesianLimitRequest { @@ -230,24 +377,16 @@ impl GetCartesianLimitRequest { GetCartesianLimitRequest { id } } } - -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct GetCartesianLimitRequestWithHeader { - pub header: RobotCommandHeader, - pub request: GetCartesianLimitRequest, -} - -impl MessageCommand for GetCartesianLimitRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + GetCartesianLimitRequestWithPandaHeader, + GetCartesianLimitRequest, + PandaCommandEnum::GetCartesianLimit +); #[derive(Serialize, Deserialize, Debug)] pub struct GetCartesianLimitResponse { - pub header: RobotCommandHeader, - pub status: GetterSetterStatus, + pub header: PandaCommandHeader, + pub status: GetterSetterStatusPanda, pub object_world_size: [f64; 3], pub object_frame: [f64; 16], pub object_activation: bool, @@ -290,21 +429,16 @@ impl SetCollisionBehaviorRequest { } } } - -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetCollisionBehaviorRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetCollisionBehaviorRequest, -} - -impl MessageCommand for SetCollisionBehaviorRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetCollisionBehaviorResponse = SetterResponse; +define_panda_request_with_header!( + SetCollisionBehaviorRequestWithPandaHeader, + SetCollisionBehaviorRequest, + PandaCommandEnum::SetCollisionBehavior +); +define_fr3_request_with_header!( + SetCollisionBehaviorRequestWithFr3Header, + SetCollisionBehaviorRequest, + Fr3CommandEnum::SetCollisionBehavior +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -320,20 +454,16 @@ impl SetJointImpedanceRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetJointImpedanceRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetJointImpedanceRequest, -} - -impl MessageCommand for SetJointImpedanceRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetJointImpedanceResponse = SetterResponse; +define_panda_request_with_header!( + SetJointImpedanceRequestWithPandaHeader, + SetJointImpedanceRequest, + PandaCommandEnum::SetJointImpedance +); +define_fr3_request_with_header!( + SetJointImpedanceRequestWithFr3Header, + SetJointImpedanceRequest, + Fr3CommandEnum::SetJointImpedance +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -349,20 +479,16 @@ impl SetCartesianImpedanceRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetCartesianImpedanceRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetCartesianImpedanceRequest, -} - -impl MessageCommand for SetCartesianImpedanceRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetCartesianImpedanceResponse = SetterResponse; +define_panda_request_with_header!( + SetCartesianImpedanceRequestWithPandaHeader, + SetCartesianImpedanceRequest, + PandaCommandEnum::SetCartesianImpedance +); +define_fr3_request_with_header!( + SetCartesianImpedanceRequestWithFr3Header, + SetCartesianImpedanceRequest, + Fr3CommandEnum::SetCartesianImpedance +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -382,20 +508,16 @@ impl SetGuidingModeRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetGuidingModeRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetGuidingModeRequest, -} - -impl MessageCommand for SetGuidingModeRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetGuidingModeResponse = SetterResponse; +define_panda_request_with_header!( + SetGuidingModeRequestWithPandaHeader, + SetGuidingModeRequest, + PandaCommandEnum::SetGuidingMode +); +define_fr3_request_with_header!( + SetGuidingModeRequestWithFr3Header, + SetGuidingModeRequest, + Fr3CommandEnum::SetGuidingMode +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -411,20 +533,16 @@ impl SetEeToKRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetEeToKRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetEeToKRequest, -} - -impl MessageCommand for SetEeToKRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetEeToKResponse = SetterResponse; +define_panda_request_with_header!( + SetEeToKRequestWithPandaHeader, + SetEeToKRequest, + PandaCommandEnum::SetEeToK +); +define_fr3_request_with_header!( + SetEeToKRequestWithFr3Header, + SetEeToKRequest, + Fr3CommandEnum::SetEeToK +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -440,20 +558,16 @@ impl SetNeToEeRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetNeToEeRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetNeToEeRequest, -} - -impl MessageCommand for SetNeToEeRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetNeToEeResponse = SetterResponse; +define_panda_request_with_header!( + SetNeToEeRequestWithPandaHeader, + SetNeToEeRequest, + PandaCommandEnum::SetNeToEe +); +define_fr3_request_with_header!( + SetNeToEeRequestWithFr3Header, + SetNeToEeRequest, + Fr3CommandEnum::SetNeToEe +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -475,20 +589,16 @@ impl SetLoadRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetLoadRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetLoadRequest, -} - -impl MessageCommand for SetLoadRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} - -pub type SetLoadResponse = SetterResponse; +define_panda_request_with_header!( + SetLoadRequestWithPandaHeader, + SetLoadRequest, + PandaCommandEnum::SetLoad +); +define_fr3_request_with_header!( + SetLoadRequestWithFr3Header, + SetLoadRequest, + Fr3CommandEnum::SetLoad +); #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] @@ -520,33 +630,24 @@ impl SetFiltersRequest { } } -#[derive(Serialize, Deserialize, Debug, Copy, Clone)] -#[repr(packed)] -pub struct SetFiltersRequestWithHeader { - pub header: RobotCommandHeader, - pub request: SetFiltersRequest, -} - -impl MessageCommand for SetFiltersRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() - } -} +define_panda_request_with_header!( + SetFiltersRequestWithPandaHeader, + SetFiltersRequest, + PandaCommandEnum::SetFilters +); pub type SetFiltersResponse = SetterResponse; #[derive(Serialize, Deserialize, Debug)] pub struct SetterResponse { - pub header: RobotCommandHeader, - pub status: GetterSetterStatus, + pub header: PandaCommandHeader, + pub status: GetterSetterStatusPanda, } -pub type AutomaticErrorRecoveryRequestWithHeader = RobotCommandHeader; - #[derive(Serialize, Deserialize, Debug)] -pub struct AutomaticErrorRecoveryResponse { - pub header: RobotCommandHeader, - pub status: AutomaticErrorRecoveryStatus, +pub struct SetterResponseFr3 { + pub header: Fr3CommandHeader, + pub status: GetterSetterStatusPanda, } #[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone)] @@ -572,36 +673,56 @@ pub struct LoadModelLibraryRequest { pub system: LoadModelLibrarySystem, } +define_panda_request_with_header!( + LoadModelLibraryRequestWithPandaHeader, + LoadModelLibraryRequest, + PandaCommandEnum::LoadModelLibrary +); +define_fr3_request_with_header!( + LoadModelLibraryRequestWithFr3Header, + LoadModelLibraryRequest, + Fr3CommandEnum::LoadModelLibrary +); + #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct LoadModelLibraryRequestWithHeader { - pub header: RobotCommandHeader, - pub request: LoadModelLibraryRequest, +pub struct PandaCommandHeader { + pub command: PandaCommandEnum, + pub command_id: u32, + pub size: u32, } -impl MessageCommand for LoadModelLibraryRequestWithHeader { - fn get_command_message_id(&self) -> u32 { - self.header.get_command_message_id() +impl Default for PandaCommandHeader { + fn default() -> Self { + PandaCommandHeader { + command: PandaCommandEnum::Connect, + command_id: 0, + size: 0, + } } } -#[derive(Serialize, Deserialize, Debug)] -pub struct LoadModelLibraryResponse { - pub header: RobotCommandHeader, - pub status: LoadModelLibraryStatus, +impl PandaCommandHeader { + pub fn new(command: PandaCommandEnum, command_id: u32, size: u32) -> PandaCommandHeader { + PandaCommandHeader { + command, + command_id, + size, + } + } } #[derive(Serialize, Deserialize, Debug, Copy, Clone)] #[repr(packed)] -pub struct RobotCommandHeader { - pub command: RobotCommandEnum, +pub struct Fr3CommandHeader { + pub command: Fr3CommandEnum, pub command_id: u32, pub size: u32, } -impl RobotCommandHeader { - pub fn new(command: RobotCommandEnum, command_id: u32, size: u32) -> RobotCommandHeader { - RobotCommandHeader { +impl Fr3CommandHeader { + pub fn new(command: Fr3CommandEnum, command_id: u32, size: u32) -> Fr3CommandHeader { + Fr3CommandHeader { command, command_id, size, @@ -609,7 +730,7 @@ impl RobotCommandHeader { } } -impl MessageCommand for RobotCommandHeader { +impl MessageCommand for PandaCommandHeader { fn get_command_message_id(&self) -> u32 { self.command_id } diff --git a/src/robot/types.rs b/src/robot/types.rs index db4301f..f1cf9b6 100644 --- a/src/robot/types.rs +++ b/src/robot/types.rs @@ -1,8 +1,8 @@ // Copyright (c) 2021 Marco Boneberger // Licensed under the EUPL-1.2-or-later + use std::fmt::Debug; -use crate::robot::types::RobotMode::Other; use serde::Deserialize; use serde::Serialize; use serde_repr::{Deserialize_repr, Serialize_repr}; @@ -26,9 +26,10 @@ pub enum ControllerMode { Other, } /// Describes the robot's current mode. -#[derive(Serialize_repr, Deserialize_repr, Debug, Copy, Clone, PartialEq)] +#[derive(Serialize_repr, Deserialize_repr, Debug, Default, Copy, Clone, PartialEq)] #[repr(u8)] pub enum RobotMode { + #[default] Other, Idle, Move, @@ -37,16 +38,11 @@ pub enum RobotMode { UserStopped, AutomaticErrorRecovery, } -impl Default for RobotMode { - fn default() -> Self { - Other - } -} #[derive(Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] #[allow(non_snake_case)] #[repr(packed)] -pub struct RobotStateIntern { +pub struct PandaStateIntern { pub message_id: u64, pub O_T_EE: [f64; 16], pub O_T_EE_d: [f64; 16], @@ -96,9 +92,31 @@ pub struct RobotStateIntern { pub control_command_success_rate: f64, } -impl RobotStateIntern { +pub type Fr3StateIntern = PandaStateIntern; + +pub trait AbstractRobotStateIntern: Copy + Clone + PartialEq { + fn get_message_id(&self) -> u64; + fn get_motion_generator_mode(&self) -> MotionGeneratorMode; + fn get_controller_mode(&self) -> ControllerMode; +} + +impl AbstractRobotStateIntern for PandaStateIntern { + fn get_message_id(&self) -> u64 { + self.message_id + } + + fn get_motion_generator_mode(&self) -> MotionGeneratorMode { + self.motion_generator_mode + } + + fn get_controller_mode(&self) -> ControllerMode { + self.controller_mode + } +} + +impl PandaStateIntern { pub fn dummy() -> Self { - RobotStateIntern { + PandaStateIntern { message_id: 0, O_T_EE: [0.; 16], O_T_EE_d: [0.; 16], @@ -280,3 +298,5 @@ pub struct RobotCommand { pub motion: MotionGeneratorCommandPacked, pub control: ControllerCommandPacked, } + +pub trait ConvertMotion {} diff --git a/src/robot/virtual_wall_cuboid.rs b/src/robot/virtual_wall_cuboid.rs index 22296d2..a2e2370 100644 --- a/src/robot/virtual_wall_cuboid.rs +++ b/src/robot/virtual_wall_cuboid.rs @@ -17,7 +17,7 @@ pub struct VirtualWallCuboid { } impl VirtualWallCuboid { - pub fn new(id: i32, response: GetCartesianLimitResponse) -> Self { + pub(crate) fn new(id: i32, response: GetCartesianLimitResponse) -> Self { VirtualWallCuboid { id, object_world_size: response.object_world_size, diff --git a/src/utils.rs b/src/utils.rs index a6136ae..5ea667a 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -2,9 +2,10 @@ // Licensed under the EUPL-1.2-or-later //! contains useful type definitions and conversion functions. -use crate::robot::control_types::{Finishable, JointPositions}; -use crate::robot::robot_state::RobotState; -use nalgebra::{Isometry3, Matrix4, MatrixMN, MatrixN, Rotation3, Vector3, VectorN, U6, U7}; +use crate::robot::control_types::JointPositions; +use crate::robot::robot_state::AbstractRobotState; +use crate::Finishable; +use nalgebra::{Isometry3, Matrix4, Rotation3, SMatrix, SVector, Vector3}; use std::time::Duration; /// converts a 4x4 column-major homogenous matrix to an Isometry @@ -20,11 +21,11 @@ pub fn array_to_isometry(array: &[f64; 16]) -> Isometry3 { ) } /// A Vector with 7 entries -pub type Vector7 = VectorN; +pub type Vector7 = SVector; /// A Matrix with 6 rows and 7 columns -pub type Matrix6x7 = MatrixMN; +pub type Matrix6x7 = SMatrix; /// A Matrix with 7 rows and 7 columns -pub type Matrix7 = MatrixN; +pub type Matrix7 = SMatrix; /// An example showing how to generate a joint pose motion to a goal position. Adapted from: /// Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots /// (Kogan Page Science Paper edition). @@ -169,15 +170,15 @@ impl MotionGenerator { /// /// # Return /// Joint positions for use inside a control loop. - pub fn generate_motion( + pub fn generate_motion( &mut self, - robot_state: &RobotState, + robot_state: &State, period: &Duration, ) -> JointPositions { self.time += period.as_secs_f64(); if self.time == 0. { - self.q_start = Vector7::from_column_slice(&robot_state.q_d); + self.q_start = Vector7::from_column_slice(&robot_state.get_q_d()); self.delta_q = self.q_goal - self.q_start; self.calculate_synchronized_values(); }